Requirements#

Your Tasks#

At a high level, your group must complete the following tasks. Each is spelled out in detail in the sections below.

Create two ROS 2 packages inside the pre-existing ~/enpm605_ws/src/final_project/ folder:
1. group<N>_final_interfaces (CMake) containing the DetectSurvivor.srv and ReportSurvivor.srv definitions.
2. group<N>_final (ament_python) containing the behavior tree nodes, service servers, the entry point script, the launch file, and the parameter YAML.
Register both packages as <exec_depend> entries in ~/enpm605_ws/src/final_project_meta/package.xml so that colcon build --packages-up-to final_project_meta picks them up.
Define two custom service interfaces (DetectSurvivor.srv and ReportSurvivor.srv) in group<N>_final_interfaces.
Implement two simulated service servers that respond to detection and report requests.
Implement a shared ZoneManager class (plain Python, not a BT node) that manages the list of search zones and tracks mission state.
Implement 9 behavior tree leaf nodes (2 conditions + 7 actions) using py_trees.behaviour.Behaviour.
Assemble the full behavior tree in an entry point script that reads parameters, builds the tree, and runs it with py_trees_ros.trees.BehaviourTree.
Write the launch file that brings up Nav2 (with your nav2_params.yaml and final_project_map.yaml), starts the two simulated service servers, and starts the behavior tree node, loading mission parameters from mission_params.yaml.
Build a map of the final project world using slam_toolbox and save it under group<N>_final/maps/ (see Simulation and Provided Code for the procedure). The saved .pgm and .yaml files must be included in your submission.
Document contributions in README.md.
Submit by zipping the ~/enpm605_ws/src/final_project/ folder and uploading it to Canvas.

Package Structure#

Your submission must contain two ROS 2 packages, both placed inside the pre-existing ~/enpm605_ws/src/final_project/ folder.

Package 1. Service interfaces (CMake):

group<N>_final_interfaces/
|-- srv/
|   |-- DetectSurvivor.srv
|   |-- ReportSurvivor.srv
|-- CMakeLists.txt
|-- package.xml

Package 2. Nodes, BT, and launch (ament_python):

group<N>_final/
|-- group<N>_final/
|   |-- __init__.py
|   |-- zone_manager.py
|   |-- bt_nodes/
|   |   |-- __init__.py
|   |   |-- conditions.py
|   |   |-- actions.py
|   |-- service_servers/
|   |   |-- __init__.py
|   |   |-- detect_survivor_server.py
|   |   |-- report_survivor_server.py
|   |-- scripts/
|       |-- __init__.py
|       |-- main_search_and_rescue.py
|-- launch/
|   |-- search_and_rescue.launch.py
|-- config/
|   |-- mission_params.yaml
|   |-- nav2_params.yaml
|-- maps/
|   |-- final_project_map.pgm
|   |-- final_project_map.yaml
|-- resource/
|   |-- group<N>_final
|-- test/
|-- package.xml
|-- setup.py
|-- setup.cfg
|-- README.md

Walkthrough video: package structure

The video below walks through a working group<N>_final/ and group<N>_final_interfaces/ layout file by file. Click the play button to start it – the video does not autoplay.

Build the Map#

Nav2 needs a pre-built occupancy grid to localize the robot. No map is provided – each group must build their own with slam_toolbox and save it with nav2_map_server. The two output files (.pgm + .yaml) live under group<N>_final/maps/ and ship with your submission.

Important

Build the map yourself by following the same workflow we used in class.

Launch the final project world and slam_toolbox in mapping mode (in two terminals):

	Command
T1	`ros2 launch rosbot_gazebo final_project_world.launch.py rviz:=False`
T2	`ros2 launch nav_demo map_nav.launch.py mode:=mapping`
Gazebo	Drive the robot with teleop
RViz	Observe the map display

Save the map under your package’s maps/ directory using nav2_map_server:

mkdir -p ~/enpm605_ws/src/final_project/group<N>_final/maps
ros2 run nav2_map_server map_saver_cli \
    -f ~/enpm605_ws/src/final_project/group<N>_final/maps/final_project_map

This produces final_project_map.pgm and final_project_map.yaml.

Service Interfaces#

Define two custom services in group<N>_final_interfaces/srv/.

DetectSurvivor.srv:

# Request
string zone_id
---
# Response
bool found
float64 survivor_x
float64 survivor_y

ReportSurvivor.srv:

# Request
string survivor_id
geometry_msgs/PointStamped location
---
# Response
bool acknowledged

CMake/package configuration (follow the template in lecture10/custom_interfaces):

CMakeLists.txt must call rosidl_generate_interfaces on both .srv files and depend on geometry_msgs.
package.xml must include rosidl_default_generators, rosidl_default_runtime, rosidl_interface_packages, and geometry_msgs entries.

Simulated Service Servers#

Implement two standalone ROS 2 nodes that act as simulated service servers. These simulate a perception system and a command center.

Important

There is no camera or computer vision in this project. Detection is entirely simulated via a service call. The DetectSurvivorServer performs a simple dictionary lookup on the zone ID string – it does not check whether the robot is physically near the zone. The behavior tree enforces the correct sequencing: the DetectSurvivor BT action node is only ticked after NavigateToZone succeeds within the Patrol Sequence, so the robot must reach the zone before detection runs. See Simulation and Provided Code for a full explanation.

DetectSurvivorServer (detect_survivor_server.py):

Advertises a service on detect_survivor using the DetectSurvivor interface.
Maintains a hardcoded dictionary mapping zone IDs to survivor locations. For example:
```
SURVIVORS = {
    "zone_a": (-2.5, 3.2),
    "zone_c": (4.1, -2.5),
}
```
These coordinates are slight offsets from the zone centers (zone_a at (-3.0, 3.0) and zone_c at (4.0, -3.0)) so the reported survivor pose is plausibly inside the zone rather than at its exact center.
When called, if the zone_id is in the dictionary, returns found=True with the survivor’s (x, y) coordinates. Otherwise returns found=False.
Logs each detection request and result.

ReportSurvivorServer (report_survivor_server.py):

Advertises a service on report_survivor using the ReportSurvivor interface.
When called, the server simulates a command center: it does not forward the message anywhere – the “report” is simply a ROS 2 log entry. Log at minimum:
- the survivor_id,
- location.header.frame_id (so it is obvious that the coordinates are in the map frame, not the robot body frame), and
- location.point.x and location.point.y.
Returns acknowledged=True.

Note

The location field uses geometry_msgs/PointStamped – not a bare Point – specifically so the frame in which the coordinates are expressed travels with the data. A receiver that gets a stamped point in "map" can transform it into any other frame with tf2. By contract, the BT node sends frame_id = "map"; the server should reject (or at least warn) if it ever receives a different frame.

ZoneManager#

Implement a plain Python class (not a BT node) at zone_manager.py that manages the mission state. All BT nodes that need zone information receive a reference to the same ZoneManager instance via their constructor.

Required interface:

class ZoneManager:
    def __init__(self, zones: list[dict], base_station: dict):
        """Initialize with zone list and base station pose."""
        ...

    def current_zone(self) -> dict:
        """Return the current zone dict (id, x, y, yaw)."""
        ...

    def has_remaining(self) -> bool:
        """True if there are unvisited zones."""
        ...

    def advance(self) -> None:
        """Move to the next zone."""
        ...

    def base_pose(self) -> dict:
        """Return the base station pose dict (x, y, yaw)."""
        ...

    def next_survivor_id(self) -> str:
        """Return a unique ID like 'survivor_1', 'survivor_2', etc."""
        ...

Behavior Tree#

The full behavior tree must be assembled in scripts/main_search_and_rescue.py. The entry point must:

Load mission parameters from mission_params.yaml (see Reading Auto-Declared YAML Parameters for the has_parameter guard pattern that avoids the ParameterAlreadyDeclaredException trap when reading auto-declared YAML keys).
Build the ZoneManager and the BT leaves.
Auto-seed AMCL with the rosbot’s spawn pose (``x=0, y=0, yaw=0``) using ``BasicNavigator.setInitialPose()`` and block until Nav2 is ACTIVE via ``waitUntilNav2Active()``. See Auto-Seeding AMCL with BasicNavigator for the full snippet. Without this step the BT would tick against an idle Nav2 stack forever, since AMCL never publishes map -> odom without an initial pose.
Wrap the assembled tree in py_trees_ros.trees.BehaviourTree and call tick_tock(period_ms=...).
Spin until shutdown.

The static structure of the tree (composites, conditions, actions) is shown below.

Search-and-rescue behavior tree structure — Fig. 126 Behavior tree structure: composites, conditions, and actions.#

The diagram above shows what the tree looks like. The diagram below shows what it does at runtime – the message flow between your BT node, Nav2, the two simulated service servers, and the static TF broadcaster. The PlantUML source for the sequence diagram lives at docs/source/_static/images/final_project/final_project.puml.

Search-and-rescue mission sequence diagram — Fig. 128 Runtime message flow during a single mission execution.#

How the tree works, tick by tick:

The root Selector (Mission or ReturnToBase, memory=False) tries the Patrol Sequence first.
The Patrol Sequence (memory=True) checks ZonesRemaining?. If all zones are visited (FAILURE), the Patrol Sequence fails and the root Selector falls back to NavigateToBase (mission complete).
If zones remain (SUCCESS), NavigateToZone sends a NavigateToPose goal to Nav2 for the current zone. It returns RUNNING while the goal is in flight and SUCCESS on arrival.
After reaching a zone, DetectSurvivor calls the detection service and stores the result internally.
The HandleDetection Selector ticks its first child, the SurvivorFound Sequence, which in turn ticks IsSurvivorDetected?:
- If found (SUCCESS): the SurvivorFound Sequence continues to BroadcastSurvivorTF (publishes a static TF frame) and then NotifyBase (calls the report service). The Sequence succeeds, so the HandleDetection Selector succeeds.
- If not found (FAILURE): the SurvivorFound Sequence fails on its first child, so HandleDetection ticks its second child, LogNoDetection, which logs a message and returns SUCCESS.
AdvanceZone moves to the next zone, and the Patrol Sequence repeats on the next tick.
Once every zone has been visited, ZonesRemaining? returns FAILURE, so the Patrol Sequence fails and the Selector ticks NavigateToBase. After the robot reaches the base station, the mission is complete – stop the BT node with Ctrl-C.

Important

Wrap NavigateToBase in py_trees.decorators.OneShot(policy=ON_COMPLETION) before adding it to the root Selector. The root Selector is memory=False (reactive), so once Patrol fails it ticks NavigateToBase every tick forever – without OneShot, initialise() re-sends a fresh Nav2 goal at the base each tick, the controller instantly reports “Reached the goal!” because the robot is already there, and the cycle repeats indefinitely (visible in the launch terminal as a flood of Begin navigating from current location ... Goal succeeded pairs every few seconds).

OneShot.ON_COMPLETION caches the first terminal status of its child, so once NavigateToBase succeeds the decorator keeps returning SUCCESS on every subsequent tick without re-entering the child. The robot drives home exactly once and the BT idles at SUCCESS until Ctrl-C. See Implementation Guide for the full snippet.

Condition Nodes#

All condition nodes must inherit from py_trees.behaviour.Behaviour and return only SUCCESS or FAILURE (never RUNNING).

Node	Constructor args	Behavior
`ZonesRemaining`	`name`, `zone_manager`	Returns SUCCESS if `zone_manager.has_remaining()` is True, FAILURE otherwise.
`IsSurvivorDetected`	`name`, `detect_node`	Holds a reference to the `DetectSurvivor` action node. Returns SUCCESS if `detect_node.was_found()` is True, FAILURE otherwise.

Action Nodes#

All action nodes must inherit from py_trees.behaviour.Behaviour. Each creates its ROS 2 resources (publishers, subscribers, service clients, action clients) in setup(**kwargs) using kwargs['node'].

Node	Constructor args	Behavior
`NavigateToZone`	`name`, `zone_manager`	Sends a `NavigateToPose` goal to Nav2 for the current zone from `zone_manager`. Uses `ActionClient` from `rclpy.action` (see `lecture13/mapping_navigation_demo/navigation_demo_interface.py` for the `BasicNavigator` pattern, but here you use the raw action client inside a BT node). Returns RUNNING while navigating, SUCCESS when Nav2 reports success, FAILURE if Nav2 fails. Cancels the goal on `terminate()`.
`NavigateToBase`	`name`, `zone_manager`	Same as `NavigateToZone` but uses `zone_manager.base_pose()` as the target.
`DetectSurvivor`	`name`, `zone_manager`	Calls the `detect_survivor` service with the current zone ID. Stores the result internally. Exposes `was_found()` and `survivor_pose()` methods. Returns SUCCESS when the service call completes (regardless of whether a survivor was found).
`IsSurvivorDetected`	(listed above as condition)
`BroadcastSurvivorTF`	`name`, `detect_node`, `zone_manager`	Creates a `tf2_ros.StaticTransformBroadcaster` in `setup()` using `kwargs['node']`. In `update()`, builds a `geometry_msgs/TransformStamped` and publishes it. Field values: `header.stamp` = current ROS time (`self._node.get_clock().now().to_msg()`). `header.frame_id = "map"` – the parent frame. This must be `"map"` because: (1) the survivor coordinates returned by `DetectSurvivor` are already expressed in the map frame (the same frame Nav2 plans in and AMCL localizes the robot in – see REP-105), and (2) `"map"` is the only world-fixed frame in the system, so a static transform under it stays valid for the rest of the mission even as the robot moves. `child_frame_id = "survivor_N"` – the new frame being created, named via `zone_manager.next_survivor_id()` (`"survivor_1"`, `"survivor_2"`, …). `transform.translation.{x,y}` from `detect_node.survivor_pose()`; `z = 0.0`. `transform.rotation.w = 1.0` (identity quaternion – we are reporting a position, not an orientation). Returns SUCCESS after `sendTransform()`. Verify with `ros2 run tf2_ros tf2_echo map survivor_1` – you should see the translation match the coordinates the `DetectSurvivorServer` returned.
`NotifyBase`	`name`, `detect_node`	Calls the `report_survivor` service with a request whose `survivor_id` is the same string used as the TF child frame (e.g., `"survivor_1"`) and whose `location` is a `geometry_msgs/PointStamped` carrying the survivor’s position in the map frame: `location.header.stamp` = current ROS time. `location.header.frame_id = "map"` – so the receiving server (and any downstream consumer) knows the point is in the global fixed frame, not in the robot’s body frame. `location.point.{x,y}` from `detect_node.survivor_pose()`; `z = 0.0`. Returns SUCCESS if the response’s `acknowledged` field is `True`, FAILURE otherwise.
`AdvanceZone`	`name`, `zone_manager`	Calls `zone_manager.advance()`. Returns SUCCESS.
`LogNoDetection`	`name`	Logs an info message that no survivor was found at the current zone. Returns SUCCESS.

Launch File#

Write a Python launch file at launch/search_and_rescue.launch.py. It is the single entry point for the mission: it must bring up Nav2 and start your two simulated service servers and start the behavior tree node. This should be run in a second terminal, after the Gazebo world is already up. See Simulation and Provided Code for the overall structure and a code skeleton.

Required actions in the launch file:

Action	Package / Source	Notes
`IncludeLaunchDescription` of Nav2 bringup	`nav2_bringup`	Pass `map:=<pkg>/maps/final_project_map.yaml`, `params_file:=<pkg>/config/nav2_params.yaml`, and `use_sim_time:=true`. Adapt from the lecture-13 `map_nav.launch.py` reference – do not invoke `map_nav.launch.py` directly.
`Node` – `search_and_rescue_exe`	`group<N>_final`	The behavior tree entry point.
`Node` – `detect_survivor_server_exe`	`group<N>_final`	Simulated detection service server.
`Node` – `report_survivor_server_exe`	`group<N>_final`	Simulated report service server.
`Node` – `rviz2`	`rviz2`	Optional but recommended. Load `rviz/nav2.rviz` (copy from `lecture13/nav_demo/rviz/nav2.rviz` into your package’s `rviz/` directory and install it via `data_files`). Gate it on a launch argument (`rviz:=true` by default) so headless graders can disable it. With auto-seeding (see Auto-Seeding AMCL with BasicNavigator), RViz is purely for situational awareness; AMCL no longer requires a manual “2D Pose Estimate” click.

Launch file requirements:

All Node actions must use output="screen" and emulate_tty=True.
- output="screen" sends the node’s stdout/stderr (including every get_logger().info() / warn() / error() call) to the launch terminal. The launch default is "log", which writes silently to ~/.ros/log/ and shows nothing live – a common cause of “my logger is broken” reports.
- emulate_tty=True allocates a pseudo-TTY for the child process so Python uses line buffering (every \n flushes immediately) instead of the default block buffering on a pipe (~4–8 KB). Without it, log lines arrive in clumps long after the events that produced them, and ANSI color codes for [INFO]/[WARN]/[ERROR] are stripped.
Resolve all package-relative paths (maps/final_project_map.yaml, config/nav2_params.yaml, config/mission_params.yaml) using get_package_share_directory('group<N>_final'). This implies the maps/ and config/ directories must be installed – add them to data_files in setup.py, otherwise get_package_share_directory() will not find them at runtime.
Load config/mission_params.yaml for the behavior tree node via the parameters field.
Declare at least one launch argument that the BT node consumes (for example, tick_rate_hz with a sensible default), and forward it to the BT node via parameters. The intent is to exercise DeclareLaunchArgument and LaunchConfiguration – not to expose every internal value.
The Gazebo simulation is not started by your launch file – the user runs ros2 launch rosbot_gazebo final_project_world.launch.py in a separate terminal first.

Parameter File#

Create config/mission_params.yaml with the following structure:

/**:
  ros__parameters:
    # Patrol order. Edit this string array to change the visit
    # order without touching any pose values below.
    zone_order: ["zone_a", "zone_b", "zone_c", "zone_d"]

    # Per-zone poses (metres + radians) in the map frame.
    zones:
      zone_a: {x: -3.0, y:  3.0, yaw:  0.0}
      zone_b: {x:  3.5, y:  3.0, yaw:  1.57}
      zone_c: {x:  4.0, y: -3.0, yaw:  3.14}
      zone_d: {x: -3.5, y: -3.0, yaw: -1.57}

    base_station:
      x: 0.0
      y: 0.0
      yaw: 0.0

    tick_rate_hz: 2.0

Important

Why two keys (``zone_order`` + ``zones``) instead of a list of dicts? ROS 2 YAML parameter files require every list element to be the same primitive type. A list of dicts – the most intuitive way to express ordered named records – fails to load:

Couldn't parse params file ... Sequence should be of same type.
Value type 'double' do not belong ...

Splitting the data into a string array (zone_order, which carries the patrol sequence) plus a nested dict (zones.<id>, which groups each zone’s pose fields) sidesteps this restriction and keeps each zone’s fields visually grouped.

Reading the parameters back. Because the patrol order is encoded in zone_order rather than YAML insertion order, the entry point joins the two halves back into the natural list-of-dicts:

# Read the patrol order
zone_order = (
    node.get_parameter("zone_order")
        .get_parameter_value()
        .string_array_value
)

# Look up each zone's pose under zones.<id>.{x, y, yaw}
zones = []
for zone_id in zone_order:
    zones.append({
        "id": zone_id,
        "x":   node.get_parameter(f"zones.{zone_id}.x").value,
        "y":   node.get_parameter(f"zones.{zone_id}.y").value,
        "yaw": node.get_parameter(f"zones.{zone_id}.yaw").value,
    })

If you create the temporary parameter node with automatically_declare_parameters_from_overrides=True and allow_undeclared_parameters=True, every key under the YAML’s ros__parameters block is auto-declared and can be read directly.

The /**: wildcard. The top-level key /** is a ROS 2 parameter-file glob: a single * matches one node-name token, and the doubled /** matches any node name in any namespace. Because of this, every node started with this YAML loaded will receive the parameters under ros__parameters – you do not have to hard-code the BT node’s name (or its namespace) here.

Why this matters in practice:

The BT entry point creates an rclpy node whose name you may change later (e.g., when prefixing with the group number). With /**, the YAML keeps working without edits.
If you launch the same node under a namespace (namespace="group3" in the launch file), a fully-qualified key like search_and_rescue: would silently fail to match (the actual node would be /group3/search_and_rescue); /** matches both forms.
You can be more selective when you need to. For example, scoping one parameter to a specific node:
```
/**:
  ros__parameters:
    tick_rate_hz: 2.0

search_and_rescue:
  ros__parameters:
    zone_order: [...]
    zones: {...}
```
Here tick_rate_hz is shared by every node, while zone_order and zones only load into a node literally named search_and_rescue.

For this assignment, the simple /** block above is enough – all parameters belong to the BT node, and there is only one consumer.

README.md#

Your README.md must contain:

A contributions section listing each group member and a short (2 to 4 sentence) summary of what they personally wrote, debugged, tested, or documented.
A brief description of your behavior tree design (which composites use memory=True vs memory=False and why).

No other sections are required.

Code Quality#

Warning

The following are mandatory and will result in point deductions if missing.

Docstrings: Every class and every method must have a Google-style docstring.
Type hints: All method parameters and return types must have type annotations.
Inline comments: Include comments that explain non-obvious logic (e.g., Nav2 goal construction, TF frame broadcasting, service call handling).
Naming conventions: snake_case for topics, services, methods, and variables. CamelCase for class names.
Logging: Use the ROS 2 logger exclusively. Never use print(). Use the appropriate severity level.
Linting: Ensure Ruff is enabled and no errors appear.

Requirements#

Your Tasks#

Package Structure#

Build the Map#

Service Interfaces#

Simulated Service Servers#

ZoneManager#

Behavior Tree#

Condition Nodes#

Action Nodes#

Launch File#

Parameter File#

README.md#

Code Quality#

This Page