Add Open-RMF CCBS based planner as an option - #40
Conversation
|
@SamuelFoo to review |
SamuelFoo
left a comment
SamuelFoo
left a comment
There was a problem hiding this comment.
Hi, left some comments :)
| 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); |
There was a problem hiding this comment.
I think there might be a half-cell offset here.
Cell::from_point seems to floor on the way in, but the trajectory comes back out of AccessibilityGraph::vertex via Cell::center_point i.e. (idx + 0.5) * cell_size, so the round trip may not be symmetric. This causes an error of at most cell_size / 2 per axis (e.g., with integer coordinates +0.5 * cell_size in x and y).
| start: [start_cell.x, start_cell.y], | ||
| yaw, | ||
| goal: [goal_cell.x, goal_cell.y], | ||
| radius: 0.45, |
There was a problem hiding this comment.
Could we take the radius from _footprints rather than hardcoding 0.45?
| let cell_size = if map.grid.info.resolution > 0.0 { | ||
| map.grid.info.resolution as f64 | ||
| } else { | ||
| 1.0 | ||
| }; |
There was a problem hiding this comment.
I think cell_size = map.grid.info.resolution might be a problem on the real maps.
The demo grid is 1.0 m/cell, but the maps in this repo are 0.03 m (warehouse.yaml) and 0.05 m (depot.yaml).
It also looks like Accessibility::new(grid, 0.45) would compute cell_shift = ceil(0.45/0.03 + 0.5) = 16, which I think inflates obstacles by 16 cells in every direction.
Would it make sense to decouple the planning resolution from the map resolution?
| 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); | ||
| } | ||
| } | ||
| } |
There was a problem hiding this comment.
Since only occupied cells reach the SparseGrid, I think everything outside the map rectangle may read as free.
Scenario.occupancy doesn't seem to carry an extent, and negotiate builds SparseGrid::new(cell_size) from the occupied cells alone (mapf/src/negotiation/mod.rs:104-113). For demo_grid.png, which has no border wall, the paths seem able to leave the mapped area.
Maybe we can mark the ring of cells just outside the grid as occupied?
| )) | ||
| })?; | ||
|
|
||
| for wp in proposal.meta.trajectory.iter() { |
There was a problem hiding this comment.
It looks like wp.time gets discarded, and the pipeline then re-times the path uniformly. Is it intentional to not follow the CCBS output schedule?
| rclrs::log!(node.logger(), "Using CCBS planner"); | ||
| Box::new(CcbsPlanner::default()) | ||
| } | ||
| "pibt-grid-world" | _ => { |
There was a problem hiding this comment.
Nit: Typos (e.g., ccbss) may cause PIBT to be selected.
Declare startup parameters for the PIBT grid resolution and global robot footprint. Apply the configured resolution to mapped and mapless planning while preserving the existing defaults. Signed-off-by: SamuelFoo <fooenzesamuel@gmail.com>
Remove the global robot footprint parameter while its interface is still under discussion. Preserve the existing 0.49 m fallback behavior. Signed-off-by: SamuelFoo <fooenzesamuel@gmail.com>
Keep the default grid resolution private to the library crate. Read the executable's parameter default from PibtPlanner instead of exporting the constant. Signed-off-by: SamuelFoo <fooenzesamuel@gmail.com>
Document the planning_grid_resolution startup parameter and align its help text with the repository's terminology. Signed-off-by: SamuelFoo <fooenzesamuel@gmail.com>
Signed-off-by: SamuelFoo <fooenzesamuel@gmail.com>
2c23b1c to
9de1aa3
Compare
… with planning grid resolution
- Add CcbsPlanner implementing MapfPlanner using mapf crate
- Implement MapfPlanner for Box<dyn MapfPlanner> and Arc<dyn MapfPlanner>
- Reconcile main.rs to support both planner parameter ('pibt-grid-world' / 'ccbs') and planning_grid_resolution parameter
- Add fine-resolution demo grid (0.1m) and updated launch scripts in rmf_path_server_demo
- Add test_ccbs_planner integration test suite
Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
9de1aa3 to
5706be4
Compare
3 participants
New feature implementation
Implemented feature
Adds a planner based on https://github.com/open-rmf/mapf
Implementation description
You can now switch between PIBT/CCBS. This showcases the flexibility of the new architecture.
GenAI Use
We follow OSRA's policy on GenAI tools
Generated-by: