Adds support for replanning when a progress error is raised

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<InnerNavigationResult>,
     mut cancelling_inner: Query<&mut CancellingInnerNavigation>,
-    inner_nav_clients: Query<&InnerNavigationClient>,
+    _inner_nav_clients: Query<&InnerNavigationClient>,
+    plan_error_publishers: Query<&PlanErrorPublisher>,
 ) -> Result<InnerNavigationRequest, InnerNavigationResult> {
     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

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<RosPublisher<Progress>>,
 }
 
+#[derive(Component)]
+pub struct PlanErrorPublisher {
+    pub publisher: Arc<RosPublisher<PlanError>>,
+}
+
 #[derive(Component, Debug, Clone, Default, Deref)]
 pub struct CurrentSafeZone(pub Option<SafeZone>);
 
@@ -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<OnAdd, Nav2Agent>,
+    mut commands: Commands,
+    agents: Query<&Nav2Agent>,
+    node: Res<RclrsNode>,
+) {
+    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::<PlanError>::new(&node, topic));
+    commands.entity(e).insert(PlanErrorPublisher {
+        publisher: Arc::clone(&publisher),
+    });
+}
+
 fn update_incremental_target(
     mut nav_target: EventWriter<InnerNavigationTarget>,
     mut subscriptions: Query<

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<P: MapfPlanner> PlanServer<P> {
             .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<P: MapfPlanner> PlanServer<P> {
 pub struct RobotPathConnections<P: MapfPlanner> {
     pub _destination_subscription: rclrs::WorkerSubscription<Destination, PlanServer<P>>,
     pub _odom_subscription: rclrs::WorkerSubscription<Odometry, PlanServer<P>>,
+    pub _plan_error_subscription: rclrs::WorkerSubscription<PlanError, PlanServer<P>>,
 }
 
 pub struct DiscoveryServer<P: MapfPlanner> {
@@ -607,11 +621,34 @@ pub fn start_path_server<P: MapfPlanner + 'static>(
                     }
                 };
 
+                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::<PlanError, _>(
+                        plan_error_topic.as_str(),
+                        move |dest_server: &mut PlanServer<P>, 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,
                     },
                 );
             }