Add Open-RMF CCBS based planner as an option

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,

path_server/rmf_path_server/src/main.rs

@@ -13,15 +13,38 @@
 // 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<dyn std::error::Error>> {
     let context = Context::default_from_env().unwrap();
     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<String> = 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<dyn MapfPlanner> = 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(),

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<dyn MapfPlanner> {
+    fn plan(
+        &self,
+        starts: &HashMap<String, Odometry>,
+        goals: &HashMap<String, Destination>,
+        footprints: &HashMap<String, Arc<dyn mapf_post::shape::Shape>>,
+        robot_ids: &[String],
+        map: &Map,
+        cancellation: Arc<AtomicBool>,
+    ) -> Result<Vec<Vec<Isometry2<f32>>>, Box<dyn std::error::Error>> {
+        self.as_ref()
+            .plan(starts, goals, footprints, robot_ids, map, cancellation)
+    }
+}
+
+impl MapfPlanner for Arc<dyn MapfPlanner> {
+    fn plan(
+        &self,
+        starts: &HashMap<String, Odometry>,
+        goals: &HashMap<String, Destination>,
+        footprints: &HashMap<String, Arc<dyn mapf_post::shape::Shape>>,
+        robot_ids: &[String],
+        map: &Map,
+        cancellation: Arc<AtomicBool>,
+    ) -> Result<Vec<Vec<Isometry2<f32>>>, Box<dyn std::error::Error>> {
+        self.as_ref()
+            .plan(starts, goals, footprints, robot_ids, map, cancellation)
+    }
+}
+
+#[derive(Clone)]
+pub struct CcbsPlanner {
+    pub queue_length_limit: Option<usize>,
+}
+
+impl Default for CcbsPlanner {
+    fn default() -> Self {
+        Self {
+            queue_length_limit: Some(100_000),
+        }
+    }
+}
+
+impl CcbsPlanner {
+    pub fn new(queue_length_limit: Option<usize>) -> Self {
+        Self { queue_length_limit }
+    }
+}
+
+impl MapfPlanner for CcbsPlanner {
+    fn plan(
+        &self,
+        starts: &HashMap<String, Odometry>,
+        goals: &HashMap<String, Destination>,
+        _footprints: &HashMap<String, Arc<dyn mapf_post::shape::Shape>>,
+        robot_ids: &[String],
+        map: &Map,
+        _cancellation: Arc<AtomicBool>,
+    ) -> Result<Vec<Vec<Isometry2<f32>>>, Box<dyn std::error::Error>> {
+        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<i64, Vec<i64>> = 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::<dyn std::error::Error>::from(format!(
+                    "Missing start odometry for agent {}",
+                    id
+                ))
+            })?;
+            let dest = goals.get(id).ok_or_else(|| {
+                Box::<dyn std::error::Error>::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::<dyn std::error::Error>::from(format!(
+                        "Destination for agent {} has invalid region points",
+                        id
+                    )));
+                }
+            } else {
+                return Err(Box::<dyn std::error::Error>::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::<dyn std::error::Error>::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::<dyn std::error::Error>::from(format!(
+                        "Failed to find name map entry for agent {}",
+                        id
+                    ))
+                })?;
+
+            let proposal = solution_node.proposals.get(&internal_idx).ok_or_else(|| {
+                Box::<dyn std::error::Error>::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)
+    }
+}

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
+    );
+}