From f7ba857e06b30df0a3056dc88ef22273d936ea10 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 24 Aug 2026 05:38:17 +0000 Subject: [PATCH 1/8] Adds support for forwarding nav graph nodes This PR adds support for forwarding navgraph nodes in a destination request. The main use case for this is to support actions such as docking. In our next generation prototype, we plan on having floating "graphs". These graphs will enable us to encode actions and be authored using the site editor. The basic idea is say you want to dock with something and trigger a workflow, you would reserve a region using the reservation system, along with a very specific graph node. When you arrive at this destination, the region is reserved allowing you to perform whatever local activiy you may want your AMR to do. This could be something as simple as offloading an item of a conveyor on to the AMR. The idea is that the region would remain reserved while you perform your task at that specific graph node. In order to distinguish that you want such a task performed we use annotated lanes. The fact that one is going through specific lanes on the graph would then be forwarded to individual robots so robots can use their local execution engines to perform these customized actions. This PR makes sure that (1) the graph nodes are forwarded by the reservation system and (2) the path server populates the relevant fields to ensure that the "fleet adapter" is aware of what action should be performed. Signed-off-by: Arjo Chakravarty --- path_server/rmf_path_server/Cargo.toml | 4 +- path_server/rmf_path_server/src/lib.rs | 143 ++++++++- path_server/rmf_path_server/src/nav_graph.rs | 166 +++++++++++ .../tests/test_map_subscription.rs | 3 +- .../rmf_path_server/tests/test_nav_graph.rs | 272 ++++++++++++++++++ .../conveyor_docking_reservation_config.yaml | 44 +++ .../src/main.rs | 72 ++++- .../src/reservation.rs | 94 ++++++ .../test/test_reservation.py | 90 ++++++ 9 files changed, 874 insertions(+), 14 deletions(-) create mode 100644 path_server/rmf_path_server/src/nav_graph.rs create mode 100644 path_server/rmf_path_server/tests/test_nav_graph.rs create mode 100644 reservation_system/rmf_reservation_destination_server/config/conveyor_docking_reservation_config.yaml diff --git a/path_server/rmf_path_server/Cargo.toml b/path_server/rmf_path_server/Cargo.toml index da7f739..ab75fe1 100644 --- a/path_server/rmf_path_server/Cargo.toml +++ b/path_server/rmf_path_server/Cargo.toml @@ -10,7 +10,9 @@ ros-env = "0.2.0" rmf_participant_discovery = { path = "../../rmf_participant_discovery" } serde = { version = "1.0", features = ["derive"] } +serde_json = "1.0" tokio = { version = "1.0", features = ["full"] } mapf = { git = "https://github.com/open-rmf/mapf" } mapf_post = { git = "https://github.com/arjo129/mapf_post", branch="arjo/feat/more_efficient_checks"} -hetpibt = { git = "https://github.com/arjo129/pibt_rs", branch="scalability-study-updates" } \ No newline at end of file +hetpibt = { git = "https://github.com/arjo129/pibt_rs", branch="scalability-study-updates" } +rmf_site_format = { git = "https://github.com/open-rmf/rmf_site", rev = "2f661c2e917bf0093496e1945d50cf25a3c872da", default-features = false } \ No newline at end of file diff --git a/path_server/rmf_path_server/src/lib.rs b/path_server/rmf_path_server/src/lib.rs index 0022cde..9422640 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, PlanId, Region, TargetRegion, TrafficDependency, Waypoint}, }, }; use std::{ @@ -26,6 +26,9 @@ use std::{ sync::Arc, }; +pub mod nav_graph; +pub use nav_graph::{NavGraphData, NavVertex, VertexAction}; + pub mod planner; pub use planner::{Map, MapfPlanner, MockPlanner, PibtPlanner}; @@ -36,6 +39,7 @@ pub struct PlanSuccess { pub goals: HashMap, pub robot_ids: Vec, pub active_plan: MapfResult, + pub target_actions: HashMap, } pub enum PlanResult { @@ -59,6 +63,8 @@ pub struct PlanServer { pub footprints: Arc>>, pub active_plan_ids: HashMap, pub map: Arc, + pub nav_graph: Option>, + pub target_actions: HashMap, } impl PlanServer

{ @@ -66,6 +72,15 @@ impl PlanServer

{ node: Node, planner: P, footprints: Arc>>, + ) -> Self { + Self::new_with_nav_graph(node, planner, footprints, None) + } + + pub fn new_with_nav_graph( + node: Node, + planner: P, + footprints: Arc>>, + nav_graph: Option>, ) -> Self { let (plan_sender, plan_receiver) = std::sync::mpsc::channel(); Self { @@ -84,10 +99,12 @@ impl PlanServer

{ footprints, active_plan_ids: HashMap::new(), map: Arc::new(Map::default()), + nav_graph, + target_actions: HashMap::new(), } } - pub fn handle_destination(&mut self, robot_id: &str, msg: Destination) { + pub fn handle_destination(&mut self, robot_id: &str, mut msg: Destination) { rclrs::log!( self.node.logger(), "PathServer (DestinationsWorker) received updated destination for {} (session UUID {})", @@ -100,6 +117,53 @@ impl PlanServer

{ .join("") ); + // Check if a graph key target node is specified and matches any vertex in the nav graph + let mut looked_up_action = None; + if let Some(nav_graph) = &self.nav_graph { + for node_target in &msg.constraints.nodes { + if let Some(vertex) = nav_graph.find_vertex(&node_target.key) { + rclrs::log!( + self.node.logger(), + "Matched destination graph key to vertex {} at ({}, {}) for robot {}", + vertex.id, + vertex.position[0], + vertex.position[1], + robot_id + ); + + // If region constraints are empty, resolve coordinates from vertex position + if msg.constraints.regions.is_empty() { + msg.constraints.regions.push(TargetRegion { + region: Region { + points: vec![vertex.position[0], vertex.position[1]], + hint: Region::HINT_POINT, + }, + ..Default::default() + }); + } + + // Check for special arrival action (e.g. docking) + if let Some(action) = &vertex.arrival_action { + rclrs::log!( + self.node.logger(), + "Found special action for robot {} at vertex {}: {:?}", + robot_id, + vertex.id, + action + ); + looked_up_action = Some(action.name.clone()); + } + break; + } + } + } + + if let Some(action) = looked_up_action { + self.target_actions.insert(robot_id.to_string(), action); + } else { + self.target_actions.remove(robot_id); + } + let is_new_session = match self.active_destinations.get(robot_id) { Some(active_dest) => active_dest.session.uuid != msg.session.uuid, None => true, @@ -150,6 +214,7 @@ impl PlanServer

{ traffic_dependencies, goals, robot_ids, + target_actions, .. } = success; @@ -186,6 +251,7 @@ impl PlanServer

{ ); continue; }; + let target_action = target_actions.get(robot_id).map(|s| s.as_str()); let plan = Self::to_plan_msg( agent_idx, traj, @@ -193,6 +259,7 @@ impl PlanServer

{ &traffic_dependencies, &robot_ids, &self.active_plan_ids, + target_action, 1.0, ); plans.insert(robot_id.clone(), plan); @@ -209,8 +276,8 @@ impl PlanServer

{ }) .collect(); wp_strs.push(format!( - " wp {}: pos {:?}, progress {}, blockers: {:?}", - j, wp.position, wp.progress, blockers + " wp {}: pos {:?}, progress {}, action: '{}', blockers: {:?}", + j, wp.position, wp.progress, wp.arrival_action, blockers )); } rclrs::log!( @@ -330,6 +397,7 @@ impl PlanServer

{ let footprints_clone = Arc::clone(&self.footprints); let sender_clone = self.plan_sender.clone(); let map_clone = self.map.clone(); + let target_actions_clone = self.target_actions.clone(); std::thread::spawn(move || { if cancellation.load(std::sync::atomic::Ordering::Relaxed) { @@ -408,6 +476,7 @@ impl PlanServer

{ goals, robot_ids, active_plan: mapf_result, + target_actions: target_actions_clone, })); }); } @@ -419,6 +488,7 @@ impl PlanServer

{ traffic_dependencies: &SemanticPlan, robot_ids: &[String], active_plan_ids: &HashMap, + target_action: Option<&str>, timestep: f32, ) -> Plan { let mut waypoints = Vec::new(); @@ -435,6 +505,12 @@ impl PlanServer

{ }); } + if let Some(action) = target_action { + if let Some(last_wp) = waypoints.last_mut() { + last_wp.arrival_action = action.to_string(); + } + } + for i in 0..traj.len() { if let Some(dep_ids) = traffic_dependencies.comes_before(&SemanticWaypoint { agent: agent_idx, @@ -504,13 +580,67 @@ pub struct PathServerRunning { pub fn start_path_server( node: rclrs::Node, planner: P, +) -> Result, Box> { + let nav_graph = match node + .declare_parameter("site_file") + .default(Arc::from("")) + .mandatory() + { + Ok(param) => { + let path: Arc = param.get(); + if !path.is_empty() { + match NavGraphData::from_site_file(&path) { + Ok(graph) => { + rclrs::log!( + node.logger(), + "Loaded navigation graph from '{}' with {} vertices", + path, + graph.vertices_by_id.len() + ); + Some(Arc::new(graph)) + } + Err(err) => { + rclrs::log_error!( + node.logger(), + "Failed to load site file '{}': {:?}", + path, + err + ); + None + } + } + } else { + None + } + } + Err(err) => { + rclrs::log_warn!( + node.logger(), + "Could not declare optional 'site_file' parameter: {:?}", + err + ); + None + } + }; + + start_path_server_with_nav_graph(node, planner, nav_graph) +} + +pub fn start_path_server_with_nav_graph( + node: rclrs::Node, + planner: P, + nav_graph: Option>, ) -> Result, Box> { let footprints = Arc::new(std::sync::Mutex::new(std::collections::HashMap::new())); let footprints_clone = Arc::clone(&footprints); // Create the Destinations worker - let destinations_worker = - node.create_worker(PlanServer::new(node.clone(), planner, footprints)); + let destinations_worker = node.create_worker(PlanServer::new_with_nav_graph( + node.clone(), + planner, + footprints, + nav_graph, + )); let map_subscription = destinations_worker.create_subscription::( "/map".transient_local().reliable(), @@ -570,7 +700,6 @@ pub fn start_path_server( .create_subscription::( destination_topic.as_str().transient_local().reliable(), move |dest_server: &mut PlanServer

, dest_msg: Destination| { - //rclrs::log!(server.node.logger(), "Received destination for robot"); dest_server.handle_destination(&robot_id_clone, dest_msg); }, ) { diff --git a/path_server/rmf_path_server/src/nav_graph.rs b/path_server/rmf_path_server/src/nav_graph.rs new file mode 100644 index 0000000..fc1eb6d --- /dev/null +++ b/path_server/rmf_path_server/src/nav_graph.rs @@ -0,0 +1,166 @@ +// 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. + +use rmf_site_format::{Category, Site}; +use serde::{Deserialize, Serialize}; +use std::collections::HashMap; + +/// A special action (such as docking) to be executed at a navigation vertex. +#[derive(Clone, Debug, Serialize, Deserialize, PartialEq)] +pub struct VertexAction { + pub action_type: String, + pub name: String, + #[serde(default, skip_serializing_if = "Option::is_none")] + pub duration: Option, +} + +/// Information about a discrete navigation vertex in the site nav graph. +#[derive(Clone, Debug, PartialEq)] +pub struct NavVertex { + pub id: u32, + pub name: Option, + pub position: [f32; 2], + pub arrival_action: Option, +} + +/// In-memory representation of the navigation graph parsed from a .site.json file. +#[derive(Clone, Debug, Default, PartialEq)] +pub struct NavGraphData { + pub vertices_by_id: HashMap, + pub vertices_by_name: HashMap, +} + +impl NavGraphData { + /// Load and parse a navigation graph from a .site.json file path. + pub fn from_site_file(path: &str) -> Result> { + let data = std::fs::read(path)?; + let site = Site::from_bytes_json(&data)?; + Ok(Self::from_site(&site)) + } + + /// Construct a NavGraphData instance from an in-memory rmf_site_format::Site. + pub fn from_site(site: &Site) -> Self { + let mut vertices_by_id = HashMap::new(); + let mut vertices_by_name = HashMap::new(); + + // 1. Extract all anchor positions across all levels + let mut anchor_positions: HashMap = HashMap::new(); + for level in site.levels.values() { + for (&anchor_id, anchor) in &level.anchors { + let [x, y] = anchor.translation_for_category(Category::General); + anchor_positions.insert(anchor_id, [x, y]); + } + } + + // Global site anchors if any + for (&anchor_id, anchor) in &site.anchors { + let [x, y] = anchor.translation_for_category(Category::General); + anchor_positions.entry(anchor_id).or_insert([x, y]); + } + + // 2. Initialize vertices from anchor coordinates + for (&id, &position) in &anchor_positions { + vertices_by_id.insert( + id, + NavVertex { + id, + name: None, + position, + arrival_action: None, + }, + ); + } + + // 3. Attach location names & tags + for location in site.navigation.guided.locations.values() { + let anchor_id = location.anchor.0; + let name = location.name.0.clone(); + if !name.is_empty() { + vertices_by_name.insert(name.clone(), anchor_id); + if let Some(v) = vertices_by_id.get_mut(&anchor_id) { + v.name = Some(name); + } + } + } + + // 4. Extract dock / arrival actions from guided lanes + for lane in site.navigation.guided.lanes.values() { + let [from_anchor, to_anchor] = lane.anchors.array(); + + // Forward motion docking action arrives at `to_anchor` + if let Some(dock) = &lane.forward.dock { + if let Some(v) = vertices_by_id.get_mut(&to_anchor) { + v.arrival_action = Some(VertexAction { + action_type: "dock".to_string(), + name: dock.name.clone(), + duration: dock.duration, + }); + } + } + + // Reverse motion docking action arrives at `from_anchor` + match &lane.reverse { + rmf_site_format::ReverseLane::Different(motion) => { + if let Some(dock) = &motion.dock { + if let Some(v) = vertices_by_id.get_mut(&from_anchor) { + v.arrival_action = Some(VertexAction { + action_type: "dock".to_string(), + name: dock.name.clone(), + duration: dock.duration, + }); + } + } + } + rmf_site_format::ReverseLane::Same => { + if let Some(dock) = &lane.forward.dock { + if let Some(v) = vertices_by_id.get_mut(&from_anchor) { + if v.arrival_action.is_none() { + v.arrival_action = Some(VertexAction { + action_type: "dock".to_string(), + name: dock.name.clone(), + duration: dock.duration, + }); + } + } + } + } + rmf_site_format::ReverseLane::Disable => {} + } + } + + Self { + vertices_by_id, + vertices_by_name, + } + } + + /// Look up a vertex in the nav graph by GraphElementKey (checking vertex id or name). + pub fn find_vertex( + &self, + key: &ros_env::rmf_prototype_msgs::msg::GraphElementKey, + ) -> Option<&NavVertex> { + if let Some(&id) = key.key.first() { + if let Some(v) = self.vertices_by_id.get(&(id as u32)) { + return Some(v); + } + } + if let Some(name_seq) = key.name.first() { + let name_str = name_seq.to_string(); + if let Some(&id) = self.vertices_by_name.get(&name_str) { + return self.vertices_by_id.get(&id); + } + } + None + } +} diff --git a/path_server/rmf_path_server/tests/test_map_subscription.rs b/path_server/rmf_path_server/tests/test_map_subscription.rs index 0781d3f..8d5a9be 100644 --- a/path_server/rmf_path_server/tests/test_map_subscription.rs +++ b/path_server/rmf_path_server/tests/test_map_subscription.rs @@ -72,8 +72,7 @@ fn test_map_subscription() -> Result<(), Box> { let robot_id = "robot1"; let odom_topic = format!("{}/odom", robot_id); - let odom_pub = test_node - .create_publisher::(odom_topic.as_str().reliable())?; + let odom_pub = test_node.create_publisher::(odom_topic.as_str().reliable())?; let dest_topic = format!("{}/destination", robot_id); let dest_pub = test_node .create_publisher::(dest_topic.as_str().transient_local().reliable())?; diff --git a/path_server/rmf_path_server/tests/test_nav_graph.rs b/path_server/rmf_path_server/tests/test_nav_graph.rs new file mode 100644 index 0000000..b406e19 --- /dev/null +++ b/path_server/rmf_path_server/tests/test_nav_graph.rs @@ -0,0 +1,272 @@ +// 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. + +use mapf_post::na::Isometry2; +use rclrs::{Context, CreateBasicExecutor, IntoPrimitiveOptions, SpinOptions}; +use rmf_path_server::{start_path_server_with_nav_graph, Map, MapfPlanner, NavGraphData}; +use ros_env::nav_msgs::msg::Odometry; +use ros_env::rmf_prototype_msgs::msg::{ + Destination, DestinationConstraints, GraphElementKey, Participant, ParticipantList, Plan, + TargetNode, +}; +use std::collections::HashMap; +use std::sync::atomic::AtomicBool; +use std::sync::{Arc, Mutex}; + +const SAMPLE_SITE_JSON: &str = r#"{ + "format_version": "0.1", + "properties": { + "name": "test_site" + }, + "levels": { + "1": { + "properties": { + "name": "L1", + "elevation": 0.0 + }, + "anchors": { + "12": { + "Translate2D": [0.0, 1.5] + }, + "13": { + "Translate2D": [0.0, 2.0] + }, + "17": { + "Translate2D": [2.5, 1.5] + }, + "18": { + "Translate2D": [2.5, 2.0] + } + }, + "floors": {}, + "rankings": { + "floors": [] + } + } + }, + "navigation": { + "guided": { + "graphs": { + "1": { + "name": "default", + "color": [1.0, 0.5, 0.3] + } + }, + "ranking": [1], + "lanes": { + "16": { + "anchors": [13, 12], + "forward": { + "orientation_constraint": "Forwards", + "speed_limit": 0.3, + "dock": { + "name": "dock_conveyor_r1_c1", + "duration": 3.0 + } + }, + "graphs": "All" + } + }, + "locations": { + "14": { + "anchor": 12, + "tags": ["HoldingPoint"], + "name": "conveyor_r1_c1_dock", + "graphs": "All" + }, + "15": { + "anchor": 13, + "tags": [], + "name": "conveyor_r1_c1_staging", + "graphs": "All" + } + } + } + } +}"#; + +#[test] +fn test_nav_graph_parsing() { + let site: rmf_site_format::Site = + serde_json::from_str(SAMPLE_SITE_JSON).expect("failed to parse site json"); + let nav_graph = NavGraphData::from_site(&site); + + assert_eq!(nav_graph.vertices_by_id.len(), 4); + assert_eq!(nav_graph.vertices_by_name.len(), 2); + + // Look up by vertex ID + let v12 = nav_graph + .vertices_by_id + .get(&12) + .expect("vertex 12 missing"); + assert_eq!(v12.position, [0.0, 1.5]); + assert_eq!(v12.name.as_deref(), Some("conveyor_r1_c1_dock")); + + let action = v12 + .arrival_action + .as_ref() + .expect("dock action missing on v12"); + assert_eq!(action.action_type, "dock"); + assert_eq!(action.name, "dock_conveyor_r1_c1"); + assert_eq!(action.duration, Some(3.0)); + + // Look up by GraphElementKey with vertex ID + let mut key_id = GraphElementKey::default(); + key_id.key = vec![12i64].try_into().unwrap(); + let matched_v = nav_graph + .find_vertex(&key_id) + .expect("failed to find by id"); + assert_eq!(matched_v.id, 12); + assert_eq!( + matched_v.arrival_action.as_ref().unwrap().name, + "dock_conveyor_r1_c1" + ); + + // Look up by GraphElementKey with semantic name + let mut key_name = GraphElementKey::default(); + key_name.name = vec!["conveyor_r1_c1_dock".to_string().into()] + .try_into() + .unwrap(); + let matched_v_name = nav_graph + .find_vertex(&key_name) + .expect("failed to find by name"); + assert_eq!(matched_v_name.id, 12); +} + +struct MockPathPlanner; + +impl MapfPlanner for MockPathPlanner { + fn plan( + &self, + starts: &HashMap, + goals: &HashMap, + _footprints: &HashMap>, + robot_ids: &[String], + _map: &Map, + _cancellation: Arc, + ) -> Result>>, Box> { + let mut plans = Vec::new(); + for robot_id in robot_ids { + let start = starts.get(robot_id).unwrap(); + let dest = goals.get(robot_id).unwrap(); + let sx = start.pose.pose.position.x as f32; + let sy = start.pose.pose.position.y as f32; + let region = dest.constraints.regions.first().unwrap(); + let gx = region.region.points[0]; + let gy = region.region.points[1]; + + // 2-waypoint plan from start to goal + plans.push(vec![ + Isometry2::translation(sx, sy), + Isometry2::translation(gx, gy), + ]); + } + Ok(plans) + } +} + +#[test] +fn test_path_server_graphkey_destination_dock_action() -> Result<(), Box> { + let context = Context::default_from_env().unwrap(); + let mut executor = context.create_basic_executor(); + let test_node = Arc::new(executor.create_node("test_graphkey_node")?); + let server_node = Arc::new(executor.create_node("path_server_graphkey")?); + + let site: rmf_site_format::Site = + serde_json::from_str(SAMPLE_SITE_JSON).expect("failed to parse site json"); + let nav_graph = Arc::new(NavGraphData::from_site(&site)); + + let _path_server_guard = start_path_server_with_nav_graph( + Arc::clone(&server_node), + MockPathPlanner, + Some(nav_graph), + )?; + + let received_plan = Arc::new(Mutex::new(None)); + let received_plan_clone = Arc::clone(&received_plan); + + let robot_id = "test_mir_dock_graphkey"; + let plan_sub = test_node.create_subscription::( + format!("{}/plan", robot_id) + .as_str() + .transient_local() + .reliable(), + move |msg: Plan| { + let mut guard = received_plan_clone.lock().unwrap(); + *guard = Some(msg); + }, + )?; + let _ = plan_sub; + + let discovery_pub = test_node.create_publisher::( + "/destination/discovery".transient_local().reliable(), + )?; + let odom_pub = + test_node.create_publisher::(format!("{}/odom", robot_id).as_str().reliable())?; + let dest_pub = test_node.create_publisher::( + format!("{}/destination", robot_id) + .as_str() + .transient_local() + .reliable(), + )?; + + // Publish discovery + let mut discovery_msg = ParticipantList::default(); + discovery_msg.participants.push(Participant { + name: robot_id.to_string(), + components: vec![], + }); + discovery_pub.publish(&discovery_msg)?; + + // Publish odometry at staging area (0.0, 2.0) + let mut odom_msg = Odometry::default(); + odom_msg.pose.pose.position.x = 0.0; + odom_msg.pose.pose.position.y = 2.0; + odom_pub.publish(&odom_msg)?; + + // Publish Destination using GraphElementKey (vertex 12 -> dock_conveyor_r1_c1) + let mut dest_msg = Destination::default(); + let mut key = GraphElementKey::default(); + key.key = vec![12i64].try_into().unwrap(); + + let mut constraints = DestinationConstraints::default(); + constraints.nodes.push(TargetNode { + key, + orientations: vec![], + }); + dest_msg.constraints = constraints; + + dest_pub.publish(&dest_msg)?; + + // Spin until plan is received + let start_time = std::time::Instant::now(); + while start_time.elapsed() < std::time::Duration::from_secs(5) { + let _ = odom_pub.publish(&odom_msg); + executor.spin(SpinOptions::spin_once().timeout(std::time::Duration::from_millis(100))); + if let Ok(guard) = received_plan.lock() { + if let Some(plan) = guard.as_ref() { + assert!(!plan.waypoints.is_empty(), "Plan should have waypoints"); + let last_wp = plan.waypoints.last().unwrap(); + assert_eq!( + last_wp.arrival_action, "dock_conveyor_r1_c1", + "Expected arrival_action 'dock_conveyor_r1_c1' on the final waypoint" + ); + assert_eq!(last_wp.position, [0.0, 1.5]); + return Ok(()); + } + } + } + + panic!("Timed out waiting for generated plan with arrival_action"); +} diff --git a/reservation_system/rmf_reservation_destination_server/config/conveyor_docking_reservation_config.yaml b/reservation_system/rmf_reservation_destination_server/config/conveyor_docking_reservation_config.yaml new file mode 100644 index 0000000..8266341 --- /dev/null +++ b/reservation_system/rmf_reservation_destination_server/config/conveyor_docking_reservation_config.yaml @@ -0,0 +1,44 @@ +# Reservation and docking configuration for factory conveyor simulation. +grid_size: 0.5 + +safe_sets: + - name: factory_floor + region: + hint: axis_aligned_rectangle + points: [-5.0, -5.0, 15.0, 15.0] + +parking_spots: + - name: parking_1 + region: + hint: axis_aligned_rectangle + points: [-2.0, 1.0, -1.0, 2.0] + - name: parking_2 + region: + hint: axis_aligned_rectangle + points: [7.0, 1.0, 8.0, 2.0] + +docking_spots: + - name: conveyor_r1_c1_dock + region: + hint: axis_aligned_rectangle + points: [-0.25, 0.5, 0.25, 1.0] + - name: conveyor_r1_c2_dock + region: + hint: axis_aligned_rectangle + points: [2.25, 0.5, 2.75, 1.0] + - name: conveyor_r1_c3_dock + region: + hint: axis_aligned_rectangle + points: [4.75, 0.5, 5.25, 1.0] + - name: conveyor_r2_c1_dock + region: + hint: axis_aligned_rectangle + points: [-0.25, 2.0, 0.25, 2.5] + - name: conveyor_r2_c2_dock + region: + hint: axis_aligned_rectangle + points: [2.25, 2.0, 2.75, 2.5] + - name: conveyor_r2_c3_dock + region: + hint: axis_aligned_rectangle + points: [4.75, 2.0, 5.25, 2.5] diff --git a/reservation_system/rmf_reservation_destination_server/src/main.rs b/reservation_system/rmf_reservation_destination_server/src/main.rs index 9930b9c..afb5719 100644 --- a/reservation_system/rmf_reservation_destination_server/src/main.rs +++ b/reservation_system/rmf_reservation_destination_server/src/main.rs @@ -23,7 +23,7 @@ use ros_env::rmf_next_gen_reservation_msgs::msg::{ }; use ros_env::rmf_prototype_msgs::msg::{ Destination, DestinationConstraints, DestinationError, DestinationGoal, Error, Region, - TargetRegion, + TargetNode, TargetRegion, }; use ros_env::unique_identifier_msgs::msg::UUID; use std::collections::HashMap; @@ -111,6 +111,7 @@ impl DomainTargetRegion { #[derive(Clone, Debug, PartialEq)] struct DomainDestinationConstraints { pub regions: Vec, + pub nodes: Vec, } impl DomainDestinationConstraints { @@ -121,12 +122,14 @@ impl DomainDestinationConstraints { .iter() .map(DomainTargetRegion::from_ros) .collect(), + nodes: ros.nodes.clone(), } } fn to_ros(&self) -> DestinationConstraints { DestinationConstraints { regions: self.regions.iter().map(|r| r.to_ros()).collect(), + nodes: self.nodes.clone(), ..Default::default() } } @@ -396,9 +399,11 @@ impl DestinationsServer { let goal = DomainDestinationGoal::from_ros(&goal_msg); rclrs::log!( self.node.logger(), - "Received goal for {} (session UUID {})", + "Received goal for {} (session UUID {}), candidates: {}, nodes in first: {:?}", robot_id, - goal.session + goal.session, + goal.one_of.len(), + goal.one_of.first().map(|c| &c.nodes) ); let outcomes = self.state.request(robot_id, goal); self.dispatch(outcomes); @@ -432,7 +437,22 @@ impl DestinationsServer { destination.session ), } - let _ = publishers.goal.publish(destination.to_ros()); + let ros_msg = destination.to_ros(); + rclrs::log!( + self.node.logger(), + "Publishing destination for {}: nodes={:?}, regions={:?}", + agent, + ros_msg.constraints.nodes, + ros_msg.constraints.regions + ); + if let Err(e) = publishers.goal.publish(ros_msg) { + rclrs::log_error!( + self.node.logger(), + "Failed to publish destination for {}: {:?}", + agent, + e + ); + } } Outcome::Error { agent, error } => { let Some(publishers) = self.robot_publishers.get(&agent) else { @@ -764,6 +784,7 @@ mod tests { points: vec![0.0, 0.0, 1.0, 1.0], }, }], + nodes: vec![], }, DomainDestinationConstraints { regions: vec![DomainTargetRegion { @@ -773,6 +794,7 @@ mod tests { points: vec![5.0, 5.0, 6.0, 6.0], }, }], + nodes: vec![], }, ]; @@ -799,4 +821,46 @@ mod tests { assert!(od.first_free_option(&only_second).is_none()); } + + #[test] + fn test_destination_ros_roundtrip() { + let mut ros_goal = DestinationGoal::default(); + ros_goal.session.uuid = [5; 16]; + let mut constraint = DestinationConstraints::default(); + let mut target_node = TargetNode::default(); + target_node.key.key = vec![42].try_into().unwrap(); + target_node.key.name = vec!["dock_spot".to_string().into()].try_into().unwrap(); + constraint.nodes.push(target_node); + ros_goal.one_of.push(constraint); + + let domain_goal = DomainDestinationGoal::from_ros(&ros_goal); + assert_eq!(domain_goal.one_of.len(), 1); + assert_eq!(domain_goal.one_of[0].nodes.len(), 1); + assert_eq!(domain_goal.one_of[0].nodes[0].key.key.as_slice(), &[42]); + assert_eq!( + domain_goal.one_of[0].nodes[0] + .key + .name + .first() + .map(|s| s.to_string()), + Some("dock_spot".to_string()) + ); + + let domain_dest = DomainDestination { + constraints: domain_goal.one_of[0].clone(), + session: domain_goal.session, + detour_for_goal: None, + }; + let ros_dest = domain_dest.to_ros(); + assert_eq!(ros_dest.constraints.nodes.len(), 1); + assert_eq!(ros_dest.constraints.nodes[0].key.key.as_slice(), &[42]); + assert_eq!( + ros_dest.constraints.nodes[0] + .key + .name + .first() + .map(|s| s.to_string()), + Some("dock_spot".to_string()) + ); + } } diff --git a/reservation_system/rmf_reservation_destination_server/src/reservation.rs b/reservation_system/rmf_reservation_destination_server/src/reservation.rs index 92046cf..1787763 100644 --- a/reservation_system/rmf_reservation_destination_server/src/reservation.rs +++ b/reservation_system/rmf_reservation_destination_server/src/reservation.rs @@ -45,6 +45,17 @@ pub(super) enum Outcome { } fn parking_constraints(spot: &ParkingSpot) -> DomainDestinationConstraints { + let mut nodes = Vec::new(); + if !spot.name.is_empty() { + let mut key = ros_env::rmf_prototype_msgs::msg::GraphElementKey::default(); + if let Ok(name_seq) = vec![spot.name.clone().into()].try_into() { + key.name = name_seq; + nodes.push(ros_env::rmf_prototype_msgs::msg::TargetNode { + key, + orientations: vec![], + }); + } + } DomainDestinationConstraints { regions: vec![DomainTargetRegion { tolerance: 0.0, @@ -53,6 +64,7 @@ fn parking_constraints(spot: &ParkingSpot) -> DomainDestinationConstraints { hint: spot.region.hint, }, }], + nodes, } } @@ -337,6 +349,7 @@ mod tests { points, }, }], + nodes: vec![], }], cost_bias: vec![], session: SessionUUID { @@ -606,4 +619,85 @@ mod tests { assert_eq!(r3.session, SessionUUID { uuid: [3; 16] }); assert!(state.queue.is_empty()); } + + #[test] + fn goal_with_graph_key_nodes_preserved() { + let mut state = ReservationState::new(queueing_config()); + + let mut key = ros_env::rmf_prototype_msgs::msg::GraphElementKey::default(); + key.key = vec![42i64].try_into().unwrap(); + key.name = vec!["station_1".to_string().into()].try_into().unwrap(); + + let target_node = ros_env::rmf_prototype_msgs::msg::TargetNode { + key: key.clone(), + orientations: vec![], + }; + + let goal = DomainDestinationGoal { + one_of: vec![DomainDestinationConstraints { + regions: vec![DomainTargetRegion { + tolerance: 0.0, + region: DomainRegion { + hint: DomainRegion::HINT_AXIS_ALIGNED_RECTANGLE, + points: vec![10.0, 10.0, 11.0, 11.0], + }, + }], + nodes: vec![target_node], + }], + cost_bias: vec![], + session: SessionUUID { uuid: [7; 16] }, + }; + + let outcomes = state.request("robot_1", goal); + let dest = reserved_for(&outcomes, "robot_1").expect("robot_1 should get reservation"); + assert_eq!(dest.constraints.nodes.len(), 1); + assert_eq!(dest.constraints.nodes[0].key.key.first(), Some(&42i64)); + assert_eq!( + dest.constraints.nodes[0] + .key + .name + .first() + .map(|s| s.to_string()), + Some("station_1".to_string()) + ); + + // Verify that converting to ROS Destination message retains the GraphElementKey details + let ros_dest = dest.to_ros(); + assert_eq!(ros_dest.constraints.nodes.len(), 1); + assert_eq!(ros_dest.constraints.nodes[0].key.key.first(), Some(&42i64)); + assert_eq!( + ros_dest.constraints.nodes[0] + .key + .name + .first() + .map(|s| s.to_string()), + Some("station_1".to_string()) + ); + } + + #[test] + fn test_rmw_conversion() { + use rclrs::MessageIDL; + let mut key = ros_env::rmf_prototype_msgs::msg::GraphElementKey::default(); + key.key = vec![42i64].try_into().unwrap(); + key.name = vec!["station_alpha".to_string().into()].try_into().unwrap(); + let target_node = ros_env::rmf_prototype_msgs::msg::TargetNode { + key, + orientations: vec![], + }; + let dest = ros_env::rmf_prototype_msgs::msg::Destination { + constraints: ros_env::rmf_prototype_msgs::msg::DestinationConstraints { + regions: vec![], + nodes: vec![target_node], + }, + ..Default::default() + }; + let rmw = ros_env::rmf_prototype_msgs::msg::Destination::into_rmw_message( + std::borrow::Cow::Owned(dest.clone()), + ) + .into_owned(); + assert_eq!(rmw.constraints.nodes.len(), 1); + let back = ros_env::rmf_prototype_msgs::msg::Destination::from_rmw_message(rmw); + assert_eq!(back.constraints.nodes.len(), 1); + } } diff --git a/reservation_system/rmf_reservation_tests/test/test_reservation.py b/reservation_system/rmf_reservation_tests/test/test_reservation.py index c0e2da3..4a2599f 100644 --- a/reservation_system/rmf_reservation_tests/test/test_reservation.py +++ b/reservation_system/rmf_reservation_tests/test/test_reservation.py @@ -25,9 +25,11 @@ Destination, DestinationConstraints, DestinationGoal, + GraphElementKey, Participant, ParticipantList, Region, + TargetNode, TargetRegion, ) @@ -140,3 +142,91 @@ def test_single_reservation(self): len(received_dest) > 0, 'Did not receive Destination message', ) + + def test_reservation_forwards_graph_key(self): + robot_name = 'robot_graph' + received_dest = [] + + reliable_transient_qos = QoSProfile( + depth=10, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + history=HistoryPolicy.KEEP_LAST, + reliability=ReliabilityPolicy.RELIABLE, + ) + + self.node.create_subscription( + Destination, + f'{robot_name}/destination', + lambda msg: received_dest.append(msg), + qos_profile=reliable_transient_qos + ) + + pub = self.node.create_publisher( + DestinationGoal, + f'{robot_name}/destination/goal', + qos_profile=reliable_transient_qos + ) + + discovery_qos = QoSProfile( + depth=1, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + history=HistoryPolicy.KEEP_LAST, + reliability=ReliabilityPolicy.RELIABLE, + ) + discovery_pub = self.node.create_publisher( + ParticipantList, + '/destination/discovery', + qos_profile=discovery_qos + ) + + time.sleep(1.0) + + parts = ParticipantList() + p = Participant() + p.name = robot_name + parts.participants.append(p) + discovery_pub.publish(parts) + + time.sleep(0.5) + + # Create goal with both region and graph element key + goal = DestinationGoal() + import uuid + session_uuid = list(uuid.uuid4().bytes) + goal.session.uuid = session_uuid + + constraint = DestinationConstraints() + target_region = TargetRegion() + target_region.region.hint = Region.HINT_AXIS_ALIGNED_RECTANGLE + target_region.region.points = [5.0, 5.0, 6.0, 6.0] + constraint.regions.append(target_region) + + target_node = TargetNode() + target_node.key = GraphElementKey() + target_node.key.key = [42] + target_node.key.name = ['station_alpha'] + constraint.nodes.append(target_node) + + goal.one_of.append(constraint) + + start_time = time.time() + timeout = 5.0 + while time.time() - start_time < timeout: + discovery_pub.publish(parts) + pub.publish(goal) + for _ in range(5): + rclpy.spin_once(self.node, timeout_sec=0.1) + if any(list(d.session.uuid) == list(goal.session.uuid) for d in received_dest): + break + + matching_dests = [ + d for d in received_dest if list(d.session.uuid) == list(goal.session.uuid) + ] + self.assertTrue( + len(matching_dests) > 0, + 'Did not receive Destination message with matching session UUID', + ) + dest = matching_dests[-1] + self.assertEqual(len(dest.constraints.nodes), 1) + self.assertEqual(list(dest.constraints.nodes[0].key.key), [42]) + self.assertEqual(list(dest.constraints.nodes[0].key.name), ['station_alpha']) From 4c45cacaebfe2b431e3033fe23d1bc4584a05aa8 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 25 Aug 2026 05:54:38 +0000 Subject: [PATCH 2/8] Add a departure trajectory for dock nodes. --- path_server/rmf_path_server/src/lib.rs | 49 ++++++++++++++++++- .../rmf_path_server/tests/test_nav_graph.rs | 10 ++++ 2 files changed, 58 insertions(+), 1 deletion(-) diff --git a/path_server/rmf_path_server/src/lib.rs b/path_server/rmf_path_server/src/lib.rs index 9422640..68470f5 100644 --- a/path_server/rmf_path_server/src/lib.rs +++ b/path_server/rmf_path_server/src/lib.rs @@ -18,7 +18,10 @@ use ros_env::{ nav_msgs::msg::{OccupancyGrid, Odometry}, rmf_prototype_msgs::{ self, - msg::{Destination, Plan, PlanId, Region, TargetRegion, TrafficDependency, Waypoint}, + msg::{ + ControlPoint, Curve, Destination, Plan, PlanId, Region, TargetRegion, + TrafficDependency, Trajectory, Waypoint, + }, }, }; use std::{ @@ -511,6 +514,50 @@ impl PlanServer

{ } } + // For any docking waypoints, populate departure_trajectory + let num_waypoints = waypoints.len(); + for i in 0..num_waypoints { + let arrival_act = waypoints[i].arrival_action.clone(); + let is_docking = !arrival_act.is_empty() + && (arrival_act.starts_with("dock") || arrival_act.contains("dock")); + if is_docking && waypoints[i].departure_trajectory.is_empty() { + let [dock_x, dock_y] = waypoints[i].position; + let depart_pos = if i + 1 < num_waypoints { + waypoints[i + 1].position + } else if i > 0 { + waypoints[i - 1].position + } else { + [dock_x, dock_y] + }; + // TODO(arjoc) parameterize it + let departure_duration = 1.0f32; + let departure_curve = Curve { + degree: 1, + control_points: vec![ + ControlPoint { + position: [dock_x, dock_y], + weight: 1.0, + }, + ControlPoint { + position: depart_pos, + weight: 1.0, + }, + ], + knots: vec![0.0, 0.0, departure_duration, departure_duration], + }; + + let departure_traj = Trajectory { + curve: departure_curve, + initial_progress_level: waypoints[i].progress, + final_progress_level: waypoints[i].progress + departure_duration, + maps: waypoints[i].maps.clone(), + keys: Vec::new(), + }; + + waypoints[i].departure_trajectory = vec![departure_traj]; + } + } + for i in 0..traj.len() { if let Some(dep_ids) = traffic_dependencies.comes_before(&SemanticWaypoint { agent: agent_idx, diff --git a/path_server/rmf_path_server/tests/test_nav_graph.rs b/path_server/rmf_path_server/tests/test_nav_graph.rs index b406e19..78acee2 100644 --- a/path_server/rmf_path_server/tests/test_nav_graph.rs +++ b/path_server/rmf_path_server/tests/test_nav_graph.rs @@ -263,6 +263,16 @@ fn test_path_server_graphkey_destination_dock_action() -> Result<(), Box Date: Wed, 2 Sep 2026 04:58:36 +0000 Subject: [PATCH 3/8] feat: add waypoint collinear simplification in rmf_path_server --- path_server/rmf_path_server/src/lib.rs | 135 ++++++++++++++++++++++++- 1 file changed, 134 insertions(+), 1 deletion(-) diff --git a/path_server/rmf_path_server/src/lib.rs b/path_server/rmf_path_server/src/lib.rs index ca704f9..dae6ed5 100644 --- a/path_server/rmf_path_server/src/lib.rs +++ b/path_server/rmf_path_server/src/lib.rs @@ -596,8 +596,10 @@ impl PlanServer

{ } } + let simplified_waypoints = simplify_waypoints(waypoints); + Plan { - waypoints, + waypoints: simplified_waypoints, start_time: builtin_interfaces::msg::Time { sec: 0, nanosec: 0 }, plan_id, workflow: String::new(), @@ -605,6 +607,56 @@ impl PlanServer

{ } } +/// Simplify a sequence of waypoints by removing redundant collinear points, +/// while strictly preserving any waypoints with traffic blockers, actions, or trajectories. +pub fn simplify_waypoints(waypoints: Vec) -> Vec { + if waypoints.len() <= 2 { + return waypoints; + } + let mut simplified = Vec::with_capacity(waypoints.len()); + simplified.push(waypoints[0].clone()); + + let mut i = 1; + while i < waypoints.len() - 1 { + let prev = simplified.last().unwrap(); + let curr = &waypoints[i]; + let next = &waypoints[i + 1]; + + // Do not prune if current waypoint has special duties + let has_special_duties = !curr.departure_blockers.is_empty() + || !curr.arrival_action.is_empty() + || !curr.departure_action.is_empty() + || !curr.departure_trajectory.is_empty(); + + if has_special_duties { + simplified.push(curr.clone()); + i += 1; + continue; + } + + // Check collinearity between prev -> curr and curr -> next + let dx1 = curr.position[0] - prev.position[0]; + let dy1 = curr.position[1] - prev.position[1]; + let dx2 = next.position[0] - curr.position[0]; + let dy2 = next.position[1] - curr.position[1]; + + let cross_product = dx1 * dy2 - dy1 * dx2; + let dot_product = dx1 * dx2 + dy1 * dy2; + + // If cross product is close to 0 and dot product is positive (same direction) + if cross_product.abs() < 1e-4 && dot_product > 0.0 { + // curr is collinear and redundant, skip it + i += 1; + } else { + simplified.push(curr.clone()); + i += 1; + } + } + + simplified.push(waypoints.last().unwrap().clone()); + simplified +} + pub struct RobotPathConnections { pub _destination_subscription: rclrs::WorkerSubscription>, pub _odom_subscription: rclrs::WorkerSubscription>, @@ -845,3 +897,84 @@ pub fn start_path_server_with_nav_graph( map_subscription, }) } + +#[cfg(test)] +mod tests { + use super::*; + + fn make_wp(pos: [f32; 2], progress: f32) -> Waypoint { + Waypoint { + position: pos, + arrival_constraints: Default::default(), + progress, + maps: Vec::new(), + departure_blockers: Vec::new(), + departure_trajectory: Vec::new(), + departure_action: String::new(), + arrival_action: String::new(), + } + } + + #[test] + fn test_simplify_straight_line() { + let waypoints = vec![ + make_wp([0.0, 0.0], 0.0), + make_wp([0.0, 1.0], 1.0), + make_wp([0.0, 2.0], 2.0), + make_wp([0.0, 3.0], 3.0), + ]; + + let simplified = simplify_waypoints(waypoints); + assert_eq!(simplified.len(), 2); + assert_eq!(simplified[0].position, [0.0, 0.0]); + assert_eq!(simplified[1].position, [0.0, 3.0]); + assert_eq!(simplified[1].progress, 3.0); + } + + #[test] + fn test_simplify_corner_turn() { + let waypoints = vec![ + make_wp([0.0, 0.0], 0.0), + make_wp([0.0, 1.0], 1.0), + make_wp([0.0, 2.0], 2.0), + make_wp([1.0, 2.0], 3.0), + make_wp([2.0, 2.0], 4.0), + ]; + + let simplified = simplify_waypoints(waypoints); + assert_eq!(simplified.len(), 3); + assert_eq!(simplified[0].position, [0.0, 0.0]); + assert_eq!(simplified[1].position, [0.0, 2.0]); + assert_eq!(simplified[2].position, [2.0, 2.0]); + } + + #[test] + fn test_simplify_preserves_blockers_and_actions() { + let mut wp_blocked = make_wp([0.0, 2.0], 2.0); + wp_blocked.departure_blockers.push(TrafficDependency { + name: "other_robot".to_string(), + plan_id: PlanId::default(), + required_progress: 5.0, + }); + + let mut wp_action = make_wp([0.0, 4.0], 4.0); + wp_action.arrival_action = "dock_conveyor".to_string(); + + let waypoints = vec![ + make_wp([0.0, 0.0], 0.0), + make_wp([0.0, 1.0], 1.0), + wp_blocked, + make_wp([0.0, 3.0], 3.0), + wp_action, + ]; + + let simplified = simplify_waypoints(waypoints); + // Should keep: [0.0, 0.0], [0.0, 2.0] (blocked), [0.0, 4.0] (action) + assert_eq!(simplified.len(), 3); + assert_eq!(simplified[0].position, [0.0, 0.0]); + assert_eq!(simplified[1].position, [0.0, 2.0]); + assert_eq!(simplified[1].departure_blockers.len(), 1); + assert_eq!(simplified[2].position, [0.0, 4.0]); + assert_eq!(simplified[2].arrival_action, "dock_conveyor"); + } +} From e21a1af503923c179c394a6683629c72c36833ff Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 3 Sep 2026 06:40:08 +0000 Subject: [PATCH 4/8] feat: Allow an external party to command nav2 agents The nav2 library used traditional nav2 APIs to direct robots. However it would only allow nav2 robots to move if such a request existed. This PR relaxes that requirement and lets robots move directly via destination requests which could potentially come in from other sources. --- .../rmf_nav2_traffic/src/navigation_server.rs | 28 ++++++++++--------- .../rmf_nav2_traffic/src/safe_zone.rs | 28 +++++++------------ 2 files changed, 25 insertions(+), 31 deletions(-) diff --git a/nav2_integration/rmf_nav2_traffic/src/navigation_server.rs b/nav2_integration/rmf_nav2_traffic/src/navigation_server.rs index e416da2..df6e847 100644 --- a/nav2_integration/rmf_nav2_traffic/src/navigation_server.rs +++ b/nav2_integration/rmf_nav2_traffic/src/navigation_server.rs @@ -378,7 +378,7 @@ fn monitor_inner_navigation_clients( mut orders: ContinuousQuery>, mut agents: Query<( &InnerNavigationClient, - &AgentPose, + Option<&AgentPose>, &mut CurrentNavigationRequest, Option<&CancellingInnerNavigation>, )>, @@ -396,21 +396,23 @@ fn monitor_inner_navigation_clients( let request = order.request(); order.streams().send(request.clone()); - if let Ok((client, pose, mut current_nav_request, cancelling_inner)) = + if let Ok((client, maybe_pose, mut current_nav_request, cancelling_inner)) = agents.get_mut(request.agent) { // If reached destination, complete order - if client.active_goal.is_none() && request.destination_reached(pose) { - info!( - "[{:?}] Destination reached, marking NavigationRequest as completed", - request.agent.index() - ); - nav_completed.write(NavigationCompleted { - agent: request.agent, - plan_id: request.plan_id.clone(), - }); - order.respond(()); - return; + if let Some(pose) = maybe_pose { + if client.active_goal.is_none() && request.destination_reached(pose) { + info!( + "[{:?}] Destination reached, marking NavigationRequest as completed", + request.agent.index() + ); + nav_completed.write(NavigationCompleted { + agent: request.agent, + plan_id: request.plan_id.clone(), + }); + order.respond(()); + return; + } } // If inner cancellation is complete, complete order if cancelling_inner.is_some_and(|cancelling| cancelling.success) { diff --git a/nav2_integration/rmf_nav2_traffic/src/safe_zone.rs b/nav2_integration/rmf_nav2_traffic/src/safe_zone.rs index 6dc369a..239fd62 100644 --- a/nav2_integration/rmf_nav2_traffic/src/safe_zone.rs +++ b/nav2_integration/rmf_nav2_traffic/src/safe_zone.rs @@ -1,7 +1,4 @@ -use crate::{ - inner_navigation_client::InnerNavigationTarget, navigation_server::CurrentNavigationRequest, - Nav2Agent, -}; +use crate::{inner_navigation_client::InnerNavigationTarget, Nav2Agent}; use bevy::prelude::*; use bevy_ros2::{RclrsNode, RosPublisher, RosSubscription}; use ros_env::{ @@ -182,19 +179,14 @@ fn create_plan_error_publisher( fn update_incremental_target( mut nav_target: EventWriter, - mut subscriptions: Query< - ( - Entity, - &SafeZoneSubscription, - &CostmapPublisher, - &ProgressPublisher, - &mut CurrentSafeZone, - &Nav2Agent, - ), - // Only respond to a SafeZone message if there is an active NavigationRequest - // for this agent - With, - >, + mut subscriptions: Query<( + Entity, + &SafeZoneSubscription, + &CostmapPublisher, + &ProgressPublisher, + &mut CurrentSafeZone, + &Nav2Agent, + )>, ) { for (e, safe_zone_sub, costmap_pub, progress_pub, mut current_safe_zone, agent) in subscriptions.iter_mut() @@ -278,7 +270,7 @@ fn next_target(safe_zone: &SafeZone) -> Option<(f32, f32, f32)> { // Assume either regions or nodes will be populated, not both. for target_region in constraints.regions.iter() { - let tolerance = target_region.tolerance; + let _tolerance = target_region.tolerance; let region = &target_region.region; let points = ®ion.points; From 7460ba9d5688e3f0b07c2c72064c094622f91c9f Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 3 Sep 2026 07:17:12 +0000 Subject: [PATCH 5/8] Revert "feat: add waypoint collinear simplification in rmf_path_server" --- path_server/rmf_path_server/src/lib.rs | 135 +------------------------ 1 file changed, 1 insertion(+), 134 deletions(-) diff --git a/path_server/rmf_path_server/src/lib.rs b/path_server/rmf_path_server/src/lib.rs index dae6ed5..ca704f9 100644 --- a/path_server/rmf_path_server/src/lib.rs +++ b/path_server/rmf_path_server/src/lib.rs @@ -596,10 +596,8 @@ impl PlanServer

{ } } - let simplified_waypoints = simplify_waypoints(waypoints); - Plan { - waypoints: simplified_waypoints, + waypoints, start_time: builtin_interfaces::msg::Time { sec: 0, nanosec: 0 }, plan_id, workflow: String::new(), @@ -607,56 +605,6 @@ impl PlanServer

{ } } -/// Simplify a sequence of waypoints by removing redundant collinear points, -/// while strictly preserving any waypoints with traffic blockers, actions, or trajectories. -pub fn simplify_waypoints(waypoints: Vec) -> Vec { - if waypoints.len() <= 2 { - return waypoints; - } - let mut simplified = Vec::with_capacity(waypoints.len()); - simplified.push(waypoints[0].clone()); - - let mut i = 1; - while i < waypoints.len() - 1 { - let prev = simplified.last().unwrap(); - let curr = &waypoints[i]; - let next = &waypoints[i + 1]; - - // Do not prune if current waypoint has special duties - let has_special_duties = !curr.departure_blockers.is_empty() - || !curr.arrival_action.is_empty() - || !curr.departure_action.is_empty() - || !curr.departure_trajectory.is_empty(); - - if has_special_duties { - simplified.push(curr.clone()); - i += 1; - continue; - } - - // Check collinearity between prev -> curr and curr -> next - let dx1 = curr.position[0] - prev.position[0]; - let dy1 = curr.position[1] - prev.position[1]; - let dx2 = next.position[0] - curr.position[0]; - let dy2 = next.position[1] - curr.position[1]; - - let cross_product = dx1 * dy2 - dy1 * dx2; - let dot_product = dx1 * dx2 + dy1 * dy2; - - // If cross product is close to 0 and dot product is positive (same direction) - if cross_product.abs() < 1e-4 && dot_product > 0.0 { - // curr is collinear and redundant, skip it - i += 1; - } else { - simplified.push(curr.clone()); - i += 1; - } - } - - simplified.push(waypoints.last().unwrap().clone()); - simplified -} - pub struct RobotPathConnections { pub _destination_subscription: rclrs::WorkerSubscription>, pub _odom_subscription: rclrs::WorkerSubscription>, @@ -897,84 +845,3 @@ pub fn start_path_server_with_nav_graph( map_subscription, }) } - -#[cfg(test)] -mod tests { - use super::*; - - fn make_wp(pos: [f32; 2], progress: f32) -> Waypoint { - Waypoint { - position: pos, - arrival_constraints: Default::default(), - progress, - maps: Vec::new(), - departure_blockers: Vec::new(), - departure_trajectory: Vec::new(), - departure_action: String::new(), - arrival_action: String::new(), - } - } - - #[test] - fn test_simplify_straight_line() { - let waypoints = vec![ - make_wp([0.0, 0.0], 0.0), - make_wp([0.0, 1.0], 1.0), - make_wp([0.0, 2.0], 2.0), - make_wp([0.0, 3.0], 3.0), - ]; - - let simplified = simplify_waypoints(waypoints); - assert_eq!(simplified.len(), 2); - assert_eq!(simplified[0].position, [0.0, 0.0]); - assert_eq!(simplified[1].position, [0.0, 3.0]); - assert_eq!(simplified[1].progress, 3.0); - } - - #[test] - fn test_simplify_corner_turn() { - let waypoints = vec![ - make_wp([0.0, 0.0], 0.0), - make_wp([0.0, 1.0], 1.0), - make_wp([0.0, 2.0], 2.0), - make_wp([1.0, 2.0], 3.0), - make_wp([2.0, 2.0], 4.0), - ]; - - let simplified = simplify_waypoints(waypoints); - assert_eq!(simplified.len(), 3); - assert_eq!(simplified[0].position, [0.0, 0.0]); - assert_eq!(simplified[1].position, [0.0, 2.0]); - assert_eq!(simplified[2].position, [2.0, 2.0]); - } - - #[test] - fn test_simplify_preserves_blockers_and_actions() { - let mut wp_blocked = make_wp([0.0, 2.0], 2.0); - wp_blocked.departure_blockers.push(TrafficDependency { - name: "other_robot".to_string(), - plan_id: PlanId::default(), - required_progress: 5.0, - }); - - let mut wp_action = make_wp([0.0, 4.0], 4.0); - wp_action.arrival_action = "dock_conveyor".to_string(); - - let waypoints = vec![ - make_wp([0.0, 0.0], 0.0), - make_wp([0.0, 1.0], 1.0), - wp_blocked, - make_wp([0.0, 3.0], 3.0), - wp_action, - ]; - - let simplified = simplify_waypoints(waypoints); - // Should keep: [0.0, 0.0], [0.0, 2.0] (blocked), [0.0, 4.0] (action) - assert_eq!(simplified.len(), 3); - assert_eq!(simplified[0].position, [0.0, 0.0]); - assert_eq!(simplified[1].position, [0.0, 2.0]); - assert_eq!(simplified[1].departure_blockers.len(), 1); - assert_eq!(simplified[2].position, [0.0, 4.0]); - assert_eq!(simplified[2].arrival_action, "dock_conveyor"); - } -} From 2d873fcd761381a514cdc29386c55b7380e54fb5 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 3 Sep 2026 07:33:46 +0000 Subject: [PATCH 6/8] fix(rmf_path_server): record active destination on new session to prevent dropping goals on replan cancellation --- path_server/rmf_path_server/src/lib.rs | 2 ++ 1 file changed, 2 insertions(+) diff --git a/path_server/rmf_path_server/src/lib.rs b/path_server/rmf_path_server/src/lib.rs index ca704f9..7cc6a6f 100644 --- a/path_server/rmf_path_server/src/lib.rs +++ b/path_server/rmf_path_server/src/lib.rs @@ -179,6 +179,8 @@ impl PlanServer

{ self.is_planning = false; self.current_planning_session = None; self.current_cancellation = None; + self.active_destinations + .insert(robot_id.to_string(), msg.clone()); self.replan_queue.push((robot_id.to_owned(), msg)); } else { rclrs::log_error!(self.node.logger(), "Duplicate session id received"); From 2b0c5c528e6bc26f6eb4e8d6f5f0c01e9b25f80b Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 8 Sep 2026 07:21:58 +0000 Subject: [PATCH 7/8] feat:Add a subscriber for plan --- .../rmf_nav2_traffic/src/safe_zone.rs | 70 ++++++++++++++++++- 1 file changed, 68 insertions(+), 2 deletions(-) diff --git a/nav2_integration/rmf_nav2_traffic/src/safe_zone.rs b/nav2_integration/rmf_nav2_traffic/src/safe_zone.rs index 239fd62..615659f 100644 --- a/nav2_integration/rmf_nav2_traffic/src/safe_zone.rs +++ b/nav2_integration/rmf_nav2_traffic/src/safe_zone.rs @@ -3,7 +3,7 @@ use bevy::prelude::*; use bevy_ros2::{RclrsNode, RosPublisher, RosSubscription}; use ros_env::{ nav2_msgs::msg::Costmap, - rmf_prototype_msgs::msg::{PlanError, Progress, Region, SafeZone}, + rmf_prototype_msgs::msg::{Plan, PlanError, Progress, Region, SafeZone}, }; use std::sync::Arc; @@ -12,6 +12,24 @@ pub struct SafeZoneSubscription { pub subscriber: Arc>, } +#[derive(Component)] +pub struct PlanSubscription { + pub subscriber: Arc>, +} + +#[derive(Component, Debug, Clone, Default, Deref)] +pub struct CurrentPlan(pub Option); + +impl CurrentPlan { + pub fn update(&mut self, value: Plan) { + self.0 = Some(value); + } + + pub fn clear(&mut self) { + self.0 = None; + } +} + #[derive(Component)] pub struct CostmapPublisher { pub publisher: Arc>, @@ -97,8 +115,9 @@ pub struct SafeZoneSubscriptionPlugin {} impl Plugin for SafeZoneSubscriptionPlugin { fn build(&self, app: &mut App) { - app.add_systems(PreUpdate, update_incremental_target) + app.add_systems(PreUpdate, (update_incremental_target, update_plan)) .add_observer(create_safe_zone_subscriber) + .add_observer(create_plan_subscriber) .add_observer(create_costmap_publisher) .add_observer(create_progress_publisher) .add_observer(create_plan_error_publisher); @@ -125,6 +144,26 @@ fn create_safe_zone_subscriber( )); } +fn create_plan_subscriber( + 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"; + let subscription = Arc::new(RosSubscription::::new(&node, topic.clone())); + commands.entity(e).insert(( + PlanSubscription { + subscriber: Arc::clone(&subscription), + }, + CurrentPlan::default(), + )); +} + fn create_costmap_publisher( trigger: Trigger, mut commands: Commands, @@ -245,6 +284,33 @@ fn update_incremental_target( } } +fn update_plan( + mut subscriptions: Query<( + &PlanSubscription, + &mut CurrentPlan, + &Nav2Agent, + )>, +) { + for (plan_sub, mut current_plan, agent) in subscriptions.iter_mut() { + let Some(plan) = plan_sub.subscriber.data_callback() else { + continue; + }; + let is_new = match current_plan.0.as_ref() { + Some(current) => current.plan_id != plan.plan_id, + None => true, + }; + if is_new { + debug!( + "[{}] Received new plan version {} with {} waypoints", + agent.name, + plan.plan_id.plan_version, + plan.waypoints.len() + ); + *current_plan = CurrentPlan(Some(plan)); + } + } +} + fn is_valid(safe_zone: &SafeZone) -> bool { if safe_zone.target_waypoint.is_empty() { error!("Received a SafeZone message with empty target_waypoint"); From e20b6427e139c0721f458abe65c5186d8fe5e61d Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 9 Sep 2026 02:55:05 +0000 Subject: [PATCH 8/8] add support for specifying a target orientation --- .../rmf_nav2_traffic/src/safe_zone.rs | 306 +++++++++++++++--- path_server/rmf_path_server/src/lib.rs | 13 +- path_server/rmf_path_server/src/planner.rs | 16 + .../rmf_path_server/tests/test_nav_graph.rs | 2 + 4 files changed, 294 insertions(+), 43 deletions(-) diff --git a/nav2_integration/rmf_nav2_traffic/src/safe_zone.rs b/nav2_integration/rmf_nav2_traffic/src/safe_zone.rs index 615659f..8b5b3bb 100644 --- a/nav2_integration/rmf_nav2_traffic/src/safe_zone.rs +++ b/nav2_integration/rmf_nav2_traffic/src/safe_zone.rs @@ -58,11 +58,11 @@ impl CurrentSafeZone { } pub fn matches(&self, other: &SafeZone) -> bool { - self.as_ref().is_some_and(|sz| sz.id == other.id) + self.0.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 { + let Some(current) = self.0.as_ref() else { return true; }; @@ -71,11 +71,42 @@ impl CurrentSafeZone { return true; } + // Resend when targeting a new waypoint in the plan (e.g. dock approach waypoint). + if !other.target_waypoint.is_empty() && current.target_waypoint != other.target_waypoint { + return true; + } + + // Resend if target orientation has significantly changed. + if let (Some(cur_yaw), Some(other_yaw)) = + (Self::get_orientation(current), Self::get_orientation(other)) + { + let diff = (cur_yaw - other_yaw).abs(); + let norm_diff = diff.min(std::f32::consts::TAU - diff); + if norm_diff > 0.05 { + return true; + } + } + self.distancesq_to_target(other) >= 0.5 } + fn get_orientation(safe_zone: &SafeZone) -> Option { + safe_zone + .incremental_target + .regions + .first() + .and_then(|r| r.orientations.first().map(|o| o.orientation_radians)) + .or_else(|| { + safe_zone + .incremental_target + .nodes + .first() + .and_then(|n| n.orientations.first().map(|o| o.orientation_radians)) + }) + } + pub fn distancesq_to_target(&self, other: &SafeZone) -> f64 { - let Some(safe_zone) = self.as_ref() else { + let Some(safe_zone) = self.0.as_ref() else { return f64::INFINITY; }; @@ -105,6 +136,16 @@ impl CurrentSafeZone { )) } } + Region::HINT_AXIS_ALIGNED_RECTANGLE | Region::HINT_RECTANGLE => { + if region.region.points.len() >= 4 { + Some(( + ((region.region.points[0] + region.region.points[2]) * 0.5) as f64, + ((region.region.points[1] + region.region.points[3]) * 0.5) as f64, + )) + } else { + None + } + } _ => None, } } @@ -224,10 +265,11 @@ fn update_incremental_target( &CostmapPublisher, &ProgressPublisher, &mut CurrentSafeZone, + Option<&CurrentPlan>, &Nav2Agent, )>, ) { - for (e, safe_zone_sub, costmap_pub, progress_pub, mut current_safe_zone, agent) in + for (e, safe_zone_sub, costmap_pub, progress_pub, mut current_safe_zone, maybe_plan, agent) in subscriptions.iter_mut() { let Some(safe_zone) = safe_zone_sub.subscriber.data_callback() else { @@ -248,16 +290,22 @@ fn update_incremental_target( continue; } - let Some((target_x, target_y, target_yaw)) = next_target(&safe_zone) else { + let plan_ref = maybe_plan.and_then(|p| p.0.as_ref()); + let Some((target_x, target_y, target_yaw)) = next_target(&safe_zone, plan_ref) else { continue; }; // Publish progress + let target_wp = safe_zone + .target_waypoint + .first() + .copied() + .unwrap_or(safe_zone.last_waypoint); let Ok(_) = progress_pub.publisher.publish(Progress { progress: safe_zone.target_progress, reached_waypoint: safe_zone.last_waypoint, - target_waypoint: safe_zone.target_waypoint[0], // TODO(@xiyuoh) review - reached_keys: vec![], // TODO(@xiyuoh) + target_waypoint: target_wp, + reached_keys: vec![], plan_id: safe_zone.id.plan_id.clone(), }) else { error!("Failed to publish progress for agent [{}]", agent.name); @@ -284,13 +332,7 @@ fn update_incremental_target( } } -fn update_plan( - mut subscriptions: Query<( - &PlanSubscription, - &mut CurrentPlan, - &Nav2Agent, - )>, -) { +fn update_plan(mut subscriptions: Query<(&PlanSubscription, &mut CurrentPlan, &Nav2Agent)>) { for (plan_sub, mut current_plan, agent) in subscriptions.iter_mut() { let Some(plan) = plan_sub.subscriber.data_callback() else { continue; @@ -326,56 +368,107 @@ fn is_valid(safe_zone: &SafeZone) -> bool { true } -fn next_target(safe_zone: &SafeZone) -> Option<(f32, f32, f32)> { - // TODO(@xiyuoh) more sophisticated point selection taking into account - // all factors (region hints, orientations, etc.) - +fn next_target(safe_zone: &SafeZone, plan: Option<&Plan>) -> Option<(f32, f32, f32)> { let constraints = &safe_zone.incremental_target; let mut xy: Option<(f32, f32)> = None; let mut yaw: Option = None; - // Assume either regions or nodes will be populated, not both. + // 1. Extract position and explicit orientations from regions for target_region in constraints.regions.iter() { - let _tolerance = target_region.tolerance; let region = &target_region.region; let points = ®ion.points; match region.hint { Region::HINT_POINT => { - // There should only be exactly 2 elements forming (x, y) - if points.len() != 2 { - continue; + if points.len() == 2 { + xy = Some((points[0], points[1])); } - // TODO(@xiyuoh) - xy = Some((points[0], points[1])); - } - Region::HINT_AXIS_ALIGNED_RECTANGLE => { - // - } - Region::HINT_RECTANGLE => { - // } - Region::HINT_CONVEX_POLYGON => { - // - } - Region::HINT_POLYGON | Region::HINT_UNSPECIFIED => { - // + Region::HINT_AXIS_ALIGNED_RECTANGLE | Region::HINT_RECTANGLE => { + if points.len() >= 4 { + xy = Some(((points[0] + points[2]) * 0.5, (points[1] + points[3]) * 0.5)); + } } _ => { - // + if points.len() >= 2 { + xy = Some((points[0], points[1])); + } } } for target_ori in target_region.orientations.iter() { yaw = Some(target_ori.orientation_radians); - // TODO(@xiyuoh) some processing using spread and tolerance } } - for _target_node in constraints.nodes.iter() { - // TODO(@xiyuoh) + + // 2. Extract explicit orientations from incremental_target.nodes + for target_node in constraints.nodes.iter() { + for target_ori in target_node.orientations.iter() { + yaw = Some(target_ori.orientation_radians); + } } - xy.zip(yaw).map(|((x, y), yaw)| (x, y, yaw)) + // 3. Fallback or override from CurrentPlan for the target waypoint + let target_wp_idx = safe_zone + .target_waypoint + .first() + .copied() + .map(|w| w as usize); + if let (Some(plan), Some(wp_idx)) = (plan, target_wp_idx) { + if let Some(waypoint) = plan.waypoints.get(wp_idx) { + if xy.is_none() { + xy = Some((waypoint.position[0], waypoint.position[1])); + } + + // Check waypoint arrival_constraints.nodes for orientation + if yaw.is_none() { + for node in &waypoint.arrival_constraints.nodes { + if let Some(target_ori) = node.orientations.first() { + yaw = Some(target_ori.orientation_radians); + break; + } + } + } + + // Check waypoint arrival_constraints.regions for orientation + if yaw.is_none() { + for region in &waypoint.arrival_constraints.regions { + if let Some(target_ori) = region.orientations.first() { + yaw = Some(target_ori.orientation_radians); + break; + } + } + } + + // Semantic dock orientation conventions (e.g. conveyor docks) + if yaw.is_none() { + let action = &waypoint.arrival_action; + if action.contains("conveyor_r1") || action.contains("dock_conveyor_r1") { + // Row 1 conveyors (Infeed at Y=0.0): approached from South corridor, face South (-pi/2) + yaw = Some(-std::f32::consts::FRAC_PI_2); + } else if action.contains("conveyor_r2") || action.contains("dock_conveyor_r2") { + // Row 2 conveyors (Outfeed at Y=5.0): approached from North corridor, face North (+pi/2) + yaw = Some(std::f32::consts::FRAC_PI_2); + } + } + + // Trajectory heading fallback: compute angle from previous waypoint + if yaw.is_none() && wp_idx > 0 { + let [curr_x, curr_y] = waypoint.position; + for prev_wp in plan.waypoints[..wp_idx].iter().rev() { + let dx = curr_x - prev_wp.position[0]; + let dy = curr_y - prev_wp.position[1]; + if dx.hypot(dy) > 1e-3 { + yaw = Some(dy.atan2(dx)); + break; + } + } + } + } + } + + let final_yaw = yaw.unwrap_or(0.0); + xy.map(|(x, y)| (x, y, final_yaw)) } #[cfg(test)] @@ -441,4 +534,135 @@ mod tests { assert!(current.should_update_target(&next)); } + + #[test] + fn next_target_uses_region_orientation() { + let mut sz = safe_zone(1, 0, 0, 1.0, 2.0); + let mut target_ori = ros_env::rmf_prototype_msgs::msg::TargetOrientation::default(); + target_ori.orientation_radians = 1.23; + sz.incremental_target.regions[0] + .orientations + .push(target_ori); + + let target = next_target(&sz, None); + assert_eq!(target, Some((1.0, 2.0, 1.23))); + } + + #[test] + fn next_target_uses_node_orientation() { + let mut sz = safe_zone(1, 0, 0, 1.0, 2.0); + let mut node = ros_env::rmf_prototype_msgs::msg::TargetNode::default(); + let mut target_ori = ros_env::rmf_prototype_msgs::msg::TargetOrientation::default(); + target_ori.orientation_radians = -1.57; + node.orientations.push(target_ori); + sz.incremental_target.nodes.push(node); + + let target = next_target(&sz, None); + assert_eq!(target, Some((1.0, 2.0, -1.57))); + } + + #[test] + fn next_target_uses_conveyor_r1_dock_orientation() { + let mut sz = safe_zone(1, 0, 0, 0.0, 1.5); + sz.target_waypoint = vec![1].try_into().unwrap(); + + let mut plan = Plan::default(); + plan.waypoints = vec![ + ros_env::rmf_prototype_msgs::msg::Waypoint { + position: [0.0, 2.0], + ..Default::default() + }, + ros_env::rmf_prototype_msgs::msg::Waypoint { + position: [0.0, 1.5], + arrival_action: "dock_conveyor_r1_c1".to_string(), + ..Default::default() + }, + ]; + + let target = next_target(&sz, Some(&plan)); + assert!(target.is_some()); + let (x, y, yaw) = target.unwrap(); + assert_eq!((x, y), (0.0, 1.5)); + assert!((yaw - (-std::f32::consts::FRAC_PI_2)).abs() < 1e-6); + } + + #[test] + fn next_target_uses_conveyor_r2_dock_orientation() { + let mut sz = safe_zone(1, 0, 0, 2.5, 3.5); + sz.target_waypoint = vec![1].try_into().unwrap(); + + let mut plan = Plan::default(); + plan.waypoints = vec![ + ros_env::rmf_prototype_msgs::msg::Waypoint { + position: [2.5, 3.0], + ..Default::default() + }, + ros_env::rmf_prototype_msgs::msg::Waypoint { + position: [2.5, 3.5], + arrival_action: "dock_conveyor_r2_c2".to_string(), + ..Default::default() + }, + ]; + + let target = next_target(&sz, Some(&plan)); + assert!(target.is_some()); + let (x, y, yaw) = target.unwrap(); + assert_eq!((x, y), (2.5, 3.5)); + assert!((yaw - std::f32::consts::FRAC_PI_2).abs() < 1e-6); + } + + #[test] + fn next_target_uses_trajectory_heading_when_no_explicit_orientation() { + let mut sz = safe_zone(1, 0, 0, 5.0, 0.0); + sz.target_waypoint = vec![1].try_into().unwrap(); + + let mut plan = Plan::default(); + plan.waypoints = vec![ + ros_env::rmf_prototype_msgs::msg::Waypoint { + position: [0.0, 0.0], + ..Default::default() + }, + ros_env::rmf_prototype_msgs::msg::Waypoint { + position: [5.0, 0.0], + ..Default::default() + }, + ]; + + let target = next_target(&sz, Some(&plan)); + assert_eq!(target, Some((5.0, 0.0, 0.0))); + } + + #[test] + fn target_update_sent_when_waypoint_advances_even_if_nearby() { + let mut current_sz = safe_zone(1, 0, 0, 0.0, 2.0); + current_sz.target_waypoint = vec![0].try_into().unwrap(); + let current = CurrentSafeZone(Some(current_sz)); + + // Next waypoint is only 0.5m away (distancesq = 0.25 < 0.5) + let mut next_sz = safe_zone(1, 0, 1, 0.0, 1.5); + next_sz.target_waypoint = vec![1].try_into().unwrap(); + + assert!(current.should_update_target(&next_sz)); + } + + #[test] + fn target_update_sent_when_orientation_changes() { + let mut current_sz = safe_zone(1, 0, 0, 0.0, 1.5); + let mut ori1 = ros_env::rmf_prototype_msgs::msg::TargetOrientation::default(); + ori1.orientation_radians = 0.0; + current_sz.incremental_target.regions[0] + .orientations + .push(ori1); + let current = CurrentSafeZone(Some(current_sz)); + + // Same position, but orientation changes to -pi/2 + let mut next_sz = safe_zone(1, 0, 1, 0.0, 1.5); + let mut ori2 = ros_env::rmf_prototype_msgs::msg::TargetOrientation::default(); + ori2.orientation_radians = -std::f32::consts::FRAC_PI_2; + next_sz.incremental_target.regions[0] + .orientations + .push(ori2); + + assert!(current.should_update_target(&next_sz)); + } } diff --git a/path_server/rmf_path_server/src/lib.rs b/path_server/rmf_path_server/src/lib.rs index 7cc6a6f..b297d8a 100644 --- a/path_server/rmf_path_server/src/lib.rs +++ b/path_server/rmf_path_server/src/lib.rs @@ -134,7 +134,8 @@ impl PlanServer

{ robot_id ); - // If region constraints are empty, resolve coordinates from vertex position + // If region constraints are empty, resolve coordinates from vertex position. + // If already present, ensure primary region matches the exact vertex position. if msg.constraints.regions.is_empty() { msg.constraints.regions.push(TargetRegion { region: Region { @@ -143,6 +144,9 @@ impl PlanServer

{ }, ..Default::default() }); + } else if let Some(first_reg) = msg.constraints.regions.first_mut() { + first_reg.region.points = vec![vertex.position[0], vertex.position[1]]; + first_reg.region.hint = Region::HINT_POINT; } // Check for special arrival action (e.g. docking) @@ -270,7 +274,7 @@ impl PlanServer

{ continue; }; let target_action = target_actions.get(robot_id).map(|s| s.as_str()); - let plan = Self::to_plan_msg( + let mut plan = Self::to_plan_msg( agent_idx, traj, plan_id, @@ -280,6 +284,11 @@ impl PlanServer

{ target_action, 1.0, ); + if let Some(dest) = goals.get(robot_id) { + if let Some(last_wp) = plan.waypoints.last_mut() { + last_wp.arrival_constraints = dest.constraints.clone(); + } + } plans.insert(robot_id.clone(), plan); } diff --git a/path_server/rmf_path_server/src/planner.rs b/path_server/rmf_path_server/src/planner.rs index 22323cc..7d3765d 100644 --- a/path_server/rmf_path_server/src/planner.rs +++ b/path_server/rmf_path_server/src/planner.rs @@ -273,6 +273,22 @@ impl MapfPlanner for PibtPlanner { } } + // Clamp the final waypoint to exact goal coordinates to prevent coarse grid discretization errors + for (agent_idx, id) in robot_ids.iter().enumerate() { + if let Some(dest) = goals.get(id) { + if let Some(region) = dest.constraints.regions.first() { + if region.region.points.len() >= 2 { + let gx_f32 = region.region.points[0]; + let gy_f32 = region.region.points[1]; + if let Some(last_pose) = trajectories[agent_idx].last_mut() { + last_pose.translation.vector[0] = gx_f32; + last_pose.translation.vector[1] = gy_f32; + } + } + } + } + } + Ok(trajectories) } } diff --git a/path_server/rmf_path_server/tests/test_nav_graph.rs b/path_server/rmf_path_server/tests/test_nav_graph.rs index 78acee2..f40f085 100644 --- a/path_server/rmf_path_server/tests/test_nav_graph.rs +++ b/path_server/rmf_path_server/tests/test_nav_graph.rs @@ -252,7 +252,9 @@ fn test_path_server_graphkey_destination_dock_action() -> Result<(), Box