diff --git a/nav2_integration/rmf_nav2_traffic/Cargo.toml b/nav2_integration/rmf_nav2_traffic/Cargo.toml index c55eb31..d0a7f21 100644 --- a/nav2_integration/rmf_nav2_traffic/Cargo.toml +++ b/nav2_integration/rmf_nav2_traffic/Cargo.toml @@ -28,7 +28,7 @@ rclrs = "0.7" reqwest = { version = "0.13", features = ["json", "blocking"] } ros-env = "0.2" rosidl_runtime_rs = "0.6" -serde = "1.0" +serde = { version = "1.0", features = ["derive"] } serde_json = "1.0" thiserror = "2.0" tokio = { version = "1.52", features = ["full"] } 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 6f798f9..5c46132 100644 --- a/nav2_integration/rmf_nav2_traffic/src/inner_navigation_client.rs +++ b/nav2_integration/rmf_nav2_traffic/src/inner_navigation_client.rs @@ -1,14 +1,17 @@ -use crate::{safe_zone::PlanErrorPublisher, Nav2Agent}; +use crate::{safe_zone::PlanErrorPublisher, workflow::*, Nav2Agent}; use bevy::prelude::*; use bevy_ros2::{RclrsExecutorCommands, RclrsNode, RosActionClient}; use crossflow::{prelude::*, service::Service}; -use futures::StreamExt; +use futures::{FutureExt, StreamExt}; use nalgebra::UnitQuaternion; use rclrs::*; use ros_env::{ builtin_interfaces::msg::Time as RosTime, geometry_msgs::msg::{Point, Pose, PoseStamped, Quaternion}, - nav2_msgs::action::{NavigateToPose, NavigateToPose_Feedback, NavigateToPose_Goal}, + nav2_msgs::action::{ + DockRobot, DockRobot_Goal, NavigateToPose, NavigateToPose_Feedback, NavigateToPose_Goal, + UndockRobot, UndockRobot_Goal, + }, rmf_prototype_msgs::msg::SafeZoneId, std_msgs::msg::Header, }; @@ -17,21 +20,61 @@ use thiserror::Error; #[derive(Clone, Debug, Event)] pub struct InnerNavigationTarget { - agent: Entity, - safe_zone_id: SafeZoneId, - x: f64, - y: f64, - yaw: f64, + pub agent: Entity, + pub safe_zone_id: SafeZoneId, + pub x: f64, + pub y: f64, + pub yaw: f64, + pub dock_action: Option, + pub workflow: Vec, } impl InnerNavigationTarget { - pub fn new(agent: Entity, safe_zone_id: SafeZoneId, x: f64, y: f64, yaw: f64) -> Self { + pub fn new( + agent: Entity, + safe_zone_id: SafeZoneId, + x: f64, + y: f64, + yaw: f64, + dock_action: Option, + ) -> Self { + let workflow = dock_action + .as_deref() + .map(parse_workflow) + .unwrap_or_default(); + Self { + agent, + safe_zone_id, + x, + y, + yaw, + dock_action, + workflow, + } + } + + pub fn with_workflow( + agent: Entity, + safe_zone_id: SafeZoneId, + x: f64, + y: f64, + yaw: f64, + workflow: Vec, + ) -> Self { + let dock_action = workflow.first().and_then(|s| match s { + WorkflowActionStep::Dock { dock_id } => Some(dock_id.clone()), + WorkflowActionStep::Undock => Some("undock".to_string()), + WorkflowActionStep::Custom { name, .. } => Some(name.clone()), + _ => None, + }); Self { agent, safe_zone_id, x, y, yaw, + dock_action, + workflow, } } } @@ -110,6 +153,8 @@ impl Plugin for InnerNavigationClientPlugin { app.add_event::() .add_event::() .add_event::() + .add_event::() + .add_event::() .add_observer(create_inner_navigation_client); // Initialize Navigation services @@ -127,6 +172,12 @@ impl Plugin for InnerNavigationClientPlugin { } } +#[derive(Component, Clone)] +pub struct InnerDockClient { + pub dock_client: Arc>, + pub undock_client: Arc>, +} + fn create_inner_navigation_client( trigger: Trigger, mut commands: Commands, @@ -137,13 +188,22 @@ fn create_inner_navigation_client( let Ok(agent_name) = agents.get(e).map(|agent| agent.name.clone()) else { return; }; - let action_name = agent_name + "/inner/navigate_to_pose"; + let action_name = agent_name.clone() + "/inner/navigate_to_pose"; // Set up client for the inner Nav2 action let inner_action_client = RosActionClient::::new(&node, action_name); + let dock_action_name = agent_name.clone() + "/inner/dock_robot"; + let undock_action_name = agent_name + "/inner/undock_robot"; + let dock_client = RosActionClient::::new(&node, dock_action_name); + let undock_client = RosActionClient::::new(&node, undock_action_name); + commands .entity(e) - .insert(InnerNavigationClient::new(Arc::new(inner_action_client))); + .insert(InnerNavigationClient::new(Arc::new(inner_action_client))) + .insert(InnerDockClient { + dock_client: Arc::new(dock_client), + undock_client: Arc::new(undock_client), + }); } #[derive(Clone)] @@ -151,6 +211,14 @@ struct InnerNavigationRequest { agent: Entity, safe_zone_id: SafeZoneId, target_pose: PoseStamped, + dock_action: Option, + workflow: Vec, +} + +#[derive(Clone)] +struct InnerWorkflowRequest { + handle: CurrentInnerNavigationGoal, + steps: Vec, } #[derive(Clone)] @@ -218,6 +286,8 @@ impl InnerNavigationServices { let async_monitor_ongoing_navigation_service = app.spawn_service(async_monitor_ongoing_navigation); let process_navigation_result_service = app.spawn_service(process_navigation_result); + let check_workflow_action_service = app.spawn_service(check_workflow_action); + let async_execute_workflow_service = app.spawn_service(async_execute_workflow); let cleanup_goal_client_service = app.spawn_service(cleanup_goal_client); let log_error_service = app.spawn_service(log_inner_navigation_error); @@ -240,6 +310,8 @@ impl InnerNavigationServices { let async_monitor_new_navigation_request = builder.create_node(async_monitor_ongoing_navigation_service); let post_nav_processing = builder.create_node(process_navigation_result_service); + let check_workflow_action = builder.create_node(check_workflow_action_service); + let async_execute_workflow = builder.create_node(async_execute_workflow_service); let cleanup_goal_client = builder.create_node(cleanup_goal_client_service); let log_error = builder.create_node(log_error_service); @@ -302,8 +374,19 @@ impl InnerNavigationServices { // If goal was aborted, retry navigation by requesting new goal // Goal client will be updated in a downstream node builder.connect(post_nav_fork_result.ok, async_request_new_goal.input); - // Otherwise, cleanup goal client - builder.connect(post_nav_fork_result.err, cleanup_goal_client.input); + + // Check if workflow action should be executed upon reaching target waypoint + builder.connect(post_nav_fork_result.err, check_workflow_action.input); + let (check_wf_fork_result_input, check_wf_fork_result) = builder.create_fork_result(); + builder.connect(check_workflow_action.output, check_wf_fork_result_input); + + // If workflow steps are present, execute workflow action + builder.connect(check_wf_fork_result.ok, async_execute_workflow.input); + // When workflow completes, cleanup goal client + builder.connect(async_execute_workflow.output, cleanup_goal_client.input); + + // If no workflow steps required or navigation error, cleanup goal client + builder.connect(check_wf_fork_result.err, cleanup_goal_client.input); // Connect errors to logging node builder.connect(cancel_goal_fork_result.err, log_error.input); @@ -368,6 +451,8 @@ fn await_new_requests( agent: target.agent, safe_zone_id: target.safe_zone_id.clone(), target_pose: goal_pose.clone(), + dock_action: target.dock_action.clone(), + workflow: target.workflow.clone(), }; info!( "[{:?}] Requesting new pending goal to pose [{}, {}]", @@ -809,6 +894,339 @@ fn process_navigation_result( return Err(result); } +fn check_workflow_action( + Blocking { + request: result, .. + }: Blocking, +) -> Result { + match result { + Ok(ref success) => { + if !success.handle.request.workflow.is_empty() { + info!( + "[{:?}] Navigation succeeded to waypoint. Initiating {} workflow step(s)", + success.handle.request.agent.index(), + success.handle.request.workflow.len() + ); + Ok(InnerWorkflowRequest { + handle: success.handle.clone(), + steps: success.handle.request.workflow.clone(), + }) + } else if let Some(ref dock_id) = success.handle.request.dock_action { + let steps = parse_workflow(dock_id); + if !steps.is_empty() { + info!( + "[{:?}] Navigation succeeded to waypoint. Initiating fallback {} workflow step(s) from dock_action '{}'", + success.handle.request.agent.index(), + steps.len(), + dock_id + ); + Ok(InnerWorkflowRequest { + handle: success.handle.clone(), + steps, + }) + } else { + Err(result) + } + } else { + Err(result) + } + } + Err(_) => Err(result), + } +} + +fn async_execute_workflow( + Async { + request: wf_req, + channel, + .. + }: Async, + inner_dock_clients: Query<&InnerDockClient>, + executor_commands: Res, +) -> impl Future { + let handle = wf_req.handle.clone(); + let agent_entity = handle.request.agent; + let safe_zone_id = handle.request.safe_zone_id.clone(); + let dock_client_result = inner_dock_clients.get(agent_entity); + let Ok(dock_client) = dock_client_result else { + warn!( + "[{:?}] InnerDockClient not found for agent entity", + agent_entity.index() + ); + return std::future::ready(Ok(InnerNavigationSuccess { handle })).left_future(); + }; + + let dock_client = dock_client.clone(); + let steps = wf_req.steps.clone(); + let fallback_handle = handle.clone(); + let total_steps = steps.len(); + + info!( + "[{:?}] Executing workflow with {} step(s)", + agent_entity.index(), + total_steps + ); + + executor_commands + .run(async move { + for (idx, step) in steps.into_iter().enumerate() { + // Broadcast WorkflowStepEvent to Bevy observers + let step_clone = step.clone(); + let sz_id = safe_zone_id.clone(); + channel.commands(move |cmds| { + cmds.trigger(WorkflowStepEvent { + agent: agent_entity, + safe_zone_id: sz_id, + step: step_clone, + step_index: idx, + total_steps, + }); + }); + + match step { + WorkflowActionStep::Dock { dock_id } => { + info!( + "[{:?}] [Step {}/{}] Starting DockRobot action for dock_id: '{}'", + agent_entity.index(), + idx + 1, + total_steps, + dock_id + ); + let dock_goal = DockRobot_Goal { + use_dock_id: true, + dock_id: dock_id.clone(), + dock_pose: handle.request.target_pose.clone(), + dock_type: "simple_charging_dock".to_string(), + max_staging_time: 1000.0, + navigate_to_staging_pose: false, + }; + let Some(goal_client) = dock_client.dock_client.request_goal(dock_goal).await else { + warn!( + "[{:?}] DockRobot action request failed or server unavailable for dock '{}'", + agent_entity.index(), + dock_id + ); + continue; + }; + + let mut stream = goal_client.stream(); + let mut success = false; + let mut last_error_code = 0; + while let Some(event) = stream.next().await { + match event { + GoalEvent::Feedback(fb) => { + info!( + "[{:?}] [DockRobot] Docking state: {}, retries: {}", + agent_entity.index(), + fb.state, + fb.num_retries + ); + } + GoalEvent::Status(s) => { + debug!("[{:?}] [DockRobot] Status: {:?}", agent_entity.index(), s.code); + } + GoalEvent::Result((status, result)) => { + last_error_code = result.error_code; + info!( + "[{:?}] [DockRobot] Result: {:?}, success={}, error_code={}, error_msg='{}'", + agent_entity.index(), + status, + result.success, + result.error_code, + result.error_msg + ); + if status == GoalStatusCode::Succeeded || result.success { + success = true; + } + } + } + } + if !success && last_error_code == 901 { + warn!( + "[{:?}] Dock '{}' not in database (code 901). Retrying with direct target dock_pose...", + agent_entity.index(), + dock_id + ); + let fallback_goal = DockRobot_Goal { + use_dock_id: false, + dock_id: String::new(), + dock_pose: handle.request.target_pose.clone(), + dock_type: "simple_charging_dock".to_string(), + max_staging_time: 1000.0, + navigate_to_staging_pose: false, + }; + if let Some(fb_client) = dock_client.dock_client.request_goal(fallback_goal).await { + let mut fb_stream = fb_client.stream(); + while let Some(event) = fb_stream.next().await { + match event { + GoalEvent::Result((status, result)) => { + info!( + "[{:?}] [DockRobot Fallback] Result: {:?}, success={}, error_code={}, error_msg='{}'", + agent_entity.index(), + status, + result.success, + result.error_code, + result.error_msg + ); + if status == GoalStatusCode::Succeeded || result.success { + success = true; + } + } + _ => {} + } + } + } + } + if !success && last_error_code == 903 { + warn!( + "[{:?}] Robot not pre-staged for dock '{}' (code 903). Retrying with navigate_to_staging_pose=true...", + agent_entity.index(), + dock_id + ); + let staging_goal = DockRobot_Goal { + use_dock_id: true, + dock_id: dock_id.clone(), + dock_pose: handle.request.target_pose.clone(), + dock_type: "simple_charging_dock".to_string(), + max_staging_time: 1000.0, + navigate_to_staging_pose: true, + }; + if let Some(st_client) = dock_client.dock_client.request_goal(staging_goal).await { + let mut st_stream = st_client.stream(); + while let Some(event) = st_stream.next().await { + if let GoalEvent::Result((status, result)) = event { + info!( + "[{:?}] [DockRobot Stage Retry] Result: {:?}, success={}, error_code={}, error_msg='{}'", + agent_entity.index(), + status, + result.success, + result.error_code, + result.error_msg + ); + if status == GoalStatusCode::Succeeded || result.success { + success = true; + } + } + } + } + } + if !success { + let sz_id = safe_zone_id.clone(); + channel.commands(move |cmds| { + cmds.trigger(WorkflowCompletedEvent { + agent: agent_entity, + safe_zone_id: sz_id, + success: false, + }); + }); + return Err(InnerNavigationError { + handle: Some(handle), + kind: InnerNavigationErrorKind::GoalAbortedError, + }); + } + } + WorkflowActionStep::Undock => { + info!( + "[{:?}] [Step {}/{}] Starting UndockRobot action", + agent_entity.index(), + idx + 1, + total_steps + ); + let undock_goal = UndockRobot_Goal { + dock_type: String::new(), + max_undocking_time: 60.0, + }; + let Some(goal_client) = dock_client.undock_client.request_goal(undock_goal).await else { + warn!( + "[{:?}] UndockRobot action request failed or server unavailable", + agent_entity.index() + ); + continue; + }; + + let mut stream = goal_client.stream(); + let mut success = false; + while let Some(event) = stream.next().await { + match event { + GoalEvent::Status(s) => { + debug!("[{:?}] [UndockRobot] Status: {:?}", agent_entity.index(), s.code); + } + GoalEvent::Result((status, result)) => { + info!( + "[{:?}] [UndockRobot] Result: {:?}, success={}", + agent_entity.index(), + status, + result.success + ); + if status == GoalStatusCode::Succeeded || result.success { + success = true; + } + } + _ => {} + } + } + if !success { + let sz_id = safe_zone_id.clone(); + channel.commands(move |cmds| { + cmds.trigger(WorkflowCompletedEvent { + agent: agent_entity, + safe_zone_id: sz_id, + success: false, + }); + }); + return Err(InnerNavigationError { + handle: Some(handle), + kind: InnerNavigationErrorKind::GoalAbortedError, + }); + } + } + WorkflowActionStep::Wait { duration_sec } => { + info!( + "[{:?}] [Step {}/{}] Waiting for {:.2} seconds", + agent_entity.index(), + idx + 1, + total_steps, + duration_sec + ); + if duration_sec > 0.0 { + tokio::time::sleep(std::time::Duration::from_secs_f32(duration_sec)).await; + } + } + WorkflowActionStep::Custom { name, payload } => { + info!( + "[{:?}] [Step {}/{}] Executing custom action '{}' (payload: {:?})", + agent_entity.index(), + idx + 1, + total_steps, + name, + payload + ); + } + } + } + + let sz_id = safe_zone_id.clone(); + channel.commands(move |cmds| { + cmds.trigger(WorkflowCompletedEvent { + agent: agent_entity, + safe_zone_id: sz_id, + success: true, + }); + }); + + info!( + "[{:?}] Successfully completed all {} workflow step(s)", + agent_entity.index(), + total_steps + ); + Ok(InnerNavigationSuccess { handle }) + }) + .then(|res| async move { + res.unwrap_or(Ok(InnerNavigationSuccess { handle: fallback_handle })) + }) + .right_future() +} + fn should_publish_path_blocked( completed_goal_id: &SafeZoneId, active_goal: Option<(&SafeZoneId, bool)>, @@ -922,4 +1340,47 @@ mod tests { assert!(!should_publish_path_blocked(&completed, None)); } + + #[test] + fn inner_navigation_target_parses_workflow_automatically() { + let target = InnerNavigationTarget::new( + Entity::from_raw(1), + safe_zone_id(1, 0, 0), + 1.0, + 2.0, + 0.0, + Some(r#"[{"action": "dock", "dock_id": "d1"}, {"action": "undock"}]"#.to_string()), + ); + + assert_eq!(target.workflow.len(), 2); + assert_eq!( + target.workflow[0], + WorkflowActionStep::Dock { + dock_id: "d1".to_string() + } + ); + assert_eq!(target.workflow[1], WorkflowActionStep::Undock); + } + + #[test] + fn inner_navigation_target_with_workflow_derives_dock_action() { + let steps = vec![ + WorkflowActionStep::Dock { + dock_id: "station_42".to_string(), + }, + WorkflowActionStep::Wait { duration_sec: 5.0 }, + WorkflowActionStep::Undock, + ]; + let target = InnerNavigationTarget::with_workflow( + Entity::from_raw(1), + safe_zone_id(1, 0, 0), + 1.0, + 2.0, + 0.0, + steps.clone(), + ); + + assert_eq!(target.workflow, steps); + assert_eq!(target.dock_action, Some("station_42".to_string())); + } } diff --git a/nav2_integration/rmf_nav2_traffic/src/lib.rs b/nav2_integration/rmf_nav2_traffic/src/lib.rs index 469761c..eba9d3f 100644 --- a/nav2_integration/rmf_nav2_traffic/src/lib.rs +++ b/nav2_integration/rmf_nav2_traffic/src/lib.rs @@ -34,6 +34,9 @@ pub use navigation_server::*; pub mod safe_zone; pub use safe_zone::*; +pub mod workflow; +pub use workflow::*; + #[derive(Default)] pub struct Nav2TrafficPlugin {} @@ -49,7 +52,25 @@ impl Plugin for Nav2TrafficPlugin { )); // Spawn agents last - let agent_names = vec!["robot0".to_string(), "robot1".to_string()]; + let agent_names: Vec = if let Ok(env_agents) = std::env::var("RMF_NAV2_AGENTS") { + env_agents + .split(|c: char| c == ',' || c.is_whitespace()) + .map(|s| s.trim().to_string()) + .filter(|s| !s.is_empty()) + .collect() + } else { + let cli_agents: Vec = std::env::args() + .skip(1) + .filter(|arg| !arg.starts_with('-') && !arg.starts_with('_')) + .collect(); + if !cli_agents.is_empty() { + cli_agents + } else { + vec!["robot0".to_string(), "robot1".to_string()] + } + }; + + info!("Nav2TrafficPlugin: spawning agents: {:?}", agent_names); for name in agent_names { app.world_mut().spawn(Nav2Agent::new(name)); } 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..9c0dbc2 100644 --- a/nav2_integration/rmf_nav2_traffic/src/safe_zone.rs +++ b/nav2_integration/rmf_nav2_traffic/src/safe_zone.rs @@ -1,12 +1,9 @@ -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::{ 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; @@ -15,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>, @@ -43,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; }; @@ -56,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; }; @@ -90,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, } } @@ -100,8 +156,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); @@ -128,6 +185,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, @@ -182,21 +259,17 @@ 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, + 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 { @@ -217,16 +290,23 @@ 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, dock_action)) = 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); @@ -237,8 +317,8 @@ fn update_incremental_target( continue; } debug!( - "[{:?}] Updating SafeZone to target: ({:.2}, {:.2}, {:.2})", - agent.name, target_x, target_y, target_yaw + "[{:?}] Updating SafeZone to target: ({:.2}, {:.2}, {:.2}), dock={:?}", + agent.name, target_x, target_y, target_yaw, dock_action ); *current_safe_zone = CurrentSafeZone(Some(safe_zone.clone())); @@ -249,10 +329,32 @@ fn update_incremental_target( target_x as f64, target_y as f64, target_yaw as f64, + dock_action, )); } } +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"); @@ -268,56 +370,145 @@ 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, Option)> { let constraints = &safe_zone.incremental_target; let mut xy: Option<(f32, f32)> = None; let mut yaw: Option = None; + let mut dock_action: 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 and dock action 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); + } + if let Some(name) = target_node.key.name.first() { + let name_str = name.to_string(); + if name_str.contains("dock") { + dock_action = Some(name_str); + } + } } - 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])); + } + + // Extract dock action or workflow from arrival_action + if !waypoint.arrival_action.is_empty() { + dock_action = Some(waypoint.arrival_action.clone()); + } + + // Fallback or override from plan.workflow if this is the target/final waypoint + if dock_action.is_none() && !plan.workflow.is_empty() { + let is_last_wp = wp_idx + 1 >= plan.waypoints.len(); + if is_last_wp { + dock_action = Some(plan.workflow.clone()); + } + } + + // Fallback dock action from waypoint arrival_constraints.nodes + if dock_action.is_none() { + for node in &waypoint.arrival_constraints.nodes { + if let Some(name) = node.key.name.first() { + let name_str = name.to_string(); + if name_str.contains("dock") { + dock_action = Some(name_str); + break; + } + } + } + } + + // 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_str = dock_action.as_deref().unwrap_or(&waypoint.arrival_action); + if action_str.contains("conveyor_r1") || action_str.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_str.contains("conveyor_r2") + || action_str.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, dock_action)) } #[cfg(test)] @@ -383,4 +574,196 @@ 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, None))); + } + + #[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, None))); + } + + #[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, dock) = target.unwrap(); + assert_eq!((x, y), (0.0, 1.5)); + assert!((yaw - (-std::f32::consts::FRAC_PI_2)).abs() < 1e-6); + assert_eq!(dock, Some("dock_conveyor_r1_c1".to_string())); + } + + #[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, dock) = target.unwrap(); + assert_eq!((x, y), (2.5, 3.5)); + assert!((yaw - std::f32::consts::FRAC_PI_2).abs() < 1e-6); + assert_eq!(dock, Some("dock_conveyor_r2_c2".to_string())); + } + + #[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, None))); + } + + #[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)); + } + + #[test] + fn next_target_extracts_json_workflow_from_arrival_action() { + let mut sz = safe_zone(1, 0, 0, 0.0, 1.5); + sz.target_waypoint = vec![1].try_into().unwrap(); + + let workflow_json = r#"[ + {"action": "dock", "dock_id": "dock_conveyor_r1_c1"}, + {"action": "wait", "duration_sec": 3.0}, + {"action": "undock"} + ]"#; + + 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: workflow_json.to_string(), + ..Default::default() + }, + ]; + + let target = next_target(&sz, Some(&plan)); + assert!(target.is_some()); + let (x, y, yaw, action) = target.unwrap(); + assert_eq!((x, y), (0.0, 1.5)); + assert!((yaw - (-std::f32::consts::FRAC_PI_2)).abs() < 1e-6); + assert_eq!(action, Some(workflow_json.to_string())); + } + + #[test] + fn next_target_uses_plan_workflow_on_final_waypoint() { + 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.workflow = "dock_conveyor_r2_c2".to_string(); + 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: String::new(), + ..Default::default() + }, + ]; + + let target = next_target(&sz, Some(&plan)); + assert!(target.is_some()); + let (x, y, yaw, action) = target.unwrap(); + assert_eq!((x, y), (2.5, 3.5)); + assert!((yaw - std::f32::consts::FRAC_PI_2).abs() < 1e-6); + assert_eq!(action, Some("dock_conveyor_r2_c2".to_string())); + } } diff --git a/nav2_integration/rmf_nav2_traffic/src/workflow.rs b/nav2_integration/rmf_nav2_traffic/src/workflow.rs new file mode 100644 index 0000000..f736548 --- /dev/null +++ b/nav2_integration/rmf_nav2_traffic/src/workflow.rs @@ -0,0 +1,582 @@ +/* + * Copyright (C) 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 bevy::prelude::*; +use ros_env::rmf_prototype_msgs::msg::SafeZoneId; +use serde::{Deserialize, Serialize}; +use std::collections::HashSet; + +/// A discrete step in a generic post-arrival or waypoint execution workflow. +#[derive(Serialize, Deserialize, Clone, Debug, PartialEq)] +#[serde(tag = "action", rename_all = "snake_case")] +pub enum WorkflowActionStep { + /// Command the robot to perform a docking procedure with the specified dock target. + Dock { + #[serde(default)] + dock_id: String, + }, + /// Command the robot to undock and return to navigation staging. + Undock, + /// Pause execution for the specified duration (in seconds). + Wait { + #[serde(default)] + duration_sec: f32, + }, + /// An arbitrary or user-defined action step. + Custom { + name: String, + #[serde(default)] + payload: serde_json::Value, + }, +} + +impl WorkflowActionStep { + pub fn is_dock(&self) -> bool { + matches!(self, Self::Dock { .. }) + } + + pub fn dock_id(&self) -> Option<&str> { + match self { + Self::Dock { dock_id } => Some(dock_id.as_str()), + _ => None, + } + } + + pub fn is_undock(&self) -> bool { + matches!(self, Self::Undock) + } + + pub fn is_wait(&self) -> bool { + matches!(self, Self::Wait { .. }) + } + + pub fn duration_sec(&self) -> Option { + match self { + Self::Wait { duration_sec } => Some(*duration_sec), + _ => None, + } + } +} + +/// A structured container for a sequence of workflow action steps. +#[derive(Serialize, Deserialize, Clone, Debug, Default, PartialEq)] +pub struct Workflow { + pub steps: Vec, +} + +impl Workflow { + pub fn new(steps: Vec) -> Self { + Self { steps } + } + + pub fn is_empty(&self) -> bool { + self.steps.is_empty() + } + + pub fn len(&self) -> usize { + self.steps.len() + } + + pub fn parse(s: &str) -> Self { + Self { + steps: parse_workflow(s), + } + } +} + +/// Event broadcast in Bevy when a specific workflow step begins execution. +#[derive(Clone, Debug, Event)] +pub struct WorkflowStepEvent { + pub agent: Entity, + pub safe_zone_id: SafeZoneId, + pub step: WorkflowActionStep, + pub step_index: usize, + pub total_steps: usize, +} + +/// Event broadcast in Bevy when an entire workflow sequence completes. +#[derive(Clone, Debug, Event)] +pub struct WorkflowCompletedEvent { + pub agent: Entity, + pub safe_zone_id: SafeZoneId, + pub success: bool, +} + +/// Parses a string representation of an arrival action, departure action, +/// or plan workflow into a sequence of `WorkflowActionStep`s. +/// +/// Supported formats: +/// 1. Simple strings: e.g. `"dock_conveyor_r1_c1"`, `"undock"` +/// 2. Single JSON actions: e.g. `{"action": "dock", "dock_id": "dock_1"}` +/// 3. Open-RMF category actions: e.g. `{"category": "dock", "description": {"dock_name": "dock_1"}}` +/// 4. JSON array of steps: `[{"action": "dock", ...}, {"action": "wait", ...}, {"action": "undock"}]` +/// 5. Wrapped sequence objects: `{"steps": [...]}` or `{"category": "sequence", "description": {"sequence": [...]}}` +/// 6. Native Crossflow diagram JSON: `{ "version": "0.1.0", "start": "...", "ops": { ... } }` +pub fn parse_workflow(input: &str) -> Vec { + let trimmed = input.trim(); + if trimmed.is_empty() { + return Vec::new(); + } + + if let Ok(value) = serde_json::from_str::(trimmed) { + return parse_workflow_from_value(&value); + } + + parse_single_string_step(trimmed) +} + +/// Parses a `serde_json::Value` into a sequence of `WorkflowActionStep`s. +pub fn parse_workflow_from_value(value: &serde_json::Value) -> Vec { + match value { + serde_json::Value::Array(arr) => arr.iter().filter_map(parse_step_from_value).collect(), + serde_json::Value::Object(map) => { + // Check if this is a Crossflow diagram + if map.contains_key("ops") && map.contains_key("start") { + let diagram_steps = parse_diagram_from_value(value); + if !diagram_steps.is_empty() { + return diagram_steps; + } + } + + // Check for explicit sequence / steps array wrappers + for key in ["steps", "sequence", "actions"] { + if let Some(steps_val) = map.get(key) { + if let Some(steps_arr) = steps_val.as_array() { + return steps_arr.iter().filter_map(parse_step_from_value).collect(); + } + } + } + + // Check for Open-RMF category = "sequence" + if let Some(cat) = map.get("category").and_then(|v| v.as_str()) { + if cat.eq_ignore_ascii_case("sequence") { + if let Some(desc) = map.get("description") { + if let Some(desc_arr) = desc.as_array() { + return desc_arr.iter().filter_map(parse_step_from_value).collect(); + } + if let Some(desc_obj) = desc.as_object() { + for key in ["sequence", "steps", "actions"] { + if let Some(arr) = desc_obj.get(key).and_then(|v| v.as_array()) { + return arr.iter().filter_map(parse_step_from_value).collect(); + } + } + } + } + } + } + + // Check for nested "workflow" field + if let Some(wf) = map.get("workflow") { + if let Some(s) = wf.as_str() { + return parse_workflow(s); + } + return parse_workflow_from_value(wf); + } + + // Single action object + parse_step_from_value(value).into_iter().collect() + } + serde_json::Value::String(s) => parse_workflow(s), + _ => Vec::new(), + } +} + +/// Parses a single step from a JSON Value. +pub fn parse_step_from_value(value: &serde_json::Value) -> Option { + if let Some(s) = value.as_str() { + return parse_single_string_step(s).into_iter().next(); + } + + // Try direct serde deserialization matching `WorkflowActionStep` + if let Ok(step) = serde_json::from_value::(value.clone()) { + return Some(step); + } + + let obj = value.as_object()?; + + // Extract kind from "action", "category", "type", or "name" + let kind = obj + .get("action") + .or_else(|| obj.get("category")) + .or_else(|| obj.get("type")) + .or_else(|| obj.get("name")) + .and_then(|v| v.as_str()) + .map(|s| s.to_ascii_lowercase())?; + + if kind == "undock" || kind.contains("undock") { + return Some(WorkflowActionStep::Undock); + } + + if kind == "dock" || (kind.contains("dock") && !kind.contains("undock")) { + let dock_id = obj + .get("dock_id") + .or_else(|| obj.get("dock_name")) + .and_then(|v| v.as_str()) + .map(|s| s.to_string()) + .or_else(|| { + obj.get("description").and_then(|desc| { + if let Some(s) = desc.as_str() { + Some(s.to_string()) + } else if let Some(desc_obj) = desc.as_object() { + desc_obj + .get("dock_id") + .or_else(|| desc_obj.get("dock_name")) + .and_then(|v| v.as_str()) + .map(|s| s.to_string()) + } else { + None + } + }) + }) + .unwrap_or_else(|| { + if kind != "dock" { + kind.clone() + } else { + String::new() + } + }); + + return Some(WorkflowActionStep::Dock { dock_id }); + } + + if kind == "wait" || kind == "sleep" || kind == "delay" { + let duration = obj + .get("duration_sec") + .or_else(|| obj.get("duration")) + .or_else(|| obj.get("seconds")) + .and_then(|v| v.as_f64()) + .or_else(|| { + obj.get("description").and_then(|desc| { + if let Some(num) = desc.as_f64() { + Some(num) + } else if let Some(desc_obj) = desc.as_object() { + desc_obj + .get("duration") + .or_else(|| desc_obj.get("duration_sec")) + .or_else(|| desc_obj.get("seconds")) + .and_then(|v| v.as_f64()) + } else { + None + } + }) + }) + .unwrap_or(0.0) as f32; + + return Some(WorkflowActionStep::Wait { + duration_sec: duration, + }); + } + + // Default to Custom action + let name = obj + .get("name") + .and_then(|v| v.as_str()) + .unwrap_or(&kind) + .to_string(); + let payload = obj + .get("payload") + .or_else(|| obj.get("description")) + .cloned() + .unwrap_or_else(|| value.clone()); + + Some(WorkflowActionStep::Custom { name, payload }) +} + +/// Fallback for non-JSON strings. +fn parse_single_string_step(s: &str) -> Vec { + let lower = s.to_ascii_lowercase(); + if lower.contains("undock") { + vec![WorkflowActionStep::Undock] + } else if lower.contains("dock") { + vec![WorkflowActionStep::Dock { + dock_id: s.to_string(), + }] + } else { + vec![WorkflowActionStep::Custom { + name: s.to_string(), + payload: serde_json::Value::Null, + }] + } +} + +/// Parses sequential steps from a Crossflow diagram JSON structure. +fn parse_diagram_from_value(value: &serde_json::Value) -> Vec { + let mut steps = Vec::new(); + let Some(ops) = value.get("ops").and_then(|v| v.as_object()) else { + return steps; + }; + let Some(start_id) = value.get("start").and_then(|v| v.as_str()) else { + return steps; + }; + + let mut current_id = start_id.to_string(); + let mut visited = HashSet::new(); + + while !current_id.is_empty() && visited.insert(current_id.clone()) { + let Some(op) = ops.get(¤t_id) else { + break; + }; + + let builder = op + .get("builder") + .and_then(|v| v.as_str()) + .unwrap_or("") + .to_ascii_lowercase(); + + let config = op.get("config").cloned().unwrap_or(serde_json::Value::Null); + + match builder.as_str() { + "dock" => { + let dock_id = config + .get("dock_id") + .or_else(|| config.get("dock_name")) + .and_then(|v| v.as_str()) + .unwrap_or("") + .to_string(); + steps.push(WorkflowActionStep::Dock { dock_id }); + } + "undock" => { + steps.push(WorkflowActionStep::Undock); + } + "wait" | "sleep" | "delay" => { + let duration = config + .get("duration_sec") + .or_else(|| config.get("duration")) + .or_else(|| config.get("seconds")) + .and_then(|v| v.as_f64()) + .unwrap_or(0.0) as f32; + steps.push(WorkflowActionStep::Wait { + duration_sec: duration, + }); + } + other if !other.is_empty() => { + steps.push(WorkflowActionStep::Custom { + name: other.to_string(), + payload: config, + }); + } + _ => {} + } + + // Advance to next node + if let Some(next_val) = op.get("next") { + if let Some(next_str) = next_val.as_str() { + current_id = next_str.to_string(); + } else if let Some(next_obj) = next_val.as_object() { + if let Some(builtin) = next_obj.get("builtin").and_then(|v| v.as_str()) { + if builtin == "terminate" || builtin == "dispose" { + break; + } + } + break; + } else if let Some(next_arr) = next_val.as_array() { + if let Some(first) = next_arr.first().and_then(|v| v.as_str()) { + current_id = first.to_string(); + } else { + break; + } + } else { + break; + } + } else { + break; + } + } + + steps +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_parse_empty_and_whitespace() { + assert!(parse_workflow("").is_empty()); + assert!(parse_workflow(" ").is_empty()); + assert!(parse_workflow("\n\t").is_empty()); + } + + #[test] + fn test_parse_simple_strings() { + let dock = parse_workflow("dock_conveyor_r1_c1"); + assert_eq!( + dock, + vec![WorkflowActionStep::Dock { + dock_id: "dock_conveyor_r1_c1".to_string() + }] + ); + + let undock = parse_workflow("undock"); + assert_eq!(undock, vec![WorkflowActionStep::Undock]); + + let custom = parse_workflow("some_random_task"); + assert_eq!( + custom, + vec![WorkflowActionStep::Custom { + name: "some_random_task".to_string(), + payload: serde_json::Value::Null, + }] + ); + } + + #[test] + fn test_parse_single_json_action() { + let dock_json = r#"{"action": "dock", "dock_id": "station_alpha"}"#; + assert_eq!( + parse_workflow(dock_json), + vec![WorkflowActionStep::Dock { + dock_id: "station_alpha".to_string() + }] + ); + + let undock_json = r#"{"action": "undock"}"#; + assert_eq!( + parse_workflow(undock_json), + vec![WorkflowActionStep::Undock] + ); + + let wait_json = r#"{"action": "wait", "duration_sec": 3.5}"#; + assert_eq!( + parse_workflow(wait_json), + vec![WorkflowActionStep::Wait { duration_sec: 3.5 }] + ); + + let custom_json = r#"{"action": "custom", "name": "scanner", "payload": {"mode": "high"}}"#; + assert_eq!( + parse_workflow(custom_json), + vec![WorkflowActionStep::Custom { + name: "scanner".to_string(), + payload: serde_json::json!({"mode": "high"}), + }] + ); + } + + #[test] + fn test_parse_rmf_action_schema() { + let rmf_dock = + r#"{"category": "dock", "description": {"dock_name": "dock_conveyor_r1_c1"}}"#; + assert_eq!( + parse_workflow(rmf_dock), + vec![WorkflowActionStep::Dock { + dock_id: "dock_conveyor_r1_c1".to_string() + }] + ); + + let rmf_undock = r#"{"category": "undock"}"#; + assert_eq!(parse_workflow(rmf_undock), vec![WorkflowActionStep::Undock]); + + let rmf_wait = r#"{"category": "wait", "description": {"duration": 2.5}}"#; + assert_eq!( + parse_workflow(rmf_wait), + vec![WorkflowActionStep::Wait { duration_sec: 2.5 }] + ); + } + + #[test] + fn test_parse_json_array_sequence() { + let array_json = r#"[ + {"action": "dock", "dock_id": "dock_station_1"}, + {"action": "wait", "duration_sec": 4.0}, + {"action": "undock"} + ]"#; + + let steps = parse_workflow(array_json); + assert_eq!(steps.len(), 3); + assert_eq!( + steps[0], + WorkflowActionStep::Dock { + dock_id: "dock_station_1".to_string() + } + ); + assert_eq!(steps[1], WorkflowActionStep::Wait { duration_sec: 4.0 }); + assert_eq!(steps[2], WorkflowActionStep::Undock); + } + + #[test] + fn test_parse_rmf_sequence_wrapper() { + let seq_json = r#"{ + "category": "sequence", + "description": { + "sequence": [ + {"category": "dock", "description": {"dock_name": "dock_station_1"}}, + {"category": "wait", "description": {"duration": 1.5}}, + {"category": "undock"} + ] + } + }"#; + + let steps = parse_workflow(seq_json); + assert_eq!(steps.len(), 3); + assert_eq!( + steps[0], + WorkflowActionStep::Dock { + dock_id: "dock_station_1".to_string() + } + ); + assert_eq!(steps[1], WorkflowActionStep::Wait { duration_sec: 1.5 }); + assert_eq!(steps[2], WorkflowActionStep::Undock); + } + + #[test] + fn test_parse_crossflow_diagram() { + let diagram_json = r#"{ + "version": "0.1.0", + "start": "step_dock", + "ops": { + "step_dock": { + "type": "node", + "builder": "dock", + "config": { "dock_id": "dock_bay_2" }, + "next": "step_wait" + }, + "step_wait": { + "type": "node", + "builder": "wait", + "config": { "duration_sec": 5.0 }, + "next": "step_undock" + }, + "step_undock": { + "type": "node", + "builder": "undock", + "next": { "builtin": "terminate" } + } + } + }"#; + + let steps = parse_workflow(diagram_json); + assert_eq!(steps.len(), 3); + assert_eq!( + steps[0], + WorkflowActionStep::Dock { + dock_id: "dock_bay_2".to_string() + } + ); + assert_eq!(steps[1], WorkflowActionStep::Wait { duration_sec: 5.0 }); + assert_eq!(steps[2], WorkflowActionStep::Undock); + } + + #[test] + fn test_workflow_struct_helpers() { + let wf = Workflow::parse(r#"[{"action": "dock", "dock_id": "d1"}, {"action": "undock"}]"#); + assert_eq!(wf.len(), 2); + assert!(!wf.is_empty()); + assert!(wf.steps[0].is_dock()); + assert_eq!(wf.steps[0].dock_id(), Some("d1")); + assert!(wf.steps[1].is_undock()); + } +} diff --git a/nav2_integration/sp_demo_nav2_bringup/params/nav2_multirobot_params_all.yaml b/nav2_integration/sp_demo_nav2_bringup/params/nav2_multirobot_params_all.yaml index 31861b8..3a0588d 100644 --- a/nav2_integration/sp_demo_nav2_bringup/params/nav2_multirobot_params_all.yaml +++ b/nav2_integration/sp_demo_nav2_bringup/params/nav2_multirobot_params_all.yaml @@ -339,14 +339,14 @@ docking_server: controller_frequency: 50.0 initial_perception_timeout: 5.0 wait_charge_timeout: 5.0 - dock_approach_timeout: 30.0 + dock_approach_timeout: 60.0 undock_linear_tolerance: 0.05 undock_angular_tolerance: 0.1 max_retries: 3 base_frame: "base_link" fixed_frame: "odom" dock_backwards: false - dock_prestaging_tolerance: 0.5 + dock_prestaging_tolerance: 0.85 # Types of docks dock_plugins: ['simple_charging_dock'] @@ -354,27 +354,124 @@ docking_server: plugin: 'opennav_docking::SimpleChargingDock' docking_threshold: 0.05 staging_x_offset: -0.7 - use_external_detection_pose: true - use_battery_status: false # true - use_stall_detection: false # true - - external_detection_timeout: 1.0 - external_detection_translation_x: -0.18 - external_detection_translation_y: 0.0 - external_detection_rotation_roll: -1.57 - external_detection_rotation_pitch: -1.57 - external_detection_rotation_yaw: 0.0 + use_external_detection_pose: false + use_battery_status: false + use_stall_detection: false filter_coef: 0.1 - # Dock instances - docks: ['home_dock'] # Input your docks here - home_dock: + # Dock instances for simulation factory conveyors + docks: [ + 'dock_conveyor_r1_c1', 'conveyor_r1_c1_dock', 'conveyor_r1_c1', 'conveyor_r1_c1_final_dock', + 'dock_conveyor_r1_c2', 'conveyor_r1_c2_dock', 'conveyor_r1_c2', 'conveyor_r1_c2_final_dock', + 'dock_conveyor_r1_c3', 'conveyor_r1_c3_dock', 'conveyor_r1_c3', 'conveyor_r1_c3_final_dock', + 'dock_conveyor_r2_c1', 'conveyor_r2_c1_dock', 'conveyor_r2_c1', 'conveyor_r2_c1_final_dock', + 'dock_conveyor_r2_c2', 'conveyor_r2_c2_dock', 'conveyor_r2_c2', 'conveyor_r2_c2_final_dock', + 'dock_conveyor_r2_c3', 'conveyor_r2_c3_dock', 'conveyor_r2_c3', 'conveyor_r2_c3_final_dock', + ] + dock_conveyor_r1_c1: + type: 'simple_charging_dock' + frame: 'map' + pose: [0.0, 0.95, -1.57079632679] + conveyor_r1_c1_dock: + type: 'simple_charging_dock' + frame: 'map' + pose: [0.0, 0.95, -1.57079632679] + conveyor_r1_c1: + type: 'simple_charging_dock' + frame: 'map' + pose: [0.0, 0.95, -1.57079632679] + conveyor_r1_c1_final_dock: + type: 'simple_charging_dock' + frame: 'map' + pose: [0.0, 0.95, -1.57079632679] + dock_conveyor_r1_c2: + type: 'simple_charging_dock' + frame: 'map' + pose: [2.5, 0.95, -1.57079632679] + conveyor_r1_c2_dock: + type: 'simple_charging_dock' + frame: 'map' + pose: [2.5, 0.95, -1.57079632679] + conveyor_r1_c2: + type: 'simple_charging_dock' + frame: 'map' + pose: [2.5, 0.95, -1.57079632679] + conveyor_r1_c2_final_dock: + type: 'simple_charging_dock' + frame: 'map' + pose: [2.5, 0.95, -1.57079632679] + dock_conveyor_r1_c3: + type: 'simple_charging_dock' + frame: 'map' + pose: [5.0, 0.95, -1.57079632679] + conveyor_r1_c3_dock: + type: 'simple_charging_dock' + frame: 'map' + pose: [5.0, 0.95, -1.57079632679] + conveyor_r1_c3: + type: 'simple_charging_dock' + frame: 'map' + pose: [5.0, 0.95, -1.57079632679] + conveyor_r1_c3_final_dock: + type: 'simple_charging_dock' + frame: 'map' + pose: [5.0, 0.95, -1.57079632679] + dock_conveyor_r2_c1: + type: 'simple_charging_dock' + frame: 'map' + pose: [0.0, 4.05, 1.57079632679] + conveyor_r2_c1_dock: + type: 'simple_charging_dock' + frame: 'map' + pose: [0.0, 4.05, 1.57079632679] + conveyor_r2_c1: + type: 'simple_charging_dock' + frame: 'map' + pose: [0.0, 4.05, 1.57079632679] + conveyor_r2_c1_final_dock: + type: 'simple_charging_dock' + frame: 'map' + pose: [0.0, 4.05, 1.57079632679] + dock_conveyor_r2_c2: + type: 'simple_charging_dock' + frame: 'map' + pose: [2.5, 4.05, 1.57079632679] + conveyor_r2_c2_dock: + type: 'simple_charging_dock' + frame: 'map' + pose: [2.5, 4.05, 1.57079632679] + conveyor_r2_c2: + type: 'simple_charging_dock' + frame: 'map' + pose: [2.5, 4.05, 1.57079632679] + conveyor_r2_c2_final_dock: + type: 'simple_charging_dock' + frame: 'map' + pose: [2.5, 4.05, 1.57079632679] + dock_conveyor_r2_c3: + type: 'simple_charging_dock' + frame: 'map' + pose: [5.0, 4.05, 1.57079632679] + conveyor_r2_c3_dock: + type: 'simple_charging_dock' + frame: 'map' + pose: [5.0, 4.05, 1.57079632679] + conveyor_r2_c3: + type: 'simple_charging_dock' + frame: 'map' + pose: [5.0, 4.05, 1.57079632679] + conveyor_r2_c3_final_dock: type: 'simple_charging_dock' - frame: map - pose: [0.0, 0.0, 0.0] + frame: 'map' + pose: [5.0, 4.05, 1.57079632679] controller: k_phi: 3.0 k_delta: 2.0 v_linear_min: 0.15 v_linear_max: 0.15 + v_angular_max: 0.75 + slowdown_radius: 0.25 + use_collision_detection: false + dock_collision_threshold: 0.8 + transform_tolerance: 0.5 diff --git a/nav2_integration/sp_demo_nav2_bringup/params/nav2_params.yaml b/nav2_integration/sp_demo_nav2_bringup/params/nav2_params.yaml index 7ec070e..d390e62 100644 --- a/nav2_integration/sp_demo_nav2_bringup/params/nav2_params.yaml +++ b/nav2_integration/sp_demo_nav2_bringup/params/nav2_params.yaml @@ -406,14 +406,14 @@ docking_server: controller_frequency: 50.0 initial_perception_timeout: 5.0 wait_charge_timeout: 5.0 - dock_approach_timeout: 30.0 + dock_approach_timeout: 60.0 undock_linear_tolerance: 0.05 undock_angular_tolerance: 0.1 max_retries: 3 base_frame: "base_link" fixed_frame: "odom" dock_backwards: false - dock_prestaging_tolerance: 0.5 + dock_prestaging_tolerance: 0.85 # Types of docks dock_plugins: ['simple_charging_dock'] @@ -421,38 +421,127 @@ docking_server: plugin: 'opennav_docking::SimpleChargingDock' docking_threshold: 0.05 staging_x_offset: -0.7 - use_external_detection_pose: true - use_battery_status: false # true - use_stall_detection: false # true - - external_detection_timeout: 1.0 - external_detection_translation_x: -0.18 - external_detection_translation_y: 0.0 - external_detection_rotation_roll: -1.57 - external_detection_rotation_pitch: -1.57 - external_detection_rotation_yaw: 0.0 + use_external_detection_pose: false + use_battery_status: false + use_stall_detection: false filter_coef: 0.1 - # Dock instances - # The following example illustrates configuring dock instances. - # docks: ['home_dock'] # Input your docks here - # home_dock: - # type: 'simple_charging_dock' - # frame: map - # pose: [0.0, 0.0, 0.0] + # Dock instances for simulation factory conveyors + docks: [ + 'dock_conveyor_r1_c1', 'conveyor_r1_c1_dock', 'conveyor_r1_c1', 'conveyor_r1_c1_final_dock', + 'dock_conveyor_r1_c2', 'conveyor_r1_c2_dock', 'conveyor_r1_c2', 'conveyor_r1_c2_final_dock', + 'dock_conveyor_r1_c3', 'conveyor_r1_c3_dock', 'conveyor_r1_c3', 'conveyor_r1_c3_final_dock', + 'dock_conveyor_r2_c1', 'conveyor_r2_c1_dock', 'conveyor_r2_c1', 'conveyor_r2_c1_final_dock', + 'dock_conveyor_r2_c2', 'conveyor_r2_c2_dock', 'conveyor_r2_c2', 'conveyor_r2_c2_final_dock', + 'dock_conveyor_r2_c3', 'conveyor_r2_c3_dock', 'conveyor_r2_c3', 'conveyor_r2_c3_final_dock', + ] + dock_conveyor_r1_c1: + type: 'simple_charging_dock' + frame: 'map' + pose: [0.0, 0.95, -1.57079632679] + conveyor_r1_c1_dock: + type: 'simple_charging_dock' + frame: 'map' + pose: [0.0, 0.95, -1.57079632679] + conveyor_r1_c1: + type: 'simple_charging_dock' + frame: 'map' + pose: [0.0, 0.95, -1.57079632679] + conveyor_r1_c1_final_dock: + type: 'simple_charging_dock' + frame: 'map' + pose: [0.0, 0.95, -1.57079632679] + dock_conveyor_r1_c2: + type: 'simple_charging_dock' + frame: 'map' + pose: [2.5, 0.95, -1.57079632679] + conveyor_r1_c2_dock: + type: 'simple_charging_dock' + frame: 'map' + pose: [2.5, 0.95, -1.57079632679] + conveyor_r1_c2: + type: 'simple_charging_dock' + frame: 'map' + pose: [2.5, 0.95, -1.57079632679] + conveyor_r1_c2_final_dock: + type: 'simple_charging_dock' + frame: 'map' + pose: [2.5, 0.95, -1.57079632679] + dock_conveyor_r1_c3: + type: 'simple_charging_dock' + frame: 'map' + pose: [5.0, 0.95, -1.57079632679] + conveyor_r1_c3_dock: + type: 'simple_charging_dock' + frame: 'map' + pose: [5.0, 0.95, -1.57079632679] + conveyor_r1_c3: + type: 'simple_charging_dock' + frame: 'map' + pose: [5.0, 0.95, -1.57079632679] + conveyor_r1_c3_final_dock: + type: 'simple_charging_dock' + frame: 'map' + pose: [5.0, 0.95, -1.57079632679] + dock_conveyor_r2_c1: + type: 'simple_charging_dock' + frame: 'map' + pose: [0.0, 4.05, 1.57079632679] + conveyor_r2_c1_dock: + type: 'simple_charging_dock' + frame: 'map' + pose: [0.0, 4.05, 1.57079632679] + conveyor_r2_c1: + type: 'simple_charging_dock' + frame: 'map' + pose: [0.0, 4.05, 1.57079632679] + conveyor_r2_c1_final_dock: + type: 'simple_charging_dock' + frame: 'map' + pose: [0.0, 4.05, 1.57079632679] + dock_conveyor_r2_c2: + type: 'simple_charging_dock' + frame: 'map' + pose: [2.5, 4.05, 1.57079632679] + conveyor_r2_c2_dock: + type: 'simple_charging_dock' + frame: 'map' + pose: [2.5, 4.05, 1.57079632679] + conveyor_r2_c2: + type: 'simple_charging_dock' + frame: 'map' + pose: [2.5, 4.05, 1.57079632679] + conveyor_r2_c2_final_dock: + type: 'simple_charging_dock' + frame: 'map' + pose: [2.5, 4.05, 1.57079632679] + dock_conveyor_r2_c3: + type: 'simple_charging_dock' + frame: 'map' + pose: [5.0, 4.05, 1.57079632679] + conveyor_r2_c3_dock: + type: 'simple_charging_dock' + frame: 'map' + pose: [5.0, 4.05, 1.57079632679] + conveyor_r2_c3: + type: 'simple_charging_dock' + frame: 'map' + pose: [5.0, 4.05, 1.57079632679] + conveyor_r2_c3_final_dock: + type: 'simple_charging_dock' + frame: 'map' + pose: [5.0, 4.05, 1.57079632679] controller: k_phi: 3.0 k_delta: 2.0 v_linear_min: 0.15 v_linear_max: 0.15 - use_collision_detection: true - costmap_topic: "local_costmap/costmap_raw" - footprint_topic: "local_costmap/published_footprint" - transform_tolerance: 0.1 - projection_time: 5.0 - simulation_step: 0.1 - dock_collision_threshold: 0.3 + v_angular_max: 0.75 + slowdown_radius: 0.25 + use_collision_detection: false + dock_collision_threshold: 0.8 + transform_tolerance: 0.5 loopback_simulator: ros__parameters: 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 af64085..b297d8a 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, PlanError, PlanId, TrafficDependency, Waypoint}, + msg::{ + ControlPoint, Curve, Destination, Plan, PlanError, PlanId, Region, TargetRegion, + TrafficDependency, Trajectory, Waypoint, + }, }, }; use std::{ @@ -26,6 +29,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 +42,7 @@ pub struct PlanSuccess { pub goals: HashMap, pub robot_ids: Vec, pub active_plan: MapfResult, + pub target_actions: HashMap, } pub enum PlanResult { @@ -59,6 +66,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 +75,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 +102,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 +120,57 @@ 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 already present, ensure primary region matches the exact 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() + }); + } 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) + 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, @@ -112,6 +183,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"); @@ -163,6 +236,7 @@ impl PlanServer

{ traffic_dependencies, goals, robot_ids, + target_actions, .. } = success; @@ -199,15 +273,22 @@ impl PlanServer

{ ); continue; }; - let plan = Self::to_plan_msg( + let target_action = target_actions.get(robot_id).map(|s| s.as_str()); + let mut plan = Self::to_plan_msg( agent_idx, traj, plan_id, &traffic_dependencies, &robot_ids, &self.active_plan_ids, + 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); } @@ -222,8 +303,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!( @@ -343,6 +424,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) { @@ -421,6 +503,7 @@ impl PlanServer

{ goals, robot_ids, active_plan: mapf_result, + target_actions: target_actions_clone, })); }); } @@ -432,6 +515,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(); @@ -448,6 +532,56 @@ impl PlanServer

{ }); } + if let Some(action) = target_action { + if let Some(last_wp) = waypoints.last_mut() { + last_wp.arrival_action = action.to_string(); + } + } + + // 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, @@ -518,13 +652,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(), @@ -584,7 +772,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/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_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..f40f085 --- /dev/null +++ b/path_server/rmf_path_server/tests/test_nav_graph.rs @@ -0,0 +1,284 @@ +// 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 _ = discovery_pub.publish(&discovery_msg); + let _ = odom_pub.publish(&odom_msg); + let _ = dest_pub.publish(&dest_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]); + + // Verify departure trajectory is populated + assert!( + !last_wp.departure_trajectory.is_empty(), + "Expected departure_trajectory to be populated on docking waypoint" + ); + let dep_traj = &last_wp.departure_trajectory[0]; + assert_eq!(dep_traj.curve.control_points.len(), 2); + assert_eq!(dep_traj.curve.control_points[0].position, [0.0, 1.5]); + assert_eq!(dep_traj.curve.control_points[1].position, [0.0, 2.0]); + 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'])