Skip to content

Commit 51be638

Browse files
arjo129arjo129
committed
feat(path_server): Use Map/OccupancyGrid in PIBT planning
Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
1 parent 3139e25 commit 51be638

2 files changed

Lines changed: 188 additions & 58 deletions

File tree

‎path_server/rmf_path_server/src/planner.rs‎

Lines changed: 79 additions & 58 deletions
Original file line numberDiff line numberDiff line change
@@ -86,75 +86,96 @@ impl MapfPlanner for PibtPlanner {
8686
goals: &HashMap<String, Destination>,
8787
_footprints: &HashMap<String, Arc<dyn mapf_post::shape::Shape>>,
8888
robot_ids: &[String],
89-
_map: &Map,
89+
map: &Map,
9090
_cancellation: Arc<AtomicBool>,
9191
) -> Result<Vec<Vec<Isometry2<f32>>>, Box<dyn std::error::Error>> {
9292
if robot_ids.is_empty() {
9393
return Ok(Vec::new());
9494
}
9595

96-
let mut min_x = f32::MAX;
97-
let mut min_y = f32::MAX;
98-
let mut max_x = f32::MIN;
99-
let mut max_y = f32::MIN;
100-
101-
for id in robot_ids {
102-
if let Some(odom) = starts.get(id) {
103-
let x = odom.pose.pose.position.x as f32;
104-
let y = odom.pose.pose.position.y as f32;
105-
if x < min_x {
106-
min_x = x;
107-
}
108-
if y < min_y {
109-
min_y = y;
110-
}
111-
if x > max_x {
112-
max_x = x;
113-
}
114-
if y > max_y {
115-
max_y = y;
96+
let use_map =
97+
map.grid.info.width > 0 && map.grid.info.height > 0 && map.grid.info.resolution > 0.0;
98+
99+
let (width, height, resolution, offset_x, offset_y, grid) = if use_map {
100+
let w = map.grid.info.width as usize;
101+
let h = map.grid.info.height as usize;
102+
let r = map.grid.info.resolution;
103+
let ox = map.grid.info.origin.position.x as f32;
104+
let oy = map.grid.info.origin.position.y as f32;
105+
106+
let mut g = vec![vec![0; h]; w];
107+
for x in 0..w {
108+
for y in 0..h {
109+
let ros_val = map.grid.data[y * w + x];
110+
g[x][y] = if ros_val > 50 || ros_val == -1 { 1 } else { 0 };
116111
}
117112
}
118-
if let Some(dest) = goals.get(id) {
119-
if let Some(region) = dest.constraints.regions.first() {
120-
if region.region.points.len() >= 2 {
121-
let x = region.region.points[0];
122-
let y = region.region.points[1];
123-
if x < min_x {
124-
min_x = x;
125-
}
126-
if y < min_y {
127-
min_y = y;
128-
}
129-
if x > max_x {
130-
max_x = x;
131-
}
132-
if y > max_y {
133-
max_y = y;
113+
(w, h, r, ox, oy, g)
114+
} else {
115+
let mut min_x = f32::MAX;
116+
let mut min_y = f32::MAX;
117+
let mut max_x = f32::MIN;
118+
let mut max_y = f32::MIN;
119+
120+
for id in robot_ids {
121+
if let Some(odom) = starts.get(id) {
122+
let x = odom.pose.pose.position.x as f32;
123+
let y = odom.pose.pose.position.y as f32;
124+
if x < min_x {
125+
min_x = x;
126+
}
127+
if y < min_y {
128+
min_y = y;
129+
}
130+
if x > max_x {
131+
max_x = x;
132+
}
133+
if y > max_y {
134+
max_y = y;
135+
}
136+
}
137+
if let Some(dest) = goals.get(id) {
138+
if let Some(region) = dest.constraints.regions.first() {
139+
if region.region.points.len() >= 2 {
140+
let x = region.region.points[0];
141+
let y = region.region.points[1];
142+
if x < min_x {
143+
min_x = x;
144+
}
145+
if y < min_y {
146+
min_y = y;
147+
}
148+
if x > max_x {
149+
max_x = x;
150+
}
151+
if y > max_y {
152+
max_y = y;
153+
}
134154
}
135155
}
136156
}
137157
}
138-
}
139158

140-
if min_x == f32::MAX {
141-
min_x = 0.0;
142-
min_y = 0.0;
143-
max_x = 0.0;
144-
max_y = 0.0;
145-
}
159+
if min_x == f32::MAX {
160+
min_x = 0.0;
161+
min_y = 0.0;
162+
max_x = 0.0;
163+
max_y = 0.0;
164+
}
146165

147-
let padding = 10.0;
148-
let offset_x = min_x.floor() - padding;
149-
let offset_y = min_y.floor() - padding;
166+
let padding = 10.0;
167+
let ox = min_x.floor() - padding;
168+
let oy = min_y.floor() - padding;
150169

151-
let width = (max_x.ceil() - offset_x + padding) as usize;
152-
let height = (max_y.ceil() - offset_y + padding) as usize;
170+
let w = (max_x.ceil() - ox + padding) as usize;
171+
let h = (max_y.ceil() - oy + padding) as usize;
153172

154-
let width = width.max(1);
155-
let height = height.max(1);
173+
let w = w.max(1);
174+
let h = h.max(1);
156175

157-
let grid = vec![vec![0; height]; width];
176+
let g = vec![vec![0; h]; w];
177+
(w, h, 1.0f32, ox, oy, g)
178+
};
158179

159180
let mut grid_starts = Vec::new();
160181
let mut grid_ends = Vec::new();
@@ -173,8 +194,8 @@ impl MapfPlanner for PibtPlanner {
173194
))
174195
})?;
175196

176-
let sx = (odom.pose.pose.position.x as f32 - offset_x).round() as usize;
177-
let sy = (odom.pose.pose.position.y as f32 - offset_y).round() as usize;
197+
let sx = ((odom.pose.pose.position.x as f32 - offset_x) / resolution).round() as usize;
198+
let sy = ((odom.pose.pose.position.y as f32 - offset_y) / resolution).round() as usize;
178199

179200
let gx_f32;
180201
let gy_f32;
@@ -196,8 +217,8 @@ impl MapfPlanner for PibtPlanner {
196217
)));
197218
}
198219

199-
let gx = (gx_f32 - offset_x).round() as usize;
200-
let gy = (gy_f32 - offset_y).round() as usize;
220+
let gx = ((gx_f32 - offset_x) / resolution).round() as usize;
221+
let gy = ((gy_f32 - offset_y) / resolution).round() as usize;
201222

202223
let sx = sx.min(width - 1);
203224
let sy = sy.min(height - 1);
@@ -219,8 +240,8 @@ impl MapfPlanner for PibtPlanner {
219240
let mut trajectories = vec![Vec::new(); robot_ids.len()];
220241
for time_step in solved_paths {
221242
for (agent_idx, pos) in time_step.iter().enumerate() {
222-
let world_x = pos.0 as f32 + offset_x;
223-
let world_y = pos.1 as f32 + offset_y;
243+
let world_x = pos.0 as f32 * resolution + offset_x;
244+
let world_y = pos.1 as f32 * resolution + offset_y;
224245
trajectories[agent_idx].push(Isometry2::translation(world_x, world_y));
225246
}
226247
}

‎path_server/rmf_path_server/tests/test_pibt_with_map.rs‎

Lines changed: 109 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,109 @@
1+
// Copyright 2026 Open Source Robotics Foundation
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
use rmf_path_server::planner::{Map, MapfPlanner, PibtPlanner};
16+
use ros_env::nav_msgs::msg::{OccupancyGrid, Odometry};
17+
use ros_env::rmf_prototype_msgs::msg::Destination;
18+
use std::collections::HashMap;
19+
use std::sync::atomic::AtomicBool;
20+
use std::sync::Arc;
21+
22+
#[test]
23+
fn test_pibt_planner_with_obstacle() {
24+
let mut occupancy_grid = OccupancyGrid::default();
25+
occupancy_grid.info.resolution = 1.0;
26+
occupancy_grid.info.width = 10;
27+
occupancy_grid.info.height = 10;
28+
occupancy_grid.info.origin.position.x = 0.0;
29+
occupancy_grid.info.origin.position.y = 0.0;
30+
occupancy_grid.data = vec![0; 100];
31+
32+
// Put obstacle at (5,5) -> index = 5 * 10 + 5 = 55
33+
occupancy_grid.data[55] = 100; // 100 is occupied
34+
35+
let map = Map {
36+
grid: occupancy_grid,
37+
};
38+
39+
let mut starts = HashMap::new();
40+
let mut odom = Odometry::default();
41+
odom.pose.pose.position.x = 0.0;
42+
odom.pose.pose.position.y = 5.0;
43+
starts.insert("robot1".to_string(), odom);
44+
45+
let mut goals = HashMap::new();
46+
let mut dest = Destination::default();
47+
dest.constraints
48+
.regions
49+
.push(ros_env::rmf_prototype_msgs::msg::TargetRegion {
50+
region: ros_env::rmf_prototype_msgs::msg::Region {
51+
points: vec![9.0, 5.0],
52+
..Default::default()
53+
},
54+
..Default::default()
55+
});
56+
goals.insert("robot1".to_string(), dest);
57+
58+
let footprints = HashMap::new();
59+
let robot_ids = vec!["robot1".to_string()];
60+
let planner = PibtPlanner::new(100);
61+
62+
let result = planner.plan(
63+
&starts,
64+
&goals,
65+
&footprints,
66+
&robot_ids,
67+
&map,
68+
Arc::new(AtomicBool::new(false)),
69+
);
70+
71+
assert!(result.is_ok(), "Planning failed: {:?}", result.err());
72+
let trajectories = result.unwrap();
73+
assert_eq!(trajectories.len(), 1);
74+
75+
let path = &trajectories[0];
76+
assert!(!path.is_empty(), "Path is empty");
77+
78+
println!("Generated path:");
79+
for (i, pose) in path.iter().enumerate() {
80+
let x = pose.translation.x;
81+
let y = pose.translation.y;
82+
println!(" {}: ({}, {})", i, x, y);
83+
84+
// Verify that the path does not hit the obstacle at (5,5)
85+
// Since resolution is 1.0 and origin is 0,0, grid coords are same as world coords (approx)
86+
let dx = (x - 5.0).abs();
87+
let dy = (y - 5.0).abs();
88+
assert!(
89+
dx > 0.1 || dy > 0.1,
90+
"Path hits obstacle at (5,5) at step {}",
91+
i
92+
);
93+
}
94+
95+
// Verify that it reached the goal (9,5)
96+
let last_pose = path.last().unwrap();
97+
let lx = last_pose.translation.x;
98+
let ly = last_pose.translation.y;
99+
assert!(
100+
(lx - 9.0).abs() < 0.1,
101+
"Last pose x is {}, expected 9.0",
102+
lx
103+
);
104+
assert!(
105+
(ly - 5.0).abs() < 0.1,
106+
"Last pose y is {}, expected 5.0",
107+
ly
108+
);
109+
}

0 commit comments

Comments
 (0)