From da373961f34b9f4cf324cbbd607bebc40f964416 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 13 Jul 2026 07:23:06 +0000 Subject: [PATCH 1/2] Adds support for replanning when a progress error is raised This PR adds support for replanning when a progress error is raised. We also add similar nav2 support for handling such progress errors. Signed-off-by: Arjo Chakravarty --- .../src/inner_navigation_client.rs | 43 +++++++++++-------- .../rmf_nav2_traffic/src/safe_zone.rs | 27 +++++++++++- path_server/rmf_path_server/src/lib.rs | 39 ++++++++++++++++- 3 files changed, 89 insertions(+), 20 deletions(-) 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..1e4d78d 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}; @@ -724,7 +724,8 @@ fn process_navigation_result( request: result, .. }: Blocking, mut cancelling_inner: Query<&mut CancellingInnerNavigation>, - inner_nav_clients: Query<&InnerNavigationClient>, + _inner_nav_clients: Query<&InnerNavigationClient>, + plan_error_publishers: Query<&PlanErrorPublisher>, ) -> Result { match result { Ok(_) => return Err(result), @@ -734,26 +735,34 @@ fn process_navigation_result( return Err(result); }; let target = &handle.request; - let target_pose = target.target_pose.clone(); - 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 diff --git a/nav2_integration/rmf_nav2_traffic/src/safe_zone.rs b/nav2_integration/rmf_nav2_traffic/src/safe_zone.rs index f5a8b3d..b7daf40 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); @@ -86,7 +91,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 +151,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< 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, }, ); } From 978ef1c17226922da77fa7a9c576f84183f13352 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 15 Jul 2026 04:18:46 +0000 Subject: [PATCH 2/2] Add launch testing Signed-off-by: Arjo Chakravarty --- .../test/test_path_server_error.py | 231 ++++++++++++++++++ 1 file changed, 231 insertions(+) create mode 100644 path_server/rmf_path_server_test/test/test_path_server_error.py 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' + )