diff --git a/discourse/3-layered-global-occupancy-map.md b/discourse/3-layered-global-occupancy-map.md index 3dbec68..256685e 100644 --- a/discourse/3-layered-global-occupancy-map.md +++ b/discourse/3-layered-global-occupancy-map.md @@ -24,6 +24,7 @@ observations, can be added later. * The Rust map server composes a static occupancy grid and active observations into `/map` * The Nav2 demo converts scans from three robots into clear ray sectors and occupied endpoint regions, then displays the source contributions and map +* The replanning demo fuses two robots' scans and replans when an updated map blocks an active route * The observation messages live in `rmf_layered_map_msgs`, leaving `rmf_prototype_msgs` unchanged @@ -133,6 +134,12 @@ The demo launches Nav2 localization, planning, and control for the fixed goal lo The scan-to-region conversion uses one convex sector per sampled beam instead of expanding a Bresenham line into many point regions. Scan sampling reduces the number of sectors, publication throttling limits the snapshot rate, and the observation range limits represented returns. The TTL controls how quickly stale observations expire. Each publisher logs its input beams and clear and obstacle regions so message density, update rate, and visual fidelity can be compared. The current demo remains a 2D occupancy approximation; it does not fuse probabilistic confidence or compress adjacent sectors into larger regions. +# Two-Robot Replanning Demo + +The replanning launch starts with a simple room by default and retains the warehouse layout as a regression scenario. It fuses both robots' scans into `/map`, spawns a blocking bar, and displays robot-colored poses, goals, observations, and Nav2 paths alongside the green RMF plans. + +The plan executor checks each remaining route against the composed map and publishes `CODE_PATH_BLOCKED` after a short debounce. The path server replans on a conservatively downsampled `1.0 m` grid, while the Nav2 bridge ignores superseded aborts and resends unchanged targets when a new plan requires them. + # Example Flow A local costmap or LiDAR observation node can publish a replacement snapshot by: diff --git a/map_server/README.md b/map_server/README.md index ee71057..15978b6 100644 --- a/map_server/README.md +++ b/map_server/README.md @@ -12,4 +12,4 @@ ros2 launch rmf_layered_map_server_demo demo.launch.py Use `use_rviz:=False` for a headless run. -See [`rmf_layered_map_server_demo/README.md`](rmf_layered_map_server_demo/README.md) for the three-robot Nav2 observation demo. +See [`rmf_layered_map_server_demo/README.md`](rmf_layered_map_server_demo/README.md) for the three-robot observation demo and two-robot replanning demo. diff --git a/map_server/rmf_layered_map_server_demo/README.md b/map_server/rmf_layered_map_server_demo/README.md index 136dac7..7ea5acb 100644 --- a/map_server/rmf_layered_map_server_demo/README.md +++ b/map_server/rmf_layered_map_server_demo/README.md @@ -1,6 +1,6 @@ # Layered Map Server Demos -This package contains two demonstrations of the layered map server. +This package contains three demonstrations of the layered map server. ## TTL Smoke Test @@ -31,3 +31,46 @@ By default, the demo opens three robot-local RViz windows and one combined-map v * Use `map` and `params_file` to override the warehouse map and shared Nav2 parameters. * `beam_stride:=1` and `publish_period_sec:=0.5` control scan sampling. * `max_observation_range:=2.5` and `ttl_sec:=10.0` control range and retention. + +## Two-Robot Obstacle Replanning Demo + +This demo connects the layered map server to the path server, plan executor, and Nav2 traffic bridge. It exercises the `CODE_PATH_BLOCKED` replanning flow from [PR #39](https://github.com/open-rmf/next_gen_prototype/pull/39). + +The default scenario is a simple room with no aisles: + +```bash +ros2 launch rmf_layered_map_server_demo replan_obstacle.launch.py +``` + +Two robots start with non-overlapping scan regions and separate goals. Once their initial plans are active, the demo spawns a red bar across both scan regions. The bar meets the right wall to form a dead end and leaves a detour around its left end. + +The global RViz view shows robot-colored poses, goals, region contributions, and Nav2 paths alongside the green RMF plans. + +Use the warehouse scenario to reproduce the original two-aisle case: + +```bash +ros2 launch rmf_layered_map_server_demo replan_obstacle.launch.py \ + scenario:=warehouse +``` + +This scenario keeps the original robot poses and 15 m shelf-connected bar. + +Run only one scenario per ROS/Gazebo domain because both use the same robot and map topic names. For concurrent runs, set different `ROS_DOMAIN_ID` and `GZ_PARTITION` values. + +The expected sequence in the global RViz window is: + +1. Both initial plans appear on the static map. +2. The long bar is spawned and its detected portions enter the combined `/map`. +3. After a 300 ms debounce, the plan executor publishes `PlanError.CODE_PATH_BLOCKED` for the blocked route. +4. The path server publishes a new plan using the updated map. + +The path server conservatively downsamples the `0.1 m` simple-room map and the `0.03 m` warehouse map to its `1.0 m` PiBT planning resolution. Any planning cell containing an occupied source cell remains occupied. Replan reports have a two-second cooldown to prevent oscillation. + +Useful launch arguments: + +* `use_nav2_rviz:=True` opens the robot-local Nav2 views. +* `use_global_rviz:=False` runs without the combined RViz view. +* `spawn_delay_sec:=1.0` controls the delay after initial plans are received. +* `scenario_timeout_sec:=180.0` controls how long the demo waits for replanning. +* `obstacle_memory_sec:=-1.0` retains obstacle endpoints for the scanner's lifetime. Use `0.0` to disable memory or a positive value for finite retention. +* `self_filter_radius:=0.22` excludes scan returns inside the robot body. diff --git a/map_server/rmf_layered_map_server_demo/launch/replan_obstacle.launch.py b/map_server/rmf_layered_map_server_demo/launch/replan_obstacle.launch.py new file mode 100644 index 0000000..95785aa --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/launch/replan_obstacle.launch.py @@ -0,0 +1,39 @@ +# Copyright 2026 Open Source Robotics Foundation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration + +from rmf_layered_map_server_demo.replan_obstacle_launch import ( + generate_replan_launch_description, +) + + +def _launch_scenario(context): + scenario = LaunchConfiguration('scenario').perform(context) + return generate_replan_launch_description(scenario).entities + + +def generate_launch_description(): + """Launch a replanning scenario.""" + return LaunchDescription([ + DeclareLaunchArgument( + 'scenario', + default_value='simple', + choices=['simple', 'warehouse'], + description='Replanning environment to launch.', + ), + OpaqueFunction(function=_launch_scenario), + ]) diff --git a/map_server/rmf_layered_map_server_demo/maps/single_room.png b/map_server/rmf_layered_map_server_demo/maps/single_room.png new file mode 100644 index 0000000..06068fb Binary files /dev/null and b/map_server/rmf_layered_map_server_demo/maps/single_room.png differ diff --git a/map_server/rmf_layered_map_server_demo/maps/single_room.yaml b/map_server/rmf_layered_map_server_demo/maps/single_room.yaml new file mode 100644 index 0000000..701c96d --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/maps/single_room.yaml @@ -0,0 +1,7 @@ +image: single_room.png +mode: trinary +resolution: 0.1 +origin: [-10.0, -8.0, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.1 diff --git a/map_server/rmf_layered_map_server_demo/package.xml b/map_server/rmf_layered_map_server_demo/package.xml index 0ec46c5..90d38f7 100644 --- a/map_server/rmf_layered_map_server_demo/package.xml +++ b/map_server/rmf_layered_map_server_demo/package.xml @@ -13,13 +13,20 @@ rclpy ament_index_python geometry_msgs + lifecycle_msgs gz_tools_vendor launch launch_ros nav_msgs + nav2_common nav2_msgs rmf_layered_map_msgs rmf_layered_map_server + rmf_nav2_traffic + rmf_path_server + rmf_path_visualizer + rmf_plan_executor + rmf_simple_destination_server rmf_prototype_msgs rviz2 sensor_msgs diff --git a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/region_update_visualizer.py b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/region_update_visualizer.py index bae3500..c0f5575 100644 --- a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/region_update_visualizer.py +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/region_update_visualizer.py @@ -23,14 +23,16 @@ from rmf_prototype_msgs.msg import Region from visualization_msgs.msg import Marker, MarkerArray +from .replan_scenarios import ROBOT_COLORS + SOURCE_COLORS = ( - (0.96, 0.26, 0.21), + ROBOT_COLORS['robot0'][:3], + ROBOT_COLORS['robot1'][:3], (0.18, 0.80, 0.44), - (0.20, 0.60, 0.98), - (1.00, 0.65, 0.15), (0.61, 0.35, 0.71), (0.10, 0.74, 0.80), + (0.96, 0.26, 0.21), ) CLEAR_COLOR_BLEND = 0.5 CLEAR_MARKER_Z = 0.04 @@ -183,10 +185,15 @@ def apply_update(self, update): if latest_stamp is not None and stamp_nsec < latest_stamp: return MarkerArray() - color = self.source_colors.setdefault( - key, - SOURCE_COLORS[len(self.source_colors) % len(SOURCE_COLORS)], - ) + color = self.source_colors.get(key) + if color is None: + robot_color = ROBOT_COLORS.get(update.source.robot_name) + color = ( + robot_color[:3] + if robot_color is not None + else SOURCE_COLORS[len(self.source_colors) % len(SOURCE_COLORS)] + ) + self.source_colors[key] = color marker_array = MarkerArray() self.markers_by_source[key] = [ marker diff --git a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_obstacle_demo.py b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_obstacle_demo.py new file mode 100644 index 0000000..6601633 --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_obstacle_demo.py @@ -0,0 +1,525 @@ +# Copyright 2026 Open Source Robotics Foundation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from math import atan2, cos, pi, sin +import subprocess +import time + +from geometry_msgs.msg import Point +from lifecycle_msgs.msg import State +from lifecycle_msgs.srv import GetState +from nav2_msgs.action import NavigateToPose +from nav_msgs.msg import OccupancyGrid, Odometry +import rclpy +from rclpy.action import ActionClient +from rclpy.node import Node +from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy +from rmf_prototype_msgs.msg import Plan +from visualization_msgs.msg import Marker, MarkerArray + +from .replan_scenarios import get_scenario, ROBOT_COLORS + + +ROBOT_MARKER_Z = 0.40 +GOAL_MARKER_Z = 0.30 + + +def make_navigation_goal(x, y, yaw=pi / 2.0): + """Create a map-frame Nav2 goal.""" + goal = NavigateToPose.Goal() + goal.pose.header.frame_id = 'map' + goal.pose.pose.position.x = float(x) + goal.pose.pose.position.y = float(y) + goal.pose.pose.orientation.z = sin(yaw / 2.0) + goal.pose.pose.orientation.w = cos(yaw / 2.0) + return goal + + +def plan_signature(plan): + """Return a plan's waypoint coordinates.""" + return tuple((waypoint.position[0], waypoint.position[1]) for waypoint in plan.waypoints) + + +def _point(x, y, z=0.1): + point = Point() + point.x = float(x) + point.y = float(y) + point.z = float(z) + return point + + +def _base_marker(marker_id, namespace, marker_type, color): + marker = Marker() + marker.header.frame_id = 'map' + marker.ns = namespace + marker.id = marker_id + marker.type = marker_type + marker.action = Marker.ADD + marker.pose.orientation.w = 1.0 + marker.scale.x = 1.0 + marker.scale.y = 1.0 + marker.scale.z = 1.0 + marker.color.r, marker.color.g, marker.color.b, marker.color.a = color + return marker + + +def triangle_marker(marker_id, namespace, x, y, yaw, color): + """Create a robot marker.""" + marker = _base_marker(marker_id, namespace, Marker.TRIANGLE_LIST, color) + local_points = ((0.65, 0.0), (-0.45, 0.4), (-0.45, -0.4)) + for local_x, local_y in local_points: + world_x = x + cos(yaw) * local_x - sin(yaw) * local_y + world_y = y + sin(yaw) * local_x + cos(yaw) * local_y + marker.points.append(_point(world_x, world_y, ROBOT_MARKER_Z)) + return marker + + +def star_marker( + marker_id, + x, + y, + color=(0.25, 0.75, 0.12, 1.0), + namespace='goal', +): + """Create a star-shaped goal marker.""" + marker = _base_marker( + marker_id, + namespace, + Marker.TRIANGLE_LIST, + color, + ) + ring = [] + for index in range(10): + radius = 0.8 if index % 2 == 0 else 0.35 + angle = pi / 2.0 + index * pi / 5.0 + ring.append( + _point( + x + radius * cos(angle), + y + radius * sin(angle), + GOAL_MARKER_Z, + ) + ) + center = _point(x, y, GOAL_MARKER_Z) + for index, point in enumerate(ring): + marker.points.extend((center, point, ring[(index + 1) % len(ring)])) + return marker + + +def bar_marker(marker_id, scenario): + """Create the obstacle marker.""" + marker = _base_marker( + marker_id, + 'spawned_obstacle', + Marker.CUBE, + (0.95, 0.05, 0.05, 0.9), + ) + marker.pose.position.x = scenario.bar_center[0] + marker.pose.position.y = scenario.bar_center[1] + marker.pose.position.z = 0.1 + marker.scale.x = scenario.bar_size[0] + marker.scale.y = scenario.bar_size[1] + marker.scale.z = 0.2 + return marker + + +def box_sdf(name, x, y, size_x, size_y, size_z, color): + """Build SDF for a static box.""" + rgba = ' '.join(str(channel) for channel in color) + return ( + "" + f"" + f'{x} {y} {size_z / 2.0} 0 0 0' + 'true' + "" + "" + f'{size_x} {size_y} {size_z}' + f'{rgba}{rgba}' + '' + "" + f'{size_x} {size_y} {size_z}' + '' + '' + '' + '' + ) + + +def yaw_from_odometry(odometry): + """Return the planar yaw from odometry.""" + orientation = odometry.pose.pose.orientation + return atan2( + 2.0 * (orientation.w * orientation.z + orientation.x * orientation.y), + 1.0 - 2.0 * (orientation.y * orientation.y + orientation.z * orientation.z), + ) + + +def spawn_bar(world_name, scenario): + """Spawn the obstacle in Gazebo.""" + sdf = box_sdf( + scenario.bar_name, + scenario.bar_center[0], + scenario.bar_center[1], + *scenario.bar_size, + color=(0.95, 0.05, 0.05, 1.0), + ) + request = f'sdf: "{sdf}"' + return subprocess.run( + [ + 'gz', + 'service', + '-s', + f'/world/{world_name}/create', + '--reqtype', + 'gz.msgs.EntityFactory', + '--reptype', + 'gz.msgs.Boolean', + '--timeout', + '1000', + '--req', + request, + ], + capture_output=True, + check=False, + text=True, + timeout=3.0, + ) + + +def spawn_succeeded(result): + """Check that Gazebo accepted the entity.""" + output = f'{result.stdout}\n{result.stderr}'.lower() + return result.returncode == 0 and 'data: true' in output + + +class ReplanObstacleDemo(Node): + """Run the two-robot replanning demo.""" + + def __init__(self): + super().__init__('replan_obstacle_demo') + scenario_name = self.declare_parameter('scenario', 'simple').value + self.scenario = get_scenario(scenario_name) + self.robot_layout = { + robot.name: robot.pose for robot in self.scenario.robots + } + self.robot_goals = { + robot.name: robot.goal for robot in self.scenario.robots + } + self.world_name = self.declare_parameter( + 'world_name', self.scenario.world_name + ).value + self.spawn_delay_sec = self.declare_parameter( + 'spawn_delay_sec', 4.0 + ).value + self.timeout_sec = self.declare_parameter( + 'scenario_timeout_sec', 180.0 + ).value + self.started_at = time.monotonic() + self.spawn_at = None + self.next_spawn_attempt = None + self.state = 'waiting_for_inputs' + self.timeout_logged = False + self.map_ready = False + self.bar_spawned = False + self.odometry = {} + self.initial_versions = {} + self.initial_paths = {} + self.replan_versions = {} + self.changed_paths = {} + + reliable_transient_qos = QoSProfile( + depth=10, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + reliability=ReliabilityPolicy.RELIABLE, + ) + self.navigation_clients = {} + self.inner_navigation_clients = {} + self.nav2_lifecycle_clients = {} + self.nav2_state_futures = {} + self.nav2_active = set() + self.goal_futures = {} + self.goal_handles = {} + self._demo_subscriptions = [] + for robot_name in self.robot_layout: + self.navigation_clients[robot_name] = ActionClient( + self, + NavigateToPose, + f'/{robot_name}/navigate_to_pose', + ) + # Wait for Nav2 to activate before sending outer goals. + self.inner_navigation_clients[robot_name] = ActionClient( + self, + NavigateToPose, + f'/{robot_name}/inner/navigate_to_pose', + ) + self.nav2_lifecycle_clients[robot_name] = self.create_client( + GetState, + f'/{robot_name}/inner/bt_navigator/get_state', + ) + self._demo_subscriptions.append( + self.create_subscription( + Plan, + f'/{robot_name}/plan', + lambda msg, name=robot_name: self.receive_plan(name, msg), + reliable_transient_qos, + ) + ) + self._demo_subscriptions.append( + self.create_subscription( + Odometry, + f'/{robot_name}/odom', + lambda msg, name=robot_name: self.receive_odometry(name, msg), + 10, + ) + ) + + self._demo_subscriptions.append( + self.create_subscription( + OccupancyGrid, + '/map', + self.receive_map, + reliable_transient_qos, + ) + ) + self.marker_publisher = self.create_publisher( + MarkerArray, + '/replan_scenario/markers', + reliable_transient_qos, + ) + self.timer = self.create_timer(0.25, self.tick) + self.marker_timer = self.create_timer(0.5, self.publish_markers) + + def receive_map(self, msg): + self.map_ready = ( + msg.info.width > 0 + and msg.info.height > 0 + and msg.info.resolution > 0.0 + ) + + def receive_odometry(self, robot_name, msg): + self.odometry[robot_name] = msg + + def receive_plan(self, robot_name, msg): + version = msg.plan_id.plan_version + signature = plan_signature(msg) + if robot_name not in self.initial_versions: + self.initial_versions[robot_name] = version + self.initial_paths[robot_name] = signature + self.get_logger().info( + f'Received initial {robot_name} plan v{version} ' + f'with {len(signature)} waypoints' + ) + elif version > self.initial_versions[robot_name]: + self.replan_versions[robot_name] = version + self.changed_paths[robot_name] = ( + signature != self.initial_paths[robot_name] + ) + self.get_logger().info( + f'Received replanned {robot_name} plan v{version}; ' + f'route changed={self.changed_paths[robot_name]}' + ) + + if ( + self.state == 'waiting_for_initial_plans' + and len(self.initial_versions) == len(self.robot_layout) + ): + self.spawn_at = time.monotonic() + self.spawn_delay_sec + self.state = 'waiting_to_spawn' + self.get_logger().info( + f'Initial plans are active; spawning the blocking bar in ' + f'{self.spawn_delay_sec:.1f}s' + ) + + if ( + self.state == 'waiting_for_replan' + and 'robot1' in self.replan_versions + ): + if self.changed_paths.get('robot1', False): + versions = ', '.join( + f'{name}=v{self.replan_versions[name]}' + for name in sorted(self.replan_versions) + ) + self.get_logger().info( + f'Replan succeeded ({versions}); ' + 'robot1 now avoids the spawned obstacle' + ) + self.state = 'complete' + else: + self.get_logger().error( + 'Plan version increased without a route change' + ) + self.state = 'failed' + + def receive_goal_response(self, robot_name, future): + try: + goal_handle = future.result() + except Exception as error: + self.get_logger().error( + f'Outer navigation request for {robot_name} failed: {error}' + ) + self.state = 'failed' + return + if not goal_handle.accepted: + self.get_logger().error( + f'Outer navigation request for {robot_name} was rejected' + ) + self.state = 'failed' + return + self.goal_handles[robot_name] = goal_handle + self.get_logger().info( + f'Outer navigation request for {robot_name} was accepted' + ) + + def nav2_servers_are_active(self): + """Check whether both Nav2 servers are active.""" + for robot_name, client in self.nav2_lifecycle_clients.items(): + future = self.nav2_state_futures.get(robot_name) + if future is not None and future.done(): + try: + response = future.result() + except Exception as error: + self.get_logger().warning( + f'Cannot read {robot_name} Nav2 lifecycle state: {error}' + ) + else: + if response.current_state.id == State.PRIMARY_STATE_ACTIVE: + self.nav2_active.add(robot_name) + del self.nav2_state_futures[robot_name] + + if ( + robot_name not in self.nav2_active + and robot_name not in self.nav2_state_futures + and client.service_is_ready() + ): + self.nav2_state_futures[robot_name] = client.call_async( + GetState.Request() + ) + + return len(self.nav2_active) == len(self.robot_layout) + + def tick(self): + now = time.monotonic() + if ( + not self.timeout_logged + and self.state not in ('complete', 'failed') + and now - self.started_at > self.timeout_sec + ): + timed_out_state = self.state + self.timeout_logged = True + self.state = 'failed' + self.get_logger().error( + f'Demo timed out in state {timed_out_state}; ' + f'initial plans={sorted(self.initial_versions)}, ' + f'replans={sorted(self.replan_versions)}' + ) + return + + if self.state == 'waiting_for_inputs': + action_servers_ready = all( + client.server_is_ready() + for client in self.navigation_clients.values() + ) and all( + client.server_is_ready() + for client in self.inner_navigation_clients.values() + ) + nav2_active = self.nav2_servers_are_active() + if ( + self.map_ready + and len(self.odometry) == len(self.robot_layout) + and action_servers_ready + and nav2_active + ): + for robot_name, (goal_x, goal_y) in self.robot_goals.items(): + goal = make_navigation_goal(goal_x, goal_y) + future = self.navigation_clients[robot_name].send_goal_async(goal) + future.add_done_callback( + lambda result, name=robot_name: self.receive_goal_response( + name, result + ) + ) + self.goal_futures[robot_name] = future + self.state = 'waiting_for_initial_plans' + self.get_logger().info('Sent navigation requests') + return + + if self.state == 'waiting_to_spawn' and now >= self.spawn_at: + if self.next_spawn_attempt is not None and now < self.next_spawn_attempt: + return + try: + result = spawn_bar(self.world_name, self.scenario) + except (OSError, subprocess.TimeoutExpired) as error: + self.get_logger().warning(f'Waiting to spawn obstacle: {error}') + self.next_spawn_attempt = now + 2.0 + return + + if not spawn_succeeded(result): + detail = result.stderr.strip() or result.stdout.strip() + if not detail: + detail = 'Gazebo did not confirm entity creation' + self.get_logger().warning(f'Waiting to spawn obstacle: {detail}') + self.next_spawn_attempt = now + 2.0 + return + + self.bar_spawned = True + self.state = 'waiting_for_replan' + self.get_logger().info( + f'Spawned bar at {self.scenario.bar_center}; ' + 'waiting for CODE_PATH_BLOCKED' + ) + + def publish_markers(self): + markers = [ + star_marker( + 100 + index, + *robot.goal, + color=ROBOT_COLORS[robot.name], + namespace=f'{robot.name}_goal', + ) + for index, robot in enumerate(self.scenario.robots) + ] + for index, (robot_name, initial_pose) in enumerate( + self.robot_layout.items() + ): + if robot_name in self.odometry: + pose = self.odometry[robot_name].pose.pose + x = pose.position.x + y = pose.position.y + yaw = yaw_from_odometry(self.odometry[robot_name]) + else: + x, y, yaw = initial_pose + color = ROBOT_COLORS[robot_name] + markers.append( + triangle_marker( + index, + f'{robot_name}_pose', + x, + y, + yaw, + color, + ) + ) + if self.bar_spawned: + markers.append(bar_marker(200, self.scenario)) + self.marker_publisher.publish(MarkerArray(markers=markers)) + + +def main(): + rclpy.init() + node = ReplanObstacleDemo() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.try_shutdown() diff --git a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_obstacle_launch.py b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_obstacle_launch.py new file mode 100644 index 0000000..3d1d8b0 --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_obstacle_launch.py @@ -0,0 +1,292 @@ +# Copyright 2026 Open Source Robotics Foundation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + EmitEvent, + ExecuteProcess, + RegisterEventHandler, + TimerAction, +) +from launch.conditions import IfCondition +from launch.event_handlers import OnProcessExit +from launch.events import Shutdown +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue +from nav2_common.launch import RewrittenYaml + +from .replan_scenarios import get_scenario + + +def _robots_argument(scenario): + """Format robot poses for Nav2 bringup.""" + return '; '.join( + f'{robot.name}={{x: {robot.pose[0]}, y: {robot.pose[1]}, ' + f'yaw: {robot.pose[2]}}}' + for robot in scenario.robots + ) + + +def generate_replan_launch_description(scenario_name): + """Build the replanning launch description.""" + scenario_config = get_scenario(scenario_name) + demo_share = get_package_share_directory('rmf_layered_map_server_demo') + nav2_share = get_package_share_directory('sp_demo_nav2_bringup') + + if scenario_name == 'simple': + default_map = os.path.join(demo_share, 'maps', 'single_room.yaml') + default_world = os.path.join(demo_share, 'worlds', 'single_room.sdf') + else: + default_map = os.path.join(nav2_share, 'maps', 'warehouse.yaml') + default_world = os.path.join( + get_package_share_directory('nav2_minimal_tb4_sim'), + 'worlds', + 'warehouse.sdf', + ) + + map_file = LaunchConfiguration('map') + world_file = LaunchConfiguration('world') + world_name = LaunchConfiguration('world_name') + params_file = LaunchConfiguration('params_file') + use_nav2_rviz = LaunchConfiguration('use_nav2_rviz') + use_global_rviz = LaunchConfiguration('use_global_rviz') + spawn_delay_sec = LaunchConfiguration('spawn_delay_sec') + scenario_timeout_sec = LaunchConfiguration('scenario_timeout_sec') + obstacle_memory_sec = LaunchConfiguration('obstacle_memory_sec') + self_filter_radius = LaunchConfiguration('self_filter_radius') + demo_params_file = RewrittenYaml( + source_file=params_file, + param_rewrites={ + ( + 'controller_server.ros__parameters.general_goal_checker.' + 'stateful' + ): 'False', + ( + 'controller_server.ros__parameters.general_goal_checker.' + 'xy_goal_tolerance' + ): '0.10', + ( + 'controller_server.ros__parameters.FollowPath.' + 'xy_goal_tolerance' + ): '0.10', + }, + convert_types=True, + ) + + declarations = [ + DeclareLaunchArgument( + 'map', + default_value=default_map, + description=f'Static map for the {scenario_name} scenario.', + ), + DeclareLaunchArgument( + 'world', + default_value=default_world, + description=f'Gazebo world for the {scenario_name} scenario.', + ), + DeclareLaunchArgument( + 'world_name', + default_value=scenario_config.world_name, + description='Gazebo world used by the obstacle spawner.', + ), + DeclareLaunchArgument( + 'params_file', + default_value=os.path.join( + nav2_share, 'params', 'nav2_multirobot_params_all.yaml' + ), + description='Nav2 parameters shared by both robots.', + ), + DeclareLaunchArgument( + 'use_nav2_rviz', + default_value='False', + description='Whether to open one local Nav2 RViz per robot.', + ), + DeclareLaunchArgument( + 'use_global_rviz', + default_value='True', + description='Whether to open the combined map and plan view.', + ), + DeclareLaunchArgument( + 'spawn_delay_sec', + default_value='1.0', + description='Delay between initial plans and obstacle spawn.', + ), + DeclareLaunchArgument( + 'scenario_timeout_sec', + default_value='180.0', + description='Timeout for observing a replan.', + ), + DeclareLaunchArgument( + 'obstacle_memory_sec', + default_value='-1.0', + description='Obstacle retention in seconds; negative keeps points.', + ), + DeclareLaunchArgument( + 'self_filter_radius', + default_value='0.22', + description='Radius excluded from each robot scan.', + ), + ] + + nav2_simulation = ExecuteProcess( + cmd=[ + 'ros2', + 'launch', + 'sp_demo_nav2_bringup', + 'cloned_multi_tb3_simulation_launch.py', + f'robots:={_robots_argument(scenario_config)}', + ['map:=', map_file], + ['world:=', world_file], + ['params_file:=', demo_params_file], + ['use_rviz:=', use_nav2_rviz], + 'use_navigation:=True', + 'staged_startup:=True', + ], + output='screen', + ) + + layered_map_server = Node( + package='rmf_layered_map_server', + executable='rmf_layered_map_server', + output='screen', + remappings=[('/map/static', '/robot0/inner/map')], + ) + region_visualizer = Node( + condition=IfCondition(use_global_rviz), + package='rmf_layered_map_server_demo', + executable='region_update_visualizer', + output='screen', + ) + observation_nodes = [ + Node( + package='rmf_layered_map_server_demo', + executable='scan_region_publisher', + namespace=f'{robot.name}/inner', + output='screen', + parameters=[{ + 'robot_name': robot.name, + 'map_frame': 'map', + 'map_name': scenario_config.map_name, + 'scan_topic': f'/{robot.name}/inner/scan', + 'beam_stride': 1, + 'publish_period_sec': 0.5, + 'ttl_sec': 10.0, + 'reset_source': True, + 'obstacle_memory_sec': ParameterValue( + obstacle_memory_sec, value_type=float + ), + 'obstacle_memory_resolution': 0.1, + 'self_filter_radius': ParameterValue( + self_filter_radius, value_type=float + ), + 'max_observation_range': scenario_config.scan_radius, + }], + remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')], + ) + for robot in scenario_config.robots + ] + + path_server = Node( + package='rmf_path_server', + executable='rmf_path_server', + name='rmf_path_server', + output='both', + ) + destination_server = Node( + package='rmf_simple_destination_server', + executable='rmf_simple_destination_server', + name='rmf_simple_destination_server', + output='both', + ) + plan_executor = Node( + package='rmf_plan_executor', + executable='rmf_plan_executor', + name='rmf_plan_executor', + output='both', + ) + nav2_traffic = Node( + package='rmf_nav2_traffic', + executable='nav2_traffic', + name='nav2_traffic', + output='both', + parameters=[{'use_sim_time': True}], + ) + path_visualizer = Node( + condition=IfCondition(use_global_rviz), + package='rmf_path_visualizer', + executable='rmf_path_visualizer', + name='rmf_path_visualizer', + output='both', + ) + scenario = TimerAction( + period=2.0, + actions=[ + Node( + package='rmf_layered_map_server_demo', + executable='replan_obstacle_demo', + output='screen', + parameters=[{ + 'scenario': scenario_name, + 'world_name': ParameterValue(world_name, value_type=str), + 'spawn_delay_sec': ParameterValue( + spawn_delay_sec, value_type=float + ), + 'scenario_timeout_sec': ParameterValue( + scenario_timeout_sec, value_type=float + ), + }], + ), + ], + ) + + global_rviz = Node( + condition=IfCondition(use_global_rviz), + package='rviz2', + executable='rviz2', + name='replan_obstacle_rviz', + arguments=[ + '-d', + os.path.join(demo_share, 'rviz', 'nav2_observations.rviz'), + ], + parameters=[{'use_sim_time': False}], + output='screen', + ) + global_rviz_exit_handler = RegisterEventHandler( + condition=IfCondition(use_global_rviz), + event_handler=OnProcessExit( + target_action=global_rviz, + on_exit=EmitEvent(event=Shutdown(reason='global RViz exited')), + ), + ) + + return LaunchDescription([ + *declarations, + nav2_simulation, + layered_map_server, + region_visualizer, + *observation_nodes, + destination_server, + path_server, + plan_executor, + nav2_traffic, + path_visualizer, + scenario, + global_rviz, + global_rviz_exit_handler, + ]) diff --git a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_scenarios.py b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_scenarios.py new file mode 100644 index 0000000..276f68b --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_scenarios.py @@ -0,0 +1,91 @@ +# Copyright 2026 Open Source Robotics Foundation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from dataclasses import dataclass +from math import pi + + +ROBOT_COLORS = { + 'robot0': (0.12, 0.55, 0.85, 1.0), + 'robot1': (1.0, 0.42, 0.08, 1.0), +} + + +@dataclass(frozen=True) +class RobotLayout: + """Initial pose and destination for one demo robot.""" + + name: str + pose: tuple[float, float, float] + goal: tuple[float, float] + + +@dataclass(frozen=True) +class ReplanScenario: + """Geometry and resource names for a replanning scenario.""" + + name: str + map_name: str + world_name: str + robots: tuple[RobotLayout, ...] + scan_radius: float + bar_name: str + bar_center: tuple[float, float] + bar_size: tuple[float, float, float] + + +SIMPLE_SCENARIO = ReplanScenario( + name='simple', + map_name='single_room', + world_name='single_room', + robots=( + RobotLayout('robot0', (-3.0, -4.5, pi / 2.0), (0.0, 6.0)), + RobotLayout('robot1', (5.5, -4.5, pi / 2.0), (6.0, 4.0)), + ), + scan_radius=4.0, + bar_name='simple_replan_bar', + bar_center=(1.75, -1.5), + bar_size=(15.5, 0.5, 1.0), +) + +WAREHOUSE_SCENARIO = ReplanScenario( + name='warehouse', + map_name='warehouse', + world_name='warehouse', + robots=( + RobotLayout('robot0', (-12.0, -21.0, pi / 2.0), (2.5, -15.0)), + RobotLayout('robot1', (3.5, -21.0, pi / 2.0), (3.5, -15.0)), + ), + scan_radius=3.5, + bar_name='warehouse_replan_bar', + bar_center=(-2.5, -18.5), + bar_size=(15.0, 0.5, 1.0), +) + + +SCENARIOS = { + SIMPLE_SCENARIO.name: SIMPLE_SCENARIO, + WAREHOUSE_SCENARIO.name: WAREHOUSE_SCENARIO, +} + + +def get_scenario(name): + """Return a scenario by name.""" + try: + return SCENARIOS[name] + except KeyError as error: + choices = ', '.join(sorted(SCENARIOS)) + raise ValueError( + f'Unknown replanning scenario {name!r}; choose one of: {choices}' + ) from error diff --git a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_region_publisher.py b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_region_publisher.py index 96a0571..58843aa 100644 --- a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_region_publisher.py +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_region_publisher.py @@ -12,6 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. +from math import atan2, cos, sin + import rclpy from rclpy.node import Node from rclpy.qos import ( @@ -32,6 +34,95 @@ from .scan_conversion import scan_regions +def _yaw_from_quaternion(quaternion): + return atan2( + 2.0 * ( + quaternion.w * quaternion.z + + quaternion.x * quaternion.y + ), + 1.0 - 2.0 * ( + quaternion.y * quaternion.y + + quaternion.z * quaternion.z + ), + ) + + +class ObstacleMemory: + """Retain obstacle points across scan updates.""" + + def __init__( + self, + retention_sec=0.0, + resolution=0.1, + self_filter_radius=0.0, + ): + self.retention_sec = float(retention_sec) + self.resolution = float(resolution) + self.self_filter_radius = float(self_filter_radius) + self._points = {} + + @property + def enabled(self): + return self.retention_sec != 0.0 + + def remember(self, obstacle_points, transform, now_sec): + filter_radius_squared = self.self_filter_radius ** 2 + if self.self_filter_radius > 0.0: + obstacle_points = [ + (x, y) + for x, y in obstacle_points + if x * x + y * y > filter_radius_squared + ] + + if not self.enabled: + return list(obstacle_points) + + yaw = _yaw_from_quaternion(transform.rotation) + cosine = cos(yaw) + sine = sin(yaw) + tx = transform.translation.x + ty = transform.translation.y + + if self.retention_sec > 0.0: + oldest = now_sec - self.retention_sec + self._points = { + key: value + for key, value in self._points.items() + if value[2] > oldest + } + + for local_x, local_y in obstacle_points: + global_x = tx + cosine * local_x - sine * local_y + global_y = ty + sine * local_x + cosine * local_y + key = ( + round(global_x / self.resolution), + round(global_y / self.resolution), + ) + self._points[key] = (global_x, global_y, now_sec) + + if self.self_filter_radius > 0.0: + self._points = { + key: value + for key, value in self._points.items() + if ( + (value[0] - tx) ** 2 + + (value[1] - ty) ** 2 + > filter_radius_squared + ) + } + + # Region points are relative to the current robot pose. + remembered = [] + for global_x, global_y, _ in self._points.values(): + dx = global_x - tx + dy = global_y - ty + remembered.append(( + cosine * dx + sine * dy, + -sine * dx + cosine * dy, + )) + return remembered + + def make_scan_patches(clear_polygons, obstacle_points, ttl_sec): """Build clear and obstacle patches for one converted laser scan.""" patches = [] @@ -76,6 +167,15 @@ def __init__(self): self.reset_source = self.declare_parameter( 'reset_source', False ).value + self.obstacle_memory_sec = self.declare_parameter( + 'obstacle_memory_sec', 0.0 + ).value + self.obstacle_memory_resolution = self.declare_parameter( + 'obstacle_memory_resolution', 0.1 + ).value + self.self_filter_radius = self.declare_parameter( + 'self_filter_radius', 0.0 + ).value self.publish_period_sec = self.declare_parameter( 'publish_period_sec', 0.5 ).value @@ -92,8 +192,17 @@ def __init__(self): raise ValueError('publish_period_sec must not be negative') if self.beam_stride < 1: raise ValueError('beam_stride must be at least one') + if self.obstacle_memory_resolution <= 0.0: + raise ValueError('obstacle_memory_resolution must be positive') + if self.self_filter_radius < 0.0: + raise ValueError('self_filter_radius must not be negative') self.source_id = f'{self.robot_name}/scan' + self.obstacle_memory = ObstacleMemory( + self.obstacle_memory_sec, + self.obstacle_memory_resolution, + self.self_filter_radius, + ) self.last_publish_stamp_sec = None self.pending_scan = None self.publish_count = 0 @@ -121,7 +230,9 @@ def __init__(self): self.get_logger().info( f'Converting {self.scan_topic} into region updates for ' f'{self.robot_name} (stride={self.beam_stride}, ' - f'period={self.publish_period_sec:.2f}s)' + f'period={self.publish_period_sec:.2f}s, ' + f'obstacle_memory={self.obstacle_memory_sec:.1f}s, ' + f'self_filter_radius={self.self_filter_radius:.2f}m)' ) def publish_scan(self, scan): @@ -172,6 +283,11 @@ def publish_pending_scan(self): self.max_observation_range, self.beam_stride, ) + obstacle_points = self.obstacle_memory.remember( + obstacle_points, + transform.transform, + self.get_clock().now().nanoseconds / 1e9, + ) update = self.make_update(transform, clear_polygons, obstacle_points) self.region_update_publisher.publish(update) self.last_publish_stamp_sec = stamp_sec diff --git a/map_server/rmf_layered_map_server_demo/rviz/nav2_observations.rviz b/map_server/rmf_layered_map_server_demo/rviz/nav2_observations.rviz index 3babd04..607eae0 100644 --- a/map_server/rmf_layered_map_server_demo/rviz/nav2_observations.rviz +++ b/map_server/rmf_layered_map_server_demo/rviz/nav2_observations.rviz @@ -7,6 +7,9 @@ Panels: - /Global Options1 - /Combined Global Map1 - /Region Contributions1 + - /Scenario Robots, Goals, and Obstacle1 + - /Robot 0 Nav2 Path1 + - /Robot 1 Nav2 Path1 Splitter Ratio: 0.5 Tree Height: 595 - Class: rviz_common/Views @@ -69,6 +72,102 @@ Visualization Manager: Reliability Policy: Reliable Value: /map/region_markers Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Scenario Robots, Goals, and Obstacle + Namespaces: + robot0_goal: true + robot0_pose: true + robot1_goal: true + robot1_pose: true + spawned_obstacle: true + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /replan_scenario/markers + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Robot 0 RMF Plan + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot0/path_markers + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Robot 1 RMF Plan + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot1/path_markers + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 31; 140; 217 + Enabled: true + Head Diameter: 0.02 + Head Length: 0.02 + Length: 0.3 + Line Style: Lines + Line Width: 0.08 + Name: Robot 0 Nav2 Path + Offset: + X: 0 + Y: 0 + Z: 0.08 + Pose Color: 31; 140; 217 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.005 + Shaft Length: 0.02 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot0/inner/plan + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 107; 20 + Enabled: true + Head Diameter: 0.02 + Head Length: 0.02 + Length: 0.3 + Line Style: Lines + Line Width: 0.08 + Name: Robot 1 Nav2 Path + Offset: + X: 0 + Y: 0 + Z: 0.08 + Pose Color: 255; 107; 20 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.005 + Shaft Length: 0.02 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot1/inner/plan + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 diff --git a/map_server/rmf_layered_map_server_demo/setup.py b/map_server/rmf_layered_map_server_demo/setup.py index 39c6bc3..003c2fa 100644 --- a/map_server/rmf_layered_map_server_demo/setup.py +++ b/map_server/rmf_layered_map_server_demo/setup.py @@ -12,8 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -import os from glob import glob +import os from setuptools import find_packages, setup @@ -28,7 +28,9 @@ ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), + (os.path.join('share', package_name, 'maps'), glob('maps/*')), (os.path.join('share', package_name, 'rviz'), glob('rviz/*.rviz')), + (os.path.join('share', package_name, 'worlds'), glob('worlds/*')), ], install_requires=['setuptools'], zip_safe=True, @@ -44,6 +46,8 @@ 'layered_map_demo_publisher = rmf_layered_map_server_demo.demo_publisher:main', 'nav2_goal_publisher = ' 'rmf_layered_map_server_demo.nav2_goal_publisher:main', + 'replan_obstacle_demo = ' + 'rmf_layered_map_server_demo.replan_obstacle_demo:main', 'region_update_visualizer = ' 'rmf_layered_map_server_demo.region_update_visualizer:main', 'scan_region_publisher = ' diff --git a/map_server/rmf_layered_map_server_demo/test/test_replan_obstacle_demo.py b/map_server/rmf_layered_map_server_demo/test/test_replan_obstacle_demo.py new file mode 100644 index 0000000..3456ce9 --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/test/test_replan_obstacle_demo.py @@ -0,0 +1,195 @@ +# Copyright 2026 Open Source Robotics Foundation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from math import hypot, pi +import subprocess + +import pytest +from rmf_layered_map_msgs.msg import MapRegionPatch, MapRegionUpdate +from rmf_layered_map_server_demo.region_update_visualizer import ( + RegionMarkerState, + SOURCE_COLORS, +) +from rmf_layered_map_server_demo.replan_obstacle_demo import ( + box_sdf, + GOAL_MARKER_Z, + make_navigation_goal, + ROBOT_COLORS, + ROBOT_MARKER_Z, + spawn_succeeded, + star_marker, + triangle_marker, +) +from rmf_layered_map_server_demo.replan_scenarios import ( + get_scenario, + SIMPLE_SCENARIO, + WAREHOUSE_SCENARIO, +) +from rmf_prototype_msgs.msg import Region +from visualization_msgs.msg import Marker + + +def test_navigation_goal_uses_the_map_frame(): + goal = make_navigation_goal(*SIMPLE_SCENARIO.robots[1].goal) + + assert goal.pose.header.frame_id == 'map' + assert goal.pose.pose.position.x == 6.0 + assert goal.pose.pose.position.y == 4.0 + assert goal.pose.pose.orientation.z == pytest.approx( + goal.pose.pose.orientation.w + ) + + +def test_box_sdf(): + sdf = box_sdf( + 'bar', + *SIMPLE_SCENARIO.bar_center, + *SIMPLE_SCENARIO.bar_size, + color=(0.95, 0.05, 0.05, 1.0), + ) + + assert "" in sdf + assert '15.5 0.5 1.0' in sdf + assert '0.95 0.05 0.05 1.0' in sdf + + +@pytest.mark.parametrize( + ('returncode', 'stdout', 'expected'), + ( + (0, 'data: true', True), + (0, 'data: false', False), + (1, 'data: true', False), + ), +) +def test_spawn_succeeded_requires_a_positive_reply( + returncode, + stdout, + expected, +): + result = subprocess.CompletedProcess([], returncode, stdout, '') + + assert spawn_succeeded(result) is expected + + +def _region_update(robot_name): + update = MapRegionUpdate() + update.source.header.stamp.sec = 1 + update.source.header.frame_id = 'map' + update.source.source_id = f'{robot_name}/scan' + update.source.robot_name = robot_name + update.source.map_name = 'single_room' + update.source.robot_pose.orientation.w = 1.0 + update.reset_source = True + patch = MapRegionPatch() + patch.update_type = MapRegionPatch.UPDATE_OBSTACLE + region = Region() + region.hint = Region.HINT_POINT + region.points = [1.0, 2.0] + patch.regions = [region] + update.patches = [patch] + return update + + +def test_region_colors_match_robots_regardless_of_arrival_order(): + state = RegionMarkerState() + + robot1_marker = state.apply_update(_region_update('robot1')).markers[0] + robot0_marker = state.apply_update(_region_update('robot0')).markers[0] + other_marker = state.apply_update(_region_update('other_robot')).markers[0] + + for marker, color in ( + (robot0_marker, ROBOT_COLORS['robot0'][:3]), + (robot1_marker, ROBOT_COLORS['robot1'][:3]), + (other_marker, SOURCE_COLORS[2]), + ): + assert ( + marker.color.r, + marker.color.g, + marker.color.b, + ) == pytest.approx(color) + + +def test_scenario_markers(): + triangle = triangle_marker( + 1, + 'robot', + 5.5, + -4.5, + pi / 2.0, + (1.0, 0.4, 0.0, 0.8), + ) + star = star_marker( + 2, + *SIMPLE_SCENARIO.robots[1].goal, + color=ROBOT_COLORS['robot1'], + ) + + assert triangle.type == Marker.TRIANGLE_LIST + assert len(triangle.points) == 3 + assert triangle.points[0].y > -4.5 + assert all(point.z == ROBOT_MARKER_Z for point in triangle.points) + assert triangle.scale.x == 1.0 + assert triangle.scale.y == 1.0 + assert triangle.scale.z == 1.0 + assert star.type == Marker.TRIANGLE_LIST + assert len(star.points) == 30 + assert all(point.z == GOAL_MARKER_Z for point in star.points) + assert ( + star.color.r, + star.color.g, + star.color.b, + star.color.a, + ) == ROBOT_COLORS['robot1'] + assert star.scale.x == 1.0 + + +def test_simple_room_dead_end_layout(): + robot0, robot1 = SIMPLE_SCENARIO.robots + center_distance = hypot( + robot1.pose[0] - robot0.pose[0], + robot1.pose[1] - robot0.pose[1], + ) + assert center_distance > 2.0 * SIMPLE_SCENARIO.scan_radius + + bar_left = ( + SIMPLE_SCENARIO.bar_center[0] - SIMPLE_SCENARIO.bar_size[0] / 2.0 + ) + bar_right = ( + SIMPLE_SCENARIO.bar_center[0] + SIMPLE_SCENARIO.bar_size[0] / 2.0 + ) + assert bar_left == -6.0 + assert bar_right == 9.5 + + for robot in SIMPLE_SCENARIO.robots: + closest_x = min(max(robot.pose[0], bar_left), bar_right) + distance = hypot( + closest_x - robot.pose[0], + SIMPLE_SCENARIO.bar_center[1] - robot.pose[1], + ) + assert distance < SIMPLE_SCENARIO.scan_radius + + goal0, goal1 = (robot.goal for robot in SIMPLE_SCENARIO.robots) + goal_distance = hypot(goal1[0] - goal0[0], goal1[1] - goal0[1]) + assert goal0 == (0.0, 6.0) + assert goal1 == (6.0, 4.0) + assert goal_distance > 6.0 + assert goal0[1] != goal1[1] + + +def test_warehouse_scenario_layout(): + assert get_scenario('warehouse') is WAREHOUSE_SCENARIO + assert WAREHOUSE_SCENARIO.robots[0].pose[:2] == (-12.0, -21.0) + assert WAREHOUSE_SCENARIO.robots[1].pose[:2] == (3.5, -21.0) + assert WAREHOUSE_SCENARIO.bar_center == (-2.5, -18.5) + assert WAREHOUSE_SCENARIO.bar_size == (15.0, 0.5, 1.0) diff --git a/map_server/rmf_layered_map_server_demo/test/test_scan_region_publisher.py b/map_server/rmf_layered_map_server_demo/test/test_scan_region_publisher.py index b6394d0..81aac18 100644 --- a/map_server/rmf_layered_map_server_demo/test/test_scan_region_publisher.py +++ b/map_server/rmf_layered_map_server_demo/test/test_scan_region_publisher.py @@ -12,11 +12,23 @@ # See the License for the specific language governing permissions and # limitations under the License. +from geometry_msgs.msg import Transform from rmf_layered_map_msgs.msg import MapRegionPatch -from rmf_layered_map_server_demo.scan_region_publisher import make_scan_patches +from rmf_layered_map_server_demo.scan_region_publisher import ( + make_scan_patches, + ObstacleMemory, +) from rmf_prototype_msgs.msg import Region +def _transform(x=0.0, y=0.0): + transform = Transform() + transform.translation.x = x + transform.translation.y = y + transform.rotation.w = 1.0 + return transform + + def test_scan_patches_clear_rays_before_marking_obstacle_endpoints(): patches = make_scan_patches( [(0.0, 0.0, 1.0, -0.1, 1.0, 0.1)], @@ -32,3 +44,66 @@ def test_scan_patches_clear_rays_before_marking_obstacle_endpoints(): assert patches[0].regions[0].hint == Region.HINT_CONVEX_POLYGON assert patches[1].occupancy_value == 100 assert patches[1].regions[0].hint == Region.HINT_POINT + + +def test_obstacle_memory_filters_points_inside_robot_body(): + memory = ObstacleMemory( + retention_sec=0.0, + self_filter_radius=0.22, + ) + + points = memory.remember( + [(0.1, 0.0), (0.3, 0.0)], + _transform(), + now_sec=1.0, + ) + + assert points == [(0.3, 0.0)] + + +def test_obstacle_memory_prunes_points_the_robot_moves_over(): + memory = ObstacleMemory( + retention_sec=-1.0, + self_filter_radius=0.22, + ) + assert memory.remember( + [(1.0, 0.0)], + _transform(), + now_sec=1.0, + ) == [(1.0, 0.0)] + + assert memory.remember( + [], + _transform(x=1.0), + now_sec=2.0, + ) == [] + + +def test_obstacle_memory_keeps_points_when_the_scan_window_moves(): + memory = ObstacleMemory(retention_sec=-1.0, resolution=0.1) + first = memory.remember( + [(1.0, 0.0), (2.0, 0.0)], + _transform(), + now_sec=1.0, + ) + moved = memory.remember( + [(1.0, 0.0)], + _transform(x=5.0), + now_sec=2.0, + ) + + assert sorted(first) == [(1.0, 0.0), (2.0, 0.0)] + assert sorted(moved) == [(-4.0, 0.0), (-3.0, 0.0), (1.0, 0.0)] + + +def test_finite_obstacle_memory_expires_old_scan_windows(): + memory = ObstacleMemory(retention_sec=5.0, resolution=0.1) + memory.remember([(1.0, 0.0)], _transform(), now_sec=1.0) + + remembered = memory.remember( + [(2.0, 0.0)], + _transform(), + now_sec=7.0, + ) + + assert remembered == [(2.0, 0.0)] diff --git a/map_server/rmf_layered_map_server_demo/worlds/single_room.sdf b/map_server/rmf_layered_map_server_demo/worlds/single_room.sdf new file mode 100644 index 0000000..52664ef --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/worlds/single_room.sdf @@ -0,0 +1,74 @@ + + + + + 0.003 + 1000.0 + 1.0 + + + + + + + ogre2 + + + + + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + -0.5 0.1 -0.9 + + + + true + + + 0 0 1100 100 + + + 0 0 1100 100 + 0.8 0.8 0.8 10.8 0.8 0.8 1 + + + + + + true + -9.75 0 0.5 0 0 0 + + 0.5 16 1 + 0.5 16 1 + + + + true + 9.75 0 0.5 0 0 0 + + 0.5 16 1 + + 0.5 16 1 + 0.05 0.35 0.8 10.05 0.35 0.8 1 + + + + + true + 0 -7.75 0.5 0 0 0 + + 20 0.5 1 + 20 0.5 1 + + + + true + 0 7.75 0.5 0 0 0 + + 20 0.5 1 + 20 0.5 1 + + + + diff --git a/nav2_integration/README.md b/nav2_integration/README.md index 4285d29..54d7ef8 100644 --- a/nav2_integration/README.md +++ b/nav2_integration/README.md @@ -51,10 +51,11 @@ graph TD - **`async_request_new_goal`**: An asynchronous service that sends a new `NavigateToPose` goal to Nav2. - **`update_goal_client`**: Updates the `InnerNavigationClient` component with the new goal handle. - **`async_monitor_ongoing_navigation`**: Monitors the progress of the navigation goal, handling feedback and final results (Succeeded, Aborted, Cancelled). -- **`process_navigation_result`**: Processes the final result of the navigation request. If aborted, it prepares to retry by requesting a new goal; otherwise, it passes the result for cleanup. +- **`process_navigation_result`**: Publishes `CODE_PATH_BLOCKED` for an aborted current goal and ignores results from superseded goals. - **`cleanup_goal_client`**: Cleans up the goal client state in the component upon completion or failure. - **`log_inner_navigation_error`**: Logs any errors encountered during goal cancellation or request. +An unchanged incremental target is resent when it belongs to a new plan, while cancellation or abort results from an older goal cannot trigger another replan. ## NavigationServices Workflow Diagram @@ -203,4 +204,4 @@ ros2 action send_goal robot1/navigate_to_pose nav2_msgs/action/NavigateToPose "{ } } }" -``` \ No newline at end of file +``` diff --git a/nav2_integration/demo_world/demo_world/staged_nav2_init.py b/nav2_integration/demo_world/demo_world/staged_nav2_init.py new file mode 100644 index 0000000..e230d35 --- /dev/null +++ b/nav2_integration/demo_world/demo_world/staged_nav2_init.py @@ -0,0 +1,216 @@ +"""Start a simulated robot's Nav2 stack in readiness order.""" + +from math import cos, sin +import sys + +from lifecycle_msgs.msg import State +from lifecycle_msgs.srv import GetState + +from nav2_msgs.srv import ManageLifecycleNodes, SetInitialPose + +from nav_msgs.msg import Odometry + +import rclpy +from rclpy.executors import SingleThreadedExecutor +from rclpy.node import Node +from rclpy.task import Future +from rclpy.time import Time + +from tf2_ros import Buffer, TransformListener + + +class StagedNav2Init(Node): + """Activate Nav2 after the simulated robot and transforms are ready.""" + + def __init__(self, x, y, yaw): + """Initialize the coordinator.""" + super().__init__('staged_nav2_init') + self.x = x + self.y = y + self.yaw = yaw + self.done = Future() + self.stage = 'waiting_for_robot' + self.pending = False + self.odom_received = False + + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + self.create_subscription(Odometry, 'odom', self.receive_odometry, 10) + self.localization_manager = self.create_client( + ManageLifecycleNodes, + 'lifecycle_manager_localization/manage_nodes', + ) + self.navigation_manager = self.create_client( + ManageLifecycleNodes, + 'lifecycle_manager_navigation/manage_nodes', + ) + self.amcl_state = self.create_client(GetState, 'amcl/get_state') + self.bt_state = self.create_client(GetState, 'bt_navigator/get_state') + self.pose_client = self.create_client( + SetInitialPose, + 'set_initial_pose', + ) + self.timer = self.create_timer(0.5, self.tick) + + def receive_odometry(self, _): + """Record that Gazebo is publishing odometry.""" + self.odom_received = True + + def transform_ready(self, target, source): + """Check whether a transform is available.""" + return self.tf_buffer.can_transform(target, source, Time()) + + def start_manager(self, client, next_stage, label): + """Request lifecycle startup.""" + if not client.service_is_ready(): + return + request = ManageLifecycleNodes.Request() + request.command = ManageLifecycleNodes.Request.STARTUP + self.pending = True + future = client.call_async(request) + future.add_done_callback( + lambda result: self.manager_started( + result, + next_stage, + label, + ) + ) + + def manager_started(self, future, next_stage, label): + """Handle a lifecycle startup response.""" + self.pending = False + try: + response = future.result() + except Exception as error: + self.get_logger().warning(f'Cannot start {label}: {error}') + return + if not response.success: + self.get_logger().warning(f'{label} startup failed; retrying') + return + self.stage = next_stage + + def wait_for_active(self, client, next_stage): + """Poll a lifecycle node until it is active.""" + if not client.service_is_ready(): + return + self.pending = True + future = client.call_async(GetState.Request()) + future.add_done_callback( + lambda result: self.state_received(result, next_stage) + ) + + def state_received(self, future, next_stage): + """Handle a lifecycle state response.""" + self.pending = False + try: + state = future.result().current_state + except Exception as error: + self.get_logger().warning(f'Cannot read lifecycle state: {error}') + return + if state.id == State.PRIMARY_STATE_ACTIVE: + self.stage = next_stage + + def send_pose(self): + """Send the robot's initial pose.""" + if not self.pose_client.service_is_ready(): + return + request = SetInitialPose.Request() + request.pose.header.frame_id = 'map' + request.pose.header.stamp = self.get_clock().now().to_msg() + request.pose.pose.pose.position.x = self.x + request.pose.pose.pose.position.y = self.y + request.pose.pose.pose.orientation.z = sin(self.yaw / 2.0) + request.pose.pose.pose.orientation.w = cos(self.yaw / 2.0) + request.pose.pose.covariance = [0.1] * 36 + self.pending = True + future = self.pose_client.call_async(request) + future.add_done_callback(self.pose_sent) + + def pose_sent(self, future): + """Continue after setting the initial pose.""" + self.pending = False + try: + future.result() + except Exception as error: + self.get_logger().warning(f'Cannot set initial pose: {error}') + return + self.stage = 'waiting_for_map_transform' + + def tick(self): + """Advance the startup sequence.""" + if self.pending: + return + + if self.stage == 'waiting_for_robot': + if ( + self.odom_received + and self.transform_ready('odom', 'base_link') + ): + self.get_logger().info( + 'Robot transforms are ready; starting localization' + ) + self.stage = 'starting_localization' + return + + if self.stage == 'starting_localization': + self.start_manager( + self.localization_manager, + 'waiting_for_amcl', + 'localization', + ) + return + + if self.stage == 'waiting_for_amcl': + self.wait_for_active(self.amcl_state, 'setting_pose') + return + + if self.stage == 'setting_pose': + self.send_pose() + return + + if self.stage == 'waiting_for_map_transform': + if self.transform_ready('map', 'base_link'): + self.get_logger().info( + 'Localization is ready; starting navigation' + ) + self.stage = 'starting_navigation' + return + + if self.stage == 'starting_navigation': + self.start_manager( + self.navigation_manager, + 'waiting_for_navigation', + 'navigation', + ) + return + + if self.stage == 'waiting_for_navigation': + self.wait_for_active(self.bt_state, 'ready') + return + + if self.stage == 'ready': + self.get_logger().info('Nav2 startup complete') + self.timer.cancel() + self.done.set_result(True) + + +def main(): + """Run the staged startup coordinator.""" + rclpy.init() + x = float(sys.argv[1]) if len(sys.argv) > 1 else 0.0 + y = float(sys.argv[2]) if len(sys.argv) > 2 else 0.0 + yaw = float(sys.argv[3]) if len(sys.argv) > 3 else 0.0 + node = StagedNav2Init(x, y, yaw) + executor = SingleThreadedExecutor() + executor.add_node(node) + try: + executor.spin_until_future_complete(node.done) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.try_shutdown() + + +if __name__ == '__main__': + main() diff --git a/nav2_integration/demo_world/package.xml b/nav2_integration/demo_world/package.xml index 0c00df3..591cda7 100644 --- a/nav2_integration/demo_world/package.xml +++ b/nav2_integration/demo_world/package.xml @@ -7,6 +7,12 @@ arjoc TODO: License declaration + lifecycle_msgs + nav2_msgs + nav_msgs + rclpy + tf2_ros + ament_copyright ament_flake8 ament_pep257 diff --git a/nav2_integration/demo_world/setup.py b/nav2_integration/demo_world/setup.py index 202f7d0..bfc42c0 100644 --- a/nav2_integration/demo_world/setup.py +++ b/nav2_integration/demo_world/setup.py @@ -21,6 +21,7 @@ entry_points={ 'console_scripts': [ 'set_init = demo_world.localization_init:main', + 'staged_nav2_init = demo_world.staged_nav2_init:main', 'robot_client = demo_world.path_reporter:main', ], }, diff --git a/nav2_integration/rmf_nav2_traffic/src/inner_navigation_client.rs b/nav2_integration/rmf_nav2_traffic/src/inner_navigation_client.rs index d90f0ec..6f798f9 100644 --- a/nav2_integration/rmf_nav2_traffic/src/inner_navigation_client.rs +++ b/nav2_integration/rmf_nav2_traffic/src/inner_navigation_client.rs @@ -1,4 +1,4 @@ -use crate::Nav2Agent; +use crate::{safe_zone::PlanErrorPublisher, Nav2Agent}; use bevy::prelude::*; use bevy_ros2::{RclrsExecutorCommands, RclrsNode, RosActionClient}; use crossflow::{prelude::*, service::Service}; @@ -40,6 +40,7 @@ impl InnerNavigationTarget { pub struct ActiveInnerGoal { pub goal_client: GoalClient, pub safe_zone_id: SafeZoneId, + superseded: bool, } impl ActiveInnerGoal { @@ -47,6 +48,7 @@ impl ActiveInnerGoal { Self { goal_client, safe_zone_id, + superseded: false, } } @@ -61,6 +63,14 @@ impl ActiveInnerGoal { pub fn id(&self) -> &SafeZoneId { &self.safe_zone_id } + + pub fn is_superseded(&self) -> bool { + self.superseded + } + + pub fn mark_superseded(&mut self) { + self.superseded = true; + } } #[derive(Component, Clone)] @@ -377,7 +387,7 @@ fn await_external_cancellation( srv: ContinuousService<(), (), StreamOf>, mut orders: ContinuousQuery<(), (), StreamOf>, mut cancel_requests: EventReader, - inner_nav_clients: Query<&InnerNavigationClient>, + mut inner_nav_clients: Query<&mut InnerNavigationClient>, ) { let Some(mut orders) = orders.get_mut(&srv.key) else { return; @@ -391,14 +401,14 @@ fn await_external_cancellation( "Received external cancellation request for agent {:?}", request.agent.index() ); - let Some(existing_goal) = inner_nav_clients - .get(request.agent) - .ok() - .and_then(|inner_client| inner_client.goal().as_ref()) - else { + let Ok(mut inner_client) = inner_nav_clients.get_mut(request.agent) else { continue; }; - let client = existing_goal.client(); + let Some(existing_goal) = inner_client.goal_mut().as_mut() else { + continue; + }; + existing_goal.mark_superseded(); + let client = existing_goal.client().clone(); orders.for_each(|order| { order.streams().send(CancelInnerNavigation { agent: request.agent, @@ -423,16 +433,15 @@ enum CheckExistingGoalResult { /// workflow can proceed to request a new goal without cancellation. fn check_existing_goal( Blocking { request, .. }: Blocking, - inner_nav_clients: Query<&InnerNavigationClient>, + mut inner_nav_clients: Query<&mut InnerNavigationClient>, ) -> Result { // TODO(@xiyuoh) Create a replan mechanism instead of cancelling goal on every // new request let mut replan_and_cancel = false; - let Some(existing_goal) = inner_nav_clients - .get(request.agent) - .ok() - .and_then(|inner_client| inner_client.goal().as_ref()) - else { + let Ok(mut inner_client) = inner_nav_clients.get_mut(request.agent) else { + return Ok(CheckExistingGoalResult::NoExistingGoal(request)); + }; + let Some(existing_goal) = inner_client.goal_mut().as_mut() else { // No existing goal, proceed to request for new goal return Ok(CheckExistingGoalResult::NoExistingGoal(request)); }; @@ -453,14 +462,15 @@ fn check_existing_goal( replan_and_cancel = true; } - let client = existing_goal.client(); - if replan_and_cancel { + // Do not treat cancellation of this goal as a new blockage. + existing_goal.mark_superseded(); + let client = existing_goal.client().clone(); return Ok(CheckExistingGoalResult::ReplanAndCancel( CancelInnerNavigation { agent: request.agent, new_request: Some(request), - cancel_client: client.clone(), + cancel_client: client, }, )); } @@ -725,6 +735,7 @@ fn process_navigation_result( }: Blocking, mut cancelling_inner: Query<&mut CancellingInnerNavigation>, inner_nav_clients: Query<&InnerNavigationClient>, + plan_error_publishers: Query<&PlanErrorPublisher>, ) -> Result { match result { Ok(_) => return Err(result), @@ -734,26 +745,50 @@ fn process_navigation_result( return Err(result); }; let target = &handle.request; - let target_pose = target.target_pose.clone(); + let active_goal = inner_nav_clients + .get(target.agent) + .ok() + .and_then(|inner_client| inner_client.goal().as_ref()); + + if !should_publish_path_blocked( + &target.safe_zone_id, + active_goal.map(|goal| (goal.id(), goal.is_superseded())), + ) { + info!( + "[{:?}] Ignoring stale Nav2 abort {:?}", + target.agent.index(), + target.safe_zone_id, + ); + return Err(result); + } - if let Ok(inner_nav_client) = inner_nav_clients.get(target.agent) { - if let Some(active_goal) = inner_nav_client.goal() { - if active_goal.id() != &target.safe_zone_id { - debug!( - "[{:?}] Aborted goal is stale, not retrying", + if let Ok(publisher) = plan_error_publishers.get(target.agent) { + let plan_error = ros_env::rmf_prototype_msgs::msg::PlanError { + error: ros_env::rmf_prototype_msgs::msg::Error { + code: ros_env::rmf_prototype_msgs::msg::PlanError::CODE_PATH_BLOCKED, + message: format!( + "Nav2 goal aborted for agent {:?}: path blocked within safe zone", target.agent.index() - ); - return Err(result); - } + ), + parameters: String::new(), + }, + plan_id: handle.request.safe_zone_id.plan_id.clone(), + }; + if let Err(e) = publisher.publisher.publish(plan_error) { + error!( + "Failed to publish PlanError for agent {:?}: {:?}", + target.agent.index(), + e + ); + } else { + warn!( + "[{:?}] Goal aborted by Nav2! Published PlanError CODE_PATH_BLOCKED to ~/plan/error.", + target.agent.index() + ); } } - debug!("[{:?}] Goal aborted. Retrying", target.agent.index()); - return Ok(InnerNavigationRequest { - agent: target.agent, - safe_zone_id: target.safe_zone_id.clone(), - target_pose, - }); + return Err(result); } InnerNavigationErrorKind::GoalCancelledError => { // Only mark cancellation success for external cancellation @@ -774,6 +809,16 @@ fn process_navigation_result( return Err(result); } +fn should_publish_path_blocked( + completed_goal_id: &SafeZoneId, + active_goal: Option<(&SafeZoneId, bool)>, +) -> bool { + matches!( + active_goal, + Some((active_goal_id, false)) if active_goal_id == completed_goal_id + ) +} + /// Clears existing goal clients for this agent after verifying that the /// completed goal matches the currently tracked active goal, to avoid /// accidentally clearing a newly requested goal. @@ -803,9 +848,8 @@ fn cleanup_goal_client( if is_current { inner_nav_client.reset_goal(); } else { - warn!( - "[{:?}] Found an incompatible SafeZoneId {:?} while attempting - to cleanup goal client!", + debug!( + "[{:?}] Ignoring stale Nav2 goal cleanup {:?}", handle.request.agent.index(), handle.request.safe_zone_id, ); @@ -817,3 +861,65 @@ fn cleanup_goal_client( ); } } + +#[cfg(test)] +mod tests { + use super::*; + + fn safe_zone_id(session: u8, plan_version: u64, safe_zone_version: u64) -> SafeZoneId { + let mut id = SafeZoneId::default(); + id.plan_id.destination_session.uuid[0] = session; + id.plan_id.plan_version = plan_version; + id.safe_zone_version = safe_zone_version; + id + } + + #[test] + fn current_abort_publishes_path_blocked() { + let completed = safe_zone_id(1, 3, 7); + + assert!(should_publish_path_blocked( + &completed, + Some((&completed, false)), + )); + } + + #[test] + fn intentionally_superseded_abort_is_ignored() { + let completed = safe_zone_id(1, 3, 7); + + assert!(!should_publish_path_blocked( + &completed, + Some((&completed, true)), + )); + } + + #[test] + fn abort_from_older_plan_is_ignored() { + let completed = safe_zone_id(1, 3, 7); + let active = safe_zone_id(1, 4, 1); + + assert!(!should_publish_path_blocked( + &completed, + Some((&active, false)), + )); + } + + #[test] + fn abort_from_older_safe_zone_is_ignored() { + let completed = safe_zone_id(1, 3, 7); + let active = safe_zone_id(1, 3, 8); + + assert!(!should_publish_path_blocked( + &completed, + Some((&active, false)), + )); + } + + #[test] + fn abort_without_an_active_goal_is_ignored() { + let completed = safe_zone_id(1, 3, 7); + + assert!(!should_publish_path_blocked(&completed, None)); + } +} diff --git a/nav2_integration/rmf_nav2_traffic/src/safe_zone.rs b/nav2_integration/rmf_nav2_traffic/src/safe_zone.rs index f5a8b3d..6dc369a 100644 --- a/nav2_integration/rmf_nav2_traffic/src/safe_zone.rs +++ b/nav2_integration/rmf_nav2_traffic/src/safe_zone.rs @@ -6,7 +6,7 @@ use bevy::prelude::*; use bevy_ros2::{RclrsNode, RosPublisher, RosSubscription}; use ros_env::{ nav2_msgs::msg::Costmap, - rmf_prototype_msgs::msg::{Progress, Region, SafeZone}, + rmf_prototype_msgs::msg::{PlanError, Progress, Region, SafeZone}, }; use std::sync::Arc; @@ -25,6 +25,11 @@ pub struct ProgressPublisher { pub publisher: Arc>, } +#[derive(Component)] +pub struct PlanErrorPublisher { + pub publisher: Arc>, +} + #[derive(Component, Debug, Clone, Default, Deref)] pub struct CurrentSafeZone(pub Option); @@ -41,13 +46,25 @@ impl CurrentSafeZone { self.as_ref().is_some_and(|sz| sz.id == other.id) } + pub fn should_update_target(&self, other: &SafeZone) -> bool { + let Some(current) = self.as_ref() else { + return true; + }; + + // Resend an unchanged target when it belongs to a new plan. + if current.id.plan_id != other.id.plan_id { + return true; + } + + self.distancesq_to_target(other) >= 0.5 + } + pub fn distancesq_to_target(&self, other: &SafeZone) -> f64 { - let Some(safe_zone) = self.0.clone() else { - // TODO(arjoc): Clean up lifetimes + let Some(safe_zone) = self.as_ref() else { return f64::INFINITY; }; - let Some((sx, sy)) = Self::get_point(&safe_zone) else { + let Some((sx, sy)) = Self::get_point(safe_zone) else { return f64::INFINITY; }; @@ -86,7 +103,8 @@ impl Plugin for SafeZoneSubscriptionPlugin { app.add_systems(PreUpdate, update_incremental_target) .add_observer(create_safe_zone_subscriber) .add_observer(create_costmap_publisher) - .add_observer(create_progress_publisher); + .add_observer(create_progress_publisher) + .add_observer(create_plan_error_publisher); } } @@ -145,6 +163,23 @@ fn create_progress_publisher( }); } +fn create_plan_error_publisher( + trigger: Trigger, + mut commands: Commands, + agents: Query<&Nav2Agent>, + node: Res, +) { + let e = trigger.target(); + let Ok(agent_name) = agents.get(e).map(|agent| agent.name.clone()) else { + return; + }; + let topic = agent_name + "/plan/error"; + let publisher = Arc::new(RosPublisher::::new(&node, topic)); + commands.entity(e).insert(PlanErrorPublisher { + publisher: Arc::clone(&publisher), + }); +} + fn update_incremental_target( mut nav_target: EventWriter, mut subscriptions: Query< @@ -198,7 +233,7 @@ fn update_incremental_target( continue; }; - if current_safe_zone.distancesq_to_target(&safe_zone) < 0.5 { + if !current_safe_zone.should_update_target(&safe_zone) { continue; } debug!( @@ -284,3 +319,68 @@ fn next_target(safe_zone: &SafeZone) -> Option<(f32, f32, f32)> { xy.zip(yaw).map(|((x, y), yaw)| (x, y, yaw)) } + +#[cfg(test)] +mod tests { + use super::*; + use ros_env::rmf_prototype_msgs::msg::TargetRegion; + + fn safe_zone( + session: u8, + plan_version: u64, + safe_zone_version: u64, + x: f32, + y: f32, + ) -> SafeZone { + let mut safe_zone = SafeZone::default(); + safe_zone.id.plan_id.destination_session.uuid[0] = session; + safe_zone.id.plan_id.plan_version = plan_version; + safe_zone.id.safe_zone_version = safe_zone_version; + + let mut target = TargetRegion::default(); + target.region.hint = Region::HINT_POINT; + target.region.points = vec![x, y]; + safe_zone.incremental_target.regions.push(target); + safe_zone + } + + #[test] + fn first_target_is_sent() { + let current = CurrentSafeZone::default(); + let next = safe_zone(1, 0, 0, 4.0, 4.0); + + assert!(current.should_update_target(&next)); + } + + #[test] + fn nearby_target_from_same_plan_is_suppressed() { + let current = CurrentSafeZone(Some(safe_zone(1, 3, 7, 4.0, 4.0))); + let next = safe_zone(1, 3, 8, 4.25, 4.0); + + assert!(!current.should_update_target(&next)); + } + + #[test] + fn moved_target_from_same_plan_is_sent() { + let current = CurrentSafeZone(Some(safe_zone(1, 3, 7, 4.0, 4.0))); + let next = safe_zone(1, 3, 8, 5.0, 4.0); + + assert!(current.should_update_target(&next)); + } + + #[test] + fn same_target_from_new_plan_is_sent() { + let current = CurrentSafeZone(Some(safe_zone(1, 3, 7, 4.0, 4.0))); + let next = safe_zone(1, 4, 0, 4.0, 4.0); + + assert!(current.should_update_target(&next)); + } + + #[test] + fn same_target_from_new_destination_session_is_sent() { + let current = CurrentSafeZone(Some(safe_zone(1, 3, 7, 4.0, 4.0))); + let next = safe_zone(2, 3, 0, 4.0, 4.0); + + assert!(current.should_update_target(&next)); + } +} diff --git a/nav2_integration/sp_demo_nav2_bringup/README.md b/nav2_integration/sp_demo_nav2_bringup/README.md index da45591..16294bf 100644 --- a/nav2_integration/sp_demo_nav2_bringup/README.md +++ b/nav2_integration/sp_demo_nav2_bringup/README.md @@ -24,13 +24,12 @@ This is how to launch multi-robot simulation with simple command line. Please se #### Cloned -This allows to bring up multiple robots, cloning a single robot N times at different positions in the map. The parameter are loaded from `nav2_multirobot_params_all.yaml` file by default. -The multiple robots that consists of name and initial pose in YAML format will be set on the command-line. The format for each robot is `robot_name={x: 0.0, y: 0.0, yaw: 0.0, roll: 0.0, pitch: 0.0, yaw: 0.0}`. +This launch file clones one robot at multiple poses and loads `nav2_multirobot_params_all.yaml` by default. Pass each robot as `robot_name={x: 0.0, y: 0.0, z: 0.0, roll: 0.0, pitch: 0.0, yaw: 0.0}`. -Please refer to below examples. +Set `staged_startup:=True` to start robots sequentially. Each stack waits for odometry and transforms before localization, initial-pose, and navigation activation. ```shell -ros2 launch nav2_bringup cloned_multi_tb3_simulation_launch.py robots:="robot1={x: 1.0, y: 1.0, yaw: 1.5707}; robot2={x: 1.0, y: 1.0, yaw: 1.5707}" +ros2 launch sp_demo_nav2_bringup cloned_multi_tb3_simulation_launch.py robots:="robot1={x: 1.0, y: 1.0, yaw: 1.5707}; robot2={x: 1.0, y: 1.0, yaw: 1.5707}" ``` #### Unique diff --git a/nav2_integration/sp_demo_nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py b/nav2_integration/sp_demo_nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py index 0b06c61..f655f04 100644 --- a/nav2_integration/sp_demo_nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py +++ b/nav2_integration/sp_demo_nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py @@ -30,8 +30,8 @@ OpaqueFunction, RegisterEventHandler, ) -from launch.conditions import IfCondition -from launch.event_handlers import OnShutdown +from launch.conditions import IfCondition, UnlessCondition +from launch.event_handlers import OnProcessExit, OnShutdown from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, TextSubstitution from launch_ros.actions import Node @@ -64,6 +64,7 @@ def generate_launch_description(): use_robot_state_pub = LaunchConfiguration('use_robot_state_pub') use_rviz = LaunchConfiguration('use_rviz') use_navigation = LaunchConfiguration('use_navigation') + staged_startup = LaunchConfiguration('staged_startup') log_settings = LaunchConfiguration('log_settings', default='true') # Declare the launch arguments @@ -115,6 +116,12 @@ def generate_launch_description(): description='Whether to enable the Nav2 planning and control stack', ) + declare_staged_startup_cmd = DeclareLaunchArgument( + 'staged_startup', + default_value='False', + description='Start each robot stack after its transforms are ready.', + ) + # Start Gazebo with plugin providing the robot spawning service world_sdf = tempfile.mktemp(prefix='nav2_', suffix='.sdf') world_sdf_xacro = ExecuteProcess( @@ -133,11 +140,14 @@ def generate_launch_description(): # Define commands for launching the navigation instances bringup_cmd_group = [] + staged_groups = [] + staged_init_nodes = [] for robot_index, robot_name in enumerate(robots_list): init_pose = robots_list[robot_name] namespace = robot_name + '/inner' - group = GroupAction( - [ + + def make_group(robot_autostart, condition, ready_node=None): + actions = [ LogInfo( msg=[ 'Launching namespace=', @@ -168,7 +178,7 @@ def generate_launch_description(): 'map': map_yaml_file, 'use_sim_time': 'True', 'params_file': params_file, - 'autostart': autostart, + 'autostart': robot_autostart, 'use_rviz': 'False', 'use_simulator': 'False', 'headless': 'False', @@ -191,9 +201,29 @@ def generate_launch_description(): }.items(), ), ] - ) + if ready_node is not None: + actions.append(ready_node) + return GroupAction(actions, condition=condition) - bringup_cmd_group.append(group) + bringup_cmd_group.append( + make_group(autostart, UnlessCondition(staged_startup)) + ) + staged_init = Node( + package='demo_world', + executable='staged_nav2_init', + arguments=[ + str(init_pose['x']), + str(init_pose['y']), + str(init_pose['yaw']), + ], + namespace=namespace, + output='screen', + remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')], + ) + staged_init_nodes.append(staged_init) + staged_groups.append( + make_group('False', IfCondition(staged_startup), staged_init) + ) set_env_vars_resources = AppendEnvironmentVariable( 'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models')) @@ -215,12 +245,14 @@ def generate_launch_description(): ld.add_action(declare_rviz_config_file_cmd) ld.add_action(declare_use_robot_state_pub_cmd) ld.add_action(declare_use_navigation_cmd) + ld.add_action(declare_staged_startup_cmd) # initial localization node for robot_name in robots_list: init_pose = robots_list[robot_name] namespace = robot_name + '/inner' ld.add_action(Node( + condition=UnlessCondition(staged_startup), package='demo_world', executable='set_init', arguments=[str(init_pose['x']), str(init_pose['y']), str(init_pose['yaw'])], @@ -259,4 +291,20 @@ def generate_launch_description(): for cmd in bringup_cmd_group: ld.add_action(cmd) + for staged_init, next_group in zip( + staged_init_nodes, + staged_groups[1:], + ): + ld.add_action( + RegisterEventHandler( + condition=IfCondition(staged_startup), + event_handler=OnProcessExit( + target_action=staged_init, + on_exit=[next_group], + ), + ) + ) + if staged_groups: + ld.add_action(staged_groups[0]) + return ld diff --git a/path_server/README.md b/path_server/README.md index b1542fe..c1db57f 100644 --- a/path_server/README.md +++ b/path_server/README.md @@ -1,7 +1,8 @@ # Path Server -This folder contains a path server implementation. It works by triggering a replan any time a new destination event comes in. The replan should only include robots that are actively moving. -Currently it uses a grid-world PIBT based planner, but it is designed to work with any MapfPlanner implementation. +This folder contains the path server and plan executor. The path server plans for new destinations and replans active robots that report `PlanError.CODE_PATH_BLOCKED`. + +The plan executor checks each remaining route against updates to `/map`, with a 300 ms debounce and two-second cooldown. The grid-world PiBT planner conservatively downsamples maps finer than `1.0 m`; other `MapfPlanner` implementations can be substituted. ## Path Server Demo Dashboard diff --git a/path_server/rmf_path_server/src/lib.rs b/path_server/rmf_path_server/src/lib.rs index abaab44..eaa73e8 100644 --- a/path_server/rmf_path_server/src/lib.rs +++ b/path_server/rmf_path_server/src/lib.rs @@ -18,7 +18,7 @@ use ros_env::{ nav_msgs::msg::{OccupancyGrid, Odometry}, rmf_prototype_msgs::{ self, - msg::{Destination, Plan, PlanId, TrafficDependency, Waypoint}, + msg::{Destination, Plan, PlanError, PlanId, TrafficDependency, Waypoint}, }, }; use std::{ @@ -131,6 +131,19 @@ impl PlanServer

{ .insert(robot_id.to_string(), msg.clone()); } + pub fn handle_plan_error(&mut self, robot_id: &str, msg: PlanError) { + if msg.error.code == PlanError::CODE_PATH_BLOCKED { + rclrs::log_warn!( + self.node.logger(), + "Received CODE_PATH_BLOCKED for robot {}. Enqueuing replan...", + robot_id + ); + if let Some(dest) = self.active_destinations.get(robot_id).cloned() { + self.replan_queue.push((robot_id.to_string(), dest)); + } + } + } + pub fn replan(&mut self) { // 1. Check if any planning results have arrived from the background thread if let Ok(receiver) = self.plan_receiver.get_mut() { @@ -472,6 +485,7 @@ impl PlanServer

{ pub struct RobotPathConnections { pub _destination_subscription: rclrs::WorkerSubscription>, pub _odom_subscription: rclrs::WorkerSubscription>, + pub _plan_error_subscription: rclrs::WorkerSubscription>, } pub struct DiscoveryServer { @@ -607,11 +621,34 @@ pub fn start_path_server( } }; + let robot_id_clone3 = robot_id.to_string(); + let plan_error_topic = robot_id.to_string() + "/plan/error"; + let plan_error_sub = match server + .destinations_worker + .create_subscription::( + plan_error_topic.as_str(), + move |dest_server: &mut PlanServer

, error_msg: PlanError| { + dest_server.handle_plan_error(&robot_id_clone3, error_msg); + }, + ) { + Ok(sub) => sub, + Err(err) => { + rclrs::log_error!( + server.node.logger(), + "Failed to create plan error subscription on DestinationsWorker for {}: {:?}", + robot_id, + err + ); + return; + } + }; + server.active_robots.insert( robot_id.to_string(), RobotPathConnections { _destination_subscription: destination_sub, _odom_subscription: odom_sub, + _plan_error_subscription: plan_error_sub, }, ); } diff --git a/path_server/rmf_path_server/src/planner.rs b/path_server/rmf_path_server/src/planner.rs index 8b2130a..22323cc 100644 --- a/path_server/rmf_path_server/src/planner.rs +++ b/path_server/rmf_path_server/src/planner.rs @@ -21,6 +21,8 @@ use std::collections::HashMap; use std::sync::atomic::AtomicBool; use std::sync::Arc; +const MIN_PLANNING_RESOLUTION: f32 = 1.0; + #[derive(Clone, Debug, Default)] pub struct Map { pub grid: OccupancyGrid, @@ -79,6 +81,44 @@ impl PibtPlanner { } } +fn planning_grid(grid: &OccupancyGrid) -> (usize, usize, f32, f32, f32, Vec>) { + let source_width = grid.info.width as usize; + let source_height = grid.info.height as usize; + let source_resolution = grid.info.resolution; + let resolution = source_resolution.max(MIN_PLANNING_RESOLUTION); + let width = ((source_width as f32 * source_resolution) / resolution) + .ceil() + .max(1.0) as usize; + let height = ((source_height as f32 * source_resolution) / resolution) + .ceil() + .max(1.0) as usize; + let mut cells = vec![vec![0; height]; width]; + + for source_x in 0..source_width { + for source_y in 0..source_height { + let value = grid + .data + .get(source_y * source_width + source_x) + .copied() + .unwrap_or(-1); + if value > 50 || value == -1 { + let x = ((source_x as f32 * source_resolution) / resolution).floor() as usize; + let y = ((source_y as f32 * source_resolution) / resolution).floor() as usize; + cells[x.min(width - 1)][y.min(height - 1)] = 1; + } + } + } + + ( + width, + height, + resolution, + grid.info.origin.position.x as f32, + grid.info.origin.position.y as f32, + cells, + ) +} + impl MapfPlanner for PibtPlanner { fn plan( &self, @@ -97,20 +137,7 @@ impl MapfPlanner for PibtPlanner { map.grid.info.width > 0 && map.grid.info.height > 0 && map.grid.info.resolution > 0.0; let (width, height, resolution, offset_x, offset_y, grid) = if use_map { - let w = map.grid.info.width as usize; - let h = map.grid.info.height as usize; - let r = map.grid.info.resolution; - let ox = map.grid.info.origin.position.x as f32; - let oy = map.grid.info.origin.position.y as f32; - - let mut g = vec![vec![0; h]; w]; - for x in 0..w { - for y in 0..h { - let ros_val = map.grid.data[y * w + x]; - g[x][y] = if ros_val > 50 || ros_val == -1 { 1 } else { 0 }; - } - } - (w, h, r, ox, oy, g) + planning_grid(&map.grid) } else { let mut min_x = f32::MAX; let mut min_y = f32::MAX; @@ -249,3 +276,45 @@ impl MapfPlanner for PibtPlanner { Ok(trajectories) } } + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn fine_occupancy_cells_are_conservatively_downsampled() { + let mut map = OccupancyGrid::default(); + map.info.resolution = 0.25; + map.info.width = 8; + map.info.height = 4; + map.info.origin.position.x = -1.0; + map.info.origin.position.y = -2.0; + map.data = vec![0; 32]; + map.data[2 * 8 + 5] = 100; + + let (width, height, resolution, offset_x, offset_y, cells) = planning_grid(&map); + + assert_eq!(width, 2); + assert_eq!(height, 1); + assert_eq!(resolution, 1.0); + assert_eq!(offset_x, -1.0); + assert_eq!(offset_y, -2.0); + assert_eq!(cells, vec![vec![0], vec![1]]); + } + + #[test] + fn native_grid_is_kept_when_it_is_already_coarse() { + let mut map = OccupancyGrid::default(); + map.info.resolution = 2.0; + map.info.width = 2; + map.info.height = 2; + map.data = vec![0, 100, 0, 0]; + + let (width, height, resolution, _, _, cells) = planning_grid(&map); + + assert_eq!(width, 2); + assert_eq!(height, 2); + assert_eq!(resolution, 2.0); + assert_eq!(cells, vec![vec![0, 0], vec![1, 0]]); + } +} diff --git a/path_server/rmf_path_server_test/test/test_path_server_error.py b/path_server/rmf_path_server_test/test/test_path_server_error.py new file mode 100644 index 0000000..e9e23f3 --- /dev/null +++ b/path_server/rmf_path_server_test/test/test_path_server_error.py @@ -0,0 +1,231 @@ +# Copyright 2026 Open Source Robotics Foundation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import time +import unittest + +from launch import LaunchDescription +import launch_ros +import launch_testing +import pytest +import rclpy +from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy +from rmf_prototype_msgs.msg import ( + Destination, + DestinationConstraints, + Participant, + ParticipantList, + Plan, + PlanError, + Region, + TargetRegion, +) + + +@pytest.mark.launch_test +def generate_test_description(): + path_server = launch_ros.actions.Node( + package='rmf_path_server', + executable='rmf_path_server', + output='screen' + ) + + robot_1_sim = launch_ros.actions.Node( + package='rmf_mock_robot_sim', + executable='rmf_mock_robot_sim', + output='screen', + parameters=[{ + 'robot_name': 'robot_1', + 'speed': 2.0, + 'update_rate': 20.0, + 'initial_x': 0.0, + 'initial_y': 0.0, + 'initial_yaw': 0.0, + 'publish_discovery': False, + }] + ) + + return LaunchDescription([ + path_server, + robot_1_sim, + launch_testing.actions.ReadyToTest(), + ]), { + 'path_server': path_server, + 'robot_1_sim': robot_1_sim, + } + + +class TestPathServerError(unittest.TestCase): + @classmethod + def setUpClass(cls): + rclpy.init() + + @classmethod + def tearDownClass(cls): + rclpy.shutdown() + + def setUp(self): + self.node = rclpy.create_node('test_path_server_error_node') + + def tearDown(self): + self.node.destroy_node() + + def create_destination(self, session_id, x, y, size): + msg = Destination() + msg.session.uuid = [session_id] * 16 + constraint = DestinationConstraints() + target_region = TargetRegion() + target_region.region.hint = Region.HINT_AXIS_ALIGNED_RECTANGLE + target_region.region.points = [ + float(x), + float(y), + float(x + size), + float(y + size), + ] + constraint.regions.append(target_region) + msg.constraints = constraint + return msg + + def test_plan_error_behavior(self): + received_plans = [] + + reliable_transient_qos = QoSProfile( + depth=10, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + history=HistoryPolicy.KEEP_LAST, + reliability=ReliabilityPolicy.RELIABLE, + ) + + def plan_cb(msg): + received_plans.append(msg) + + self.node.create_subscription( + Plan, + 'robot_1/plan', + plan_cb, + qos_profile=reliable_transient_qos + ) + + dest_pub = self.node.create_publisher( + Destination, + 'robot_1/destination', + qos_profile=reliable_transient_qos + ) + + error_pub = self.node.create_publisher( + PlanError, + 'robot_1/plan/error', + qos_profile=reliable_transient_qos + ) + + discovery_qos = QoSProfile( + depth=1, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + history=HistoryPolicy.KEEP_LAST + ) + + discovery_pub = self.node.create_publisher( + ParticipantList, + '/destination/discovery', + qos_profile=discovery_qos + ) + + discovery_msg = ParticipantList() + p1 = Participant() + p1.name = 'robot_1' + p1.components = [] + discovery_msg.participants.append(p1) + + time.sleep(2.0) + + dest = self.create_destination(1, 5.0, 0.0, 1.0) + + # 1. Publish discovery and destination to receive initial plan + for _ in range(5): + discovery_pub.publish(discovery_msg) + dest_pub.publish(dest) + rclpy.spin_once(self.node, timeout_sec=0.1) + time.sleep(0.1) + + start_time = time.time() + timeout = 10.0 + while time.time() - start_time < timeout: + discovery_pub.publish(discovery_msg) + rclpy.spin_once(self.node, timeout_sec=0.1) + if received_plans: + break + time.sleep(0.1) + + self.assertGreater( + len(received_plans), 0, 'Did not receive initial plan for robot_1' + ) + initial_plan = received_plans[-1] + initial_version = initial_plan.plan_id.plan_version + + # 2. Test publishing an unhandled error code (e.g. CODE_UNRECOGNIZED_ACTION) + # Verify that it does NOT trigger a replan + unhandled_error_msg = PlanError() + unhandled_error_msg.error.code = PlanError.CODE_UNRECOGNIZED_ACTION + unhandled_error_msg.error.message = 'Unrecognized action test' + unhandled_error_msg.plan_id = initial_plan.plan_id + + error_pub.publish(unhandled_error_msg) + spin_start = time.time() + while time.time() - spin_start < 1.0: + discovery_pub.publish(discovery_msg) + rclpy.spin_once(self.node, timeout_sec=0.1) + time.sleep(0.1) + + self.assertEqual( + received_plans[-1].plan_id.plan_version, + initial_version, + 'Unhandled error code should not trigger a replan' + ) + + # 3. Publish CODE_PATH_BLOCKED PlanError to trigger replanning + plan_error_msg = PlanError() + plan_error_msg.error.code = PlanError.CODE_PATH_BLOCKED + plan_error_msg.error.message = 'Path is blocked' + plan_error_msg.plan_id = initial_plan.plan_id + + num_plans_before_error = len(received_plans) + error_pub.publish(plan_error_msg) + + start_time = time.time() + new_plan_received = False + while time.time() - start_time < timeout: + discovery_pub.publish(discovery_msg) + rclpy.spin_once(self.node, timeout_sec=0.1) + if len(received_plans) > num_plans_before_error: + latest_plan = received_plans[-1] + if latest_plan.plan_id.plan_version > initial_version: + new_plan_received = True + break + time.sleep(0.1) + + self.assertTrue( + new_plan_received, + 'Expected new plan with incremented version after publishing CODE_PATH_BLOCKED' + ) + latest_plan = received_plans[-1] + self.assertEqual( + latest_plan.plan_id.plan_version, + initial_version + 1, + f'Plan version should be incremented from {initial_version} to {initial_version + 1}' + ) + self.assertEqual( + list(latest_plan.plan_id.destination_session.uuid), + list(initial_plan.plan_id.destination_session.uuid), + 'Session UUID should remain the same across replans for the same destination session' + ) diff --git a/path_server/rmf_plan_executor/src/lib.rs b/path_server/rmf_plan_executor/src/lib.rs index 601cd5f..d53df13 100644 --- a/path_server/rmf_plan_executor/src/lib.rs +++ b/path_server/rmf_plan_executor/src/lib.rs @@ -22,17 +22,23 @@ use ros_env::builtin_interfaces; use ros_env::geometry_msgs::msg::Pose; use ros_env::nav2_msgs; use ros_env::nav2_msgs::msg::Costmap; -use ros_env::nav_msgs::msg::Odometry; +use ros_env::nav_msgs::msg::{OccupancyGrid, Odometry}; use ros_env::rmf_prototype_msgs; use ros_env::rmf_prototype_msgs::msg::{ - DestinationConstraints, Plan, PlanRelease, SafeZone, SafeZoneId, TargetOrientation, + DestinationConstraints, Plan, PlanError, PlanId, PlanRelease, SafeZone, SafeZoneId, + TargetOrientation, }; use ros_env::std_msgs; use std::{ collections::{BTreeMap, HashMap}, sync::Arc, + time::{Duration, Instant}, }; +const BLOCKAGE_DEBOUNCE: Duration = Duration::from_millis(300); +const REPLAN_COOLDOWN: Duration = Duration::from_secs(2); +const OCCUPIED_THRESHOLD: i8 = 50; + pub struct RobotState { pub radius: f32, pub latest_odom: Option, @@ -40,6 +46,48 @@ pub struct RobotState { pub waypoint_follower: Option, pub safe_zone_version: u64, pub last_incremental_target_wp: Option, + blockage_monitor: BlockageMonitor, +} + +#[derive(Default)] +struct BlockageMonitor { + blocked_since: Option, + reported_plan: Option, + last_reported_at: Option, +} + +impl BlockageMonitor { + fn begin_plan(&mut self) { + self.blocked_since = None; + } + + fn observe(&mut self, blocked: bool, plan_id: &PlanId, now: Instant) -> bool { + if !blocked { + self.blocked_since = None; + return false; + } + + if self.reported_plan.as_ref() == Some(plan_id) { + return false; + } + + let blocked_since = self.blocked_since.get_or_insert(now); + if now.duration_since(*blocked_since) < BLOCKAGE_DEBOUNCE { + return false; + } + + if self + .last_reported_at + .is_some_and(|last| now.duration_since(last) < REPLAN_COOLDOWN) + { + return false; + } + + self.reported_plan = Some(plan_id.clone()); + self.last_reported_at = Some(now); + self.blocked_since = None; + true + } } pub struct PlanExecutor { @@ -50,11 +98,38 @@ pub struct PlanExecutor { pub active_robots: BTreeMap, pub plan_release_publishers: HashMap>, pub safezone_publishers: HashMap>, + pub plan_error_publishers: HashMap>, pub grid: Arc, pub grid_width: u32, pub grid_height: u32, pub grid_resolution: f32, pub grid_origin: Pose, + pub latest_map: Option, +} + +fn target_yaw(plan: &Plan, target_idx: usize) -> f32 { + let Some(target) = plan.waypoints.get(target_idx) else { + return 0.0; + }; + let [target_x, target_y] = target.position; + + for waypoint in plan.waypoints[..target_idx].iter().rev() { + let dx = target_x - waypoint.position[0]; + let dy = target_y - waypoint.position[1]; + if dx.hypot(dy) > 1e-3 { + return dy.atan2(dx); + } + } + + for waypoint in plan.waypoints.iter().skip(target_idx + 1) { + let dx = waypoint.position[0] - target_x; + let dy = waypoint.position[1] - target_y; + if dx.hypot(dy) > 1e-3 { + return dy.atan2(dx); + } + } + + 0.0 } impl PlanExecutor { @@ -66,11 +141,13 @@ impl PlanExecutor { active_robots: BTreeMap::new(), plan_release_publishers: HashMap::new(), safezone_publishers: HashMap::new(), + plan_error_publishers: HashMap::new(), grid: Arc::new(Grid2D::new(vec![vec![0; 20]; 20], 1.0)), grid_width: 20, grid_height: 20, grid_resolution: 1.0, grid_origin: origin, + latest_map: None, } } @@ -91,6 +168,7 @@ impl PlanExecutor { waypoint_follower: None, safe_zone_version: 0, last_incremental_target_wp: None, + blockage_monitor: BlockageMonitor::default(), }, ); self.reindex_followers(); @@ -106,6 +184,7 @@ impl PlanExecutor { ); self.plan_release_publishers.remove(robot_id); self.safezone_publishers.remove(robot_id); + self.plan_error_publishers.remove(robot_id); self.reindex_followers(); } } @@ -164,37 +243,50 @@ impl PlanExecutor { state.plan = Some(msg); state.safe_zone_version = 0; state.last_incremental_target_wp = None; + state.blockage_monitor.begin_plan(); // Reindex because we updated the plan self.reindex_followers(); } + pub fn handle_map(&mut self, msg: OccupancyGrid) { + self.latest_map = Some(msg); + let robot_ids: Vec<_> = self.active_robots.keys().cloned().collect(); + for robot_id in robot_ids { + self.update_route_blockage(&robot_id); + } + } + pub fn handle_odometry(&mut self, robot_id: &str, msg: Odometry) { let current_x = msg.pose.pose.position.x as f32; let current_y = msg.pose.pose.position.y as f32; - let Some(state) = self.active_robots.get_mut(robot_id) else { - return; - }; - - state.latest_odom = Some(msg.clone()); - - if let Some(fw) = &mut state.waypoint_follower { - let before = fw.get_semantic_waypoint().trajectory_index; - let position = Isometry2::new(Vector2::new(current_x, current_y), 0.0); - fw.update_position_estimate(&position, 0.5); - let after = fw.get_semantic_waypoint().trajectory_index; - rclrs::log_debug!( - self.node.logger(), - "[Executor Debug] Robot {} pos=({}, {}) index before={}, after={}", - robot_id, - current_x, - current_y, - before, - after - ); + { + let Some(state) = self.active_robots.get_mut(robot_id) else { + return; + }; + + state.latest_odom = Some(msg.clone()); + + if let Some(fw) = &mut state.waypoint_follower { + let before = fw.get_semantic_waypoint().trajectory_index; + let position = Isometry2::new(Vector2::new(current_x, current_y), 0.0); + fw.update_position_estimate(&position, 0.5); + let after = fw.get_semantic_waypoint().trajectory_index; + rclrs::log_debug!( + self.node.logger(), + "[Executor Debug] Robot {} pos=({}, {}) index before={}, after={}", + robot_id, + current_x, + current_y, + before, + after + ); + } } + self.update_route_blockage(robot_id); + if !self.ready_to_execute() { return; } @@ -412,6 +504,7 @@ impl PlanExecutor { let target_x = plan.waypoints[released_wp_idx].position[0]; let target_y = plan.waypoints[released_wp_idx].position[1]; + let target_yaw = target_yaw(plan, released_wp_idx); let costmap = Self::to_costmap_msg( &positions, @@ -433,16 +526,17 @@ impl PlanExecutor { let safe_zone = SafeZone { incremental_target: DestinationConstraints { regions: vec![rmf_prototype_msgs::msg::TargetRegion { - tolerance: 0.2, + tolerance: 0.1, region: rmf_prototype_msgs::msg::Region { points: vec![target_x, target_y], hint: rmf_prototype_msgs::msg::Region::HINT_POINT, }, orientations: vec![TargetOrientation { - orientation_radians: 0.0, // TODO(@xiyuoh) + // Avoid turning in place at hold points. + orientation_radians: target_yaw, spread_radians: 0.0, tolerance_radians: 0.0, - }], // TODO(@xiyuoh) calculate actual orientation + }], }], nodes: vec![], }, @@ -474,6 +568,82 @@ impl PlanExecutor { } } + fn update_route_blockage(&mut self, robot_id: &str) { + let Some(map) = self.latest_map.as_ref() else { + return; + }; + + let Some(state) = self.active_robots.get_mut(robot_id) else { + return; + }; + let (Some(plan), Some(odom), Some(follower)) = ( + state.plan.as_ref(), + state.latest_odom.as_ref(), + state.waypoint_follower.as_mut(), + ) else { + return; + }; + + let remaining = follower.remaining_trajectory(); + let mut route = Vec::with_capacity(remaining.len() + 1); + route.push(( + odom.pose.pose.position.x as f32, + odom.pose.pose.position.y as f32, + )); + route.extend(remaining); + + let blocked = route_intersects_map(map, &route, state.radius); + let plan_id = plan.plan_id.clone(); + if !state + .blockage_monitor + .observe(blocked, &plan_id, Instant::now()) + { + return; + } + + let publisher = match self.plan_error_publishers.entry(robot_id.to_string()) { + std::collections::hash_map::Entry::Occupied(entry) => entry.into_mut(), + std::collections::hash_map::Entry::Vacant(entry) => { + let topic = format!("{robot_id}/plan/error"); + match self.node.create_publisher(topic.as_str()) { + Ok(publisher) => entry.insert(publisher), + Err(error) => { + rclrs::log_error!( + self.node.logger(), + "Failed to create plan error publisher for {}: {:?}", + robot_id, + error + ); + return; + } + } + } + }; + + let error = PlanError { + error: rmf_prototype_msgs::msg::Error { + code: PlanError::CODE_PATH_BLOCKED, + message: format!("Updated map blocks the remaining route for {robot_id}"), + parameters: String::new(), + }, + plan_id, + }; + if let Err(error) = publisher.publish(error) { + rclrs::log_error!( + self.node.logger(), + "Failed to publish path blockage for {}: {:?}", + robot_id, + error + ); + } else { + rclrs::log_warn!( + self.node.logger(), + "Updated map blocks the remaining route for {}. Requesting a replan.", + robot_id + ); + } + } + fn ready_to_execute(&self) -> bool { if self.active_robots.is_empty() { return false; @@ -527,10 +697,185 @@ impl PlanExecutor { } } +fn route_intersects_map(map: &OccupancyGrid, route: &[(f32, f32)], radius: f32) -> bool { + if route.len() < 2 || map.info.resolution <= 0.0 { + return false; + } + + let width = map.info.width as isize; + let height = map.info.height as isize; + if width == 0 || height == 0 { + return false; + } + + let resolution = map.info.resolution; + let q = &map.info.origin.orientation; + let yaw = (2.0 * (q.w * q.z + q.x * q.y)).atan2(1.0 - 2.0 * (q.y * q.y + q.z * q.z)) as f32; + let cos_yaw = yaw.cos(); + let sin_yaw = yaw.sin(); + let origin_x = map.info.origin.position.x as f32; + let origin_y = map.info.origin.position.y as f32; + let clearance = radius.max(0.0) + resolution * std::f32::consts::FRAC_1_SQRT_2; + let clearance_squared = clearance * clearance; + + let to_map = |(x, y): (f32, f32)| { + let dx = x - origin_x; + let dy = y - origin_y; + (cos_yaw * dx + sin_yaw * dy, -sin_yaw * dx + cos_yaw * dy) + }; + + for segment in route.windows(2) { + let start = to_map(segment[0]); + let end = to_map(segment[1]); + let min_x = (((start.0.min(end.0) - clearance) / resolution).floor() as isize).max(0); + let max_x = + (((start.0.max(end.0) + clearance) / resolution).floor() as isize).min(width - 1); + let min_y = (((start.1.min(end.1) - clearance) / resolution).floor() as isize).max(0); + let max_y = + (((start.1.max(end.1) + clearance) / resolution).floor() as isize).min(height - 1); + + for y in min_y..=max_y { + for x in min_x..=max_x { + let index = y as usize * width as usize + x as usize; + if map.data.get(index).copied().unwrap_or(-1) <= OCCUPIED_THRESHOLD { + continue; + } + + let center = ((x as f32 + 0.5) * resolution, (y as f32 + 0.5) * resolution); + if distance_squared_to_segment(center, start, end) <= clearance_squared { + return true; + } + } + } + } + + false +} + +fn distance_squared_to_segment(point: (f32, f32), start: (f32, f32), end: (f32, f32)) -> f32 { + let segment = (end.0 - start.0, end.1 - start.1); + let length_squared = segment.0 * segment.0 + segment.1 * segment.1; + if length_squared <= f32::EPSILON { + return (point.0 - start.0).powi(2) + (point.1 - start.1).powi(2); + } + + let offset = (point.0 - start.0, point.1 - start.1); + let t = ((offset.0 * segment.0 + offset.1 * segment.1) / length_squared).clamp(0.0, 1.0); + let closest = (start.0 + t * segment.0, start.1 + t * segment.1); + (point.0 - closest.0).powi(2) + (point.1 - closest.1).powi(2) +} + #[cfg(test)] mod tests { + use super::{ + route_intersects_map, target_yaw, BlockageMonitor, BLOCKAGE_DEBOUNCE, REPLAN_COOLDOWN, + }; use mapf_post::na::{Isometry2, Vector2}; use mapf_post::{Trajectory, WaypointFollower}; + use ros_env::{ + nav_msgs::msg::OccupancyGrid, + rmf_prototype_msgs::msg::{Plan, PlanId, Waypoint}, + }; + use std::time::Instant; + + fn plan_with_positions(positions: &[[f32; 2]]) -> Plan { + Plan { + waypoints: positions + .iter() + .map(|position| Waypoint { + position: *position, + ..Default::default() + }) + .collect(), + ..Default::default() + } + } + + #[test] + fn target_yaw_ignores_stationary_wait_waypoints() { + let plan = plan_with_positions(&[[3.0, 4.0], [3.0, 4.0], [3.0, 4.0], [3.0, 5.0]]); + + assert!((target_yaw(&plan, 3) - std::f32::consts::FRAC_PI_2).abs() < 1e-6); + } + + #[test] + fn target_yaw_uses_departure_direction_at_trajectory_start() { + let plan = plan_with_positions(&[[3.0, 4.0], [3.0, 4.0], [2.0, 4.0]]); + + assert!((target_yaw(&plan, 0) - std::f32::consts::PI).abs() < 1e-6); + } + + #[test] + fn stationary_trajectory_has_neutral_yaw() { + let plan = plan_with_positions(&[[3.0, 4.0], [3.0, 4.0]]); + + assert_eq!(target_yaw(&plan, 1), 0.0); + } + + #[test] + fn occupied_cell_on_remaining_route_is_blocked() { + let mut map = OccupancyGrid::default(); + map.info.resolution = 1.0; + map.info.width = 10; + map.info.height = 10; + map.info.origin.orientation.w = 1.0; + map.data = vec![0; 100]; + map.data[5 * 10 + 5] = 100; + + assert!(route_intersects_map(&map, &[(1.5, 5.5), (8.5, 5.5)], 0.25)); + assert!(!route_intersects_map(&map, &[(1.5, 8.5), (8.5, 8.5)], 0.25)); + } + + #[test] + fn blockage_must_persist_before_reporting() { + let mut monitor = BlockageMonitor::default(); + let plan_id = PlanId::default(); + let start = Instant::now(); + + assert!(!monitor.observe(true, &plan_id, start)); + assert!(!monitor.observe(true, &plan_id, start + BLOCKAGE_DEBOUNCE / 2)); + assert!(monitor.observe(true, &plan_id, start + BLOCKAGE_DEBOUNCE)); + let later = start + BLOCKAGE_DEBOUNCE + REPLAN_COOLDOWN; + assert!(!monitor.observe(true, &plan_id, later)); + } + + #[test] + fn clear_route_resets_the_debounce_window() { + let mut monitor = BlockageMonitor::default(); + let plan_id = PlanId::default(); + let start = Instant::now(); + + assert!(!monitor.observe(true, &plan_id, start)); + assert!(!monitor.observe(false, &plan_id, start + BLOCKAGE_DEBOUNCE)); + assert!(!monitor.observe(true, &plan_id, start + BLOCKAGE_DEBOUNCE)); + let before_debounce = start + BLOCKAGE_DEBOUNCE * 3 / 2; + assert!(!monitor.observe(true, &plan_id, before_debounce)); + assert!(monitor.observe(true, &plan_id, start + BLOCKAGE_DEBOUNCE * 2)); + } + + #[test] + fn cooldown_delays_a_blockage_on_the_next_plan() { + let mut monitor = BlockageMonitor::default(); + let first_plan = PlanId::default(); + let second_plan = PlanId { + plan_version: 1, + ..Default::default() + }; + let start = Instant::now(); + + assert!(!monitor.observe(true, &first_plan, start)); + assert!(monitor.observe(true, &first_plan, start + BLOCKAGE_DEBOUNCE)); + monitor.begin_plan(); + let during_cooldown = start + BLOCKAGE_DEBOUNCE * 2; + assert!(!monitor.observe(true, &second_plan, during_cooldown)); + let after_debounce = start + BLOCKAGE_DEBOUNCE * 3; + assert!(!monitor.observe(true, &second_plan, after_debounce)); + assert!(monitor.observe( + true, + &second_plan, + start + BLOCKAGE_DEBOUNCE + REPLAN_COOLDOWN, + )); + } #[test] fn test_robot_2_follower() { diff --git a/path_server/rmf_plan_executor/src/main.rs b/path_server/rmf_plan_executor/src/main.rs index 1bb2c1d..8533b48 100644 --- a/path_server/rmf_plan_executor/src/main.rs +++ b/path_server/rmf_plan_executor/src/main.rs @@ -14,7 +14,7 @@ use rclrs::{Context, CreateBasicExecutor, IntoPrimitiveOptions, SpinOptions}; use rmf_plan_executor::PlanExecutor; -use ros_env::nav_msgs::msg::Odometry; +use ros_env::nav_msgs::msg::{OccupancyGrid, Odometry}; use ros_env::rmf_prototype_msgs::msg::{ParticipantList, Plan}; use std::collections::HashMap; @@ -73,6 +73,13 @@ fn main() -> Result<(), Box> { }, )?; + let _map_subscription = executor_worker.create_subscription::( + "/map".transient_local().reliable(), + move |executor: &mut PlanExecutor, msg: OccupancyGrid| { + executor.handle_map(msg); + }, + )?; + // 2. Subscribe to discovery on the discovery worker to manage odom/plan subscriptions let _discovery_subscription = rmf_participant_discovery::create_discovery_subscription( &discovery_worker,