diff --git a/path_server/rmf_path_server/src/lib.rs b/path_server/rmf_path_server/src/lib.rs index abaab44..9497d9b 100644 --- a/path_server/rmf_path_server/src/lib.rs +++ b/path_server/rmf_path_server/src/lib.rs @@ -27,7 +27,7 @@ use std::{ }; pub mod planner; -pub use planner::{Map, MapfPlanner, MockPlanner, PibtPlanner}; +pub use planner::{CcbsPlanner, Map, MapfPlanner, MockPlanner, PibtPlanner}; pub struct PlanSuccess { pub session_id: u64, diff --git a/path_server/rmf_path_server/src/main.rs b/path_server/rmf_path_server/src/main.rs index d0e0a3f..af82b32 100644 --- a/path_server/rmf_path_server/src/main.rs +++ b/path_server/rmf_path_server/src/main.rs @@ -13,7 +13,8 @@ // limitations under the License. use rclrs::{Context, CreateBasicExecutor, SpinOptions}; -use rmf_path_server::{start_path_server, PibtPlanner}; +use rmf_path_server::{start_path_server, CcbsPlanner, MapfPlanner, PibtPlanner}; +use std::env; use std::sync::Arc; fn main() -> Result<(), Box> { @@ -21,7 +22,29 @@ fn main() -> Result<(), Box> { let mut executor = context.create_basic_executor(); let node = Arc::new(executor.create_node("path_server")?); - let _path_server_guard = start_path_server(Arc::clone(&node), PibtPlanner::default())?; + let args: Vec = env::args().collect(); + let planner_name = args + .iter() + .position(|arg| arg == "--planner") + .and_then(|i| args.get(i + 1).cloned()) + .or_else(|| { + args.iter() + .find_map(|arg| arg.strip_prefix("--planner=").map(String::from)) + }) + .unwrap_or_else(|| "pibt-grid-world".to_string()); + + let planner: Box = match planner_name.to_lowercase().as_str() { + "ccbs" => { + rclrs::log!(node.logger(), "Using CCBS planner"); + Box::new(CcbsPlanner::default()) + } + "pibt-grid-world" | _ => { + rclrs::log!(node.logger(), "Using PIBT grid world planner"); + Box::new(PibtPlanner::default()) + } + }; + + let _path_server_guard = start_path_server(Arc::clone(&node), planner)?; rclrs::log!( node.logger(), diff --git a/path_server/rmf_path_server/src/planner.rs b/path_server/rmf_path_server/src/planner.rs index 8b2130a..e84b9aa 100644 --- a/path_server/rmf_path_server/src/planner.rs +++ b/path_server/rmf_path_server/src/planner.rs @@ -13,6 +13,9 @@ // limitations under the License. use hetpibt::external_tracks_pibt::PiBTWithExternalTracks; +use mapf::graph::occupancy::Cell; +use mapf::motion::se2::Point; +use mapf::negotiation::{negotiate, Agent, Scenario}; use mapf_post::na::Isometry2; use ros_env::nav_msgs::msg::{OccupancyGrid, Odometry}; use ros_env::rmf_prototype_msgs::msg::Destination; @@ -249,3 +252,211 @@ impl MapfPlanner for PibtPlanner { Ok(trajectories) } } + +impl MapfPlanner for Box { + fn plan( + &self, + starts: &HashMap, + goals: &HashMap, + footprints: &HashMap>, + robot_ids: &[String], + map: &Map, + cancellation: Arc, + ) -> Result>>, Box> { + self.as_ref() + .plan(starts, goals, footprints, robot_ids, map, cancellation) + } +} + +impl MapfPlanner for Arc { + fn plan( + &self, + starts: &HashMap, + goals: &HashMap, + footprints: &HashMap>, + robot_ids: &[String], + map: &Map, + cancellation: Arc, + ) -> Result>>, Box> { + self.as_ref() + .plan(starts, goals, footprints, robot_ids, map, cancellation) + } +} + +#[derive(Clone)] +pub struct CcbsPlanner { + pub queue_length_limit: Option, +} + +impl Default for CcbsPlanner { + fn default() -> Self { + Self { + queue_length_limit: Some(100_000), + } + } +} + +impl CcbsPlanner { + pub fn new(queue_length_limit: Option) -> Self { + Self { queue_length_limit } + } +} + +impl MapfPlanner for CcbsPlanner { + fn plan( + &self, + starts: &HashMap, + goals: &HashMap, + _footprints: &HashMap>, + robot_ids: &[String], + map: &Map, + _cancellation: Arc, + ) -> Result>>, Box> { + if robot_ids.is_empty() { + return Ok(Vec::new()); + } + + let cell_size = if map.grid.info.resolution > 0.0 { + map.grid.info.resolution as f64 + } else { + 1.0 + }; + + let origin_x = map.grid.info.origin.position.x as f64; + let origin_y = map.grid.info.origin.position.y as f64; + + let mut occupancy: HashMap> = HashMap::new(); + let w = map.grid.info.width as usize; + let h = map.grid.info.height as usize; + if w > 0 && h > 0 { + for x in 0..w { + for y in 0..h { + let ros_val = map.grid.data[y * w + x]; + if ros_val > 50 || ros_val == -1 { + occupancy.entry(y as i64).or_default().push(x as i64); + } + } + } + for row in occupancy.values_mut() { + row.sort_unstable(); + } + } + + let mut agents = std::collections::BTreeMap::new(); + + for id in robot_ids { + let odom = starts.get(id).ok_or_else(|| { + Box::::from(format!( + "Missing start odometry for agent {}", + id + )) + })?; + let dest = goals.get(id).ok_or_else(|| { + Box::::from(format!( + "Missing goal destination for agent {}", + id + )) + })?; + + let sx_world = odom.pose.pose.position.x as f64; + let sy_world = odom.pose.pose.position.y as f64; + let sx_local = sx_world - origin_x; + let sy_local = sy_world - origin_y; + + let q = &odom.pose.pose.orientation; + let yaw = 2.0 * f64::atan2(q.z as f64, q.w as f64); + + let start_cell = Cell::from_point(Point::new(sx_local, sy_local), cell_size); + + let gx_world: f64; + let gy_world: f64; + + if let Some(region) = dest.constraints.regions.first() { + if region.region.points.len() >= 2 { + gx_world = region.region.points[0] as f64; + gy_world = region.region.points[1] as f64; + } else { + return Err(Box::::from(format!( + "Destination for agent {} has invalid region points", + id + ))); + } + } else { + return Err(Box::::from(format!( + "Destination for agent {} has no target regions", + id + ))); + } + + let gx_local = gx_world - origin_x; + let gy_local = gy_world - origin_y; + let goal_cell = Cell::from_point(Point::new(gx_local, gy_local), cell_size); + + agents.insert( + id.clone(), + Agent { + start: [start_cell.x, start_cell.y], + yaw, + goal: [goal_cell.x, goal_cell.y], + radius: 0.45, + speed: 0.75, + spin: 60.0_f64.to_radians(), + }, + ); + } + + let scenario = Scenario { + agents, + obstacles: Vec::new(), + occupancy, + cell_size, + camera_bounds: None, + }; + + let (solution_node, _node_history, name_map) = + match negotiate(&scenario, self.queue_length_limit) { + Ok(solution) => solution, + Err(err) => { + return Err(Box::::from(format!( + "CCBS negotiation failed: {:?}", + err + ))); + } + }; + + let mut trajectories = vec![Vec::new(); robot_ids.len()]; + + for (agent_idx, id) in robot_ids.iter().enumerate() { + let internal_idx = name_map + .iter() + .find(|(_, name)| *name == id) + .map(|(idx, _)| *idx) + .ok_or_else(|| { + Box::::from(format!( + "Failed to find name map entry for agent {}", + id + )) + })?; + + let proposal = solution_node.proposals.get(&internal_idx).ok_or_else(|| { + Box::::from(format!( + "Missing proposal solution for agent {}", + id + )) + })?; + + for wp in proposal.meta.trajectory.iter() { + let world_x = (wp.position.translation.x + origin_x) as f32; + let world_y = (wp.position.translation.y + origin_y) as f32; + let angle = wp.position.rotation.angle() as f32; + + trajectories[agent_idx].push(Isometry2::new( + mapf_post::na::Vector2::new(world_x, world_y), + angle, + )); + } + } + + Ok(trajectories) + } +} diff --git a/path_server/rmf_path_server/tests/test_ccbs_planner.rs b/path_server/rmf_path_server/tests/test_ccbs_planner.rs new file mode 100644 index 0000000..d2122fe --- /dev/null +++ b/path_server/rmf_path_server/tests/test_ccbs_planner.rs @@ -0,0 +1,99 @@ +// 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_path_server::planner::{CcbsPlanner, Map, MapfPlanner}; +use ros_env::nav_msgs::msg::{OccupancyGrid, Odometry}; +use ros_env::rmf_prototype_msgs::msg::Destination; +use std::collections::HashMap; +use std::sync::atomic::AtomicBool; +use std::sync::Arc; + +#[test] +fn test_ccbs_planner_with_map() { + let mut occupancy_grid = OccupancyGrid::default(); + occupancy_grid.info.resolution = 1.0; + occupancy_grid.info.width = 10; + occupancy_grid.info.height = 10; + occupancy_grid.info.origin.position.x = 0.0; + occupancy_grid.info.origin.position.y = 0.0; + occupancy_grid.data = vec![0; 100]; + + // Put obstacle at (5,5) -> index = 5 * 10 + 5 = 55 + occupancy_grid.data[55] = 100; // 100 is occupied + + let map = Map { + grid: occupancy_grid, + }; + + let mut starts = HashMap::new(); + let mut odom = Odometry::default(); + odom.pose.pose.position.x = 0.0; + odom.pose.pose.position.y = 5.0; + starts.insert("robot1".to_string(), odom); + + let mut goals = HashMap::new(); + let mut dest = Destination::default(); + dest.constraints + .regions + .push(ros_env::rmf_prototype_msgs::msg::TargetRegion { + region: ros_env::rmf_prototype_msgs::msg::Region { + points: vec![9.0, 5.0], + ..Default::default() + }, + ..Default::default() + }); + goals.insert("robot1".to_string(), dest); + + let footprints = HashMap::new(); + let robot_ids = vec!["robot1".to_string()]; + let planner = CcbsPlanner::default(); + + let result = planner.plan( + &starts, + &goals, + &footprints, + &robot_ids, + &map, + Arc::new(AtomicBool::new(false)), + ); + + assert!(result.is_ok(), "Planning failed: {:?}", result.err()); + let trajectories = result.unwrap(); + assert_eq!(trajectories.len(), 1); + + let path = &trajectories[0]; + assert!(!path.is_empty(), "Path is empty"); + + println!("Generated CCBS path:"); + for (i, pose) in path.iter().enumerate() { + let x = pose.translation.x; + let y = pose.translation.y; + println!(" {}: ({}, {})", i, x, y); + } + + // Verify that it reached near the goal (9.0, 5.0) + let last_pose = path.last().unwrap(); + let lx = last_pose.translation.x; + let ly = last_pose.translation.y; + assert!( + (lx - 9.0).abs() < 1.0, + "Last pose x is {}, expected ~9.0", + lx + ); + assert!( + (ly - 5.0).abs() < 1.0, + "Last pose y is {}, expected ~5.0", + ly + ); +}