1use rust_robotics_core::{Obstacles, RoboticsError, RoboticsResult};
7
8#[derive(Debug, Clone)]
10pub struct Node {
11 pub x: i32,
12 pub y: i32,
13 pub cost: f64,
14 pub parent_index: Option<usize>,
15}
16
17impl Node {
18 pub fn new(x: i32, y: i32, cost: f64, parent_index: Option<usize>) -> Self {
19 Node {
20 x,
21 y,
22 cost,
23 parent_index,
24 }
25 }
26}
27
28pub fn get_motion_model_8() -> Vec<(i32, i32, f64)> {
30 vec![
31 (1, 0, 1.0),
32 (0, 1, 1.0),
33 (-1, 0, 1.0),
34 (0, -1, 1.0),
35 (-1, -1, std::f64::consts::SQRT_2),
36 (-1, 1, std::f64::consts::SQRT_2),
37 (1, -1, std::f64::consts::SQRT_2),
38 (1, 1, std::f64::consts::SQRT_2),
39 ]
40}
41
42pub fn get_motion_model_4() -> Vec<(i32, i32, f64)> {
44 vec![(1, 0, 1.0), (0, 1, 1.0), (-1, 0, 1.0), (0, -1, 1.0)]
45}
46
47#[derive(Debug, Clone)]
49pub struct GridMap {
50 pub resolution: f64,
51 pub robot_radius: f64,
52 pub min_x: f64,
53 pub min_y: f64,
54 pub max_x: f64,
55 pub max_y: f64,
56 pub x_width: i32,
57 pub y_width: i32,
58 pub obstacle_map: Vec<Vec<bool>>,
59}
60
61impl GridMap {
62 pub fn new(ox: &[f64], oy: &[f64], resolution: f64, robot_radius: f64) -> Self {
64 Self::try_new(ox, oy, resolution, robot_radius).expect(
65 "invalid grid map input: obstacle list must be non-empty, finite, and match lengths; resolution must be > 0; robot_radius must be >= 0",
66 )
67 }
68
69 pub fn try_new(
71 ox: &[f64],
72 oy: &[f64],
73 resolution: f64,
74 robot_radius: f64,
75 ) -> RoboticsResult<Self> {
76 Self::validate_inputs(ox, oy, resolution, robot_radius)?;
77
78 let min_x = ox.iter().fold(f64::INFINITY, |a, &b| a.min(b)).round();
79 let min_y = oy.iter().fold(f64::INFINITY, |a, &b| a.min(b)).round();
80 let max_x = ox.iter().fold(f64::NEG_INFINITY, |a, &b| a.max(b)).round();
81 let max_y = oy.iter().fold(f64::NEG_INFINITY, |a, &b| a.max(b)).round();
82
83 let x_width = ((max_x - min_x) / resolution).round() as i32;
84 let y_width = ((max_y - min_y) / resolution).round() as i32;
85
86 if x_width <= 0 || y_width <= 0 {
87 return Err(RoboticsError::InvalidParameter(
88 "obstacles must span a non-zero 2D area at the requested resolution".to_string(),
89 ));
90 }
91
92 let mut obstacle_map = vec![vec![false; y_width as usize]; x_width as usize];
94
95 let reach = (robot_radius / resolution).ceil() as i32 + 1;
101 for (&iox, &ioy) in ox.iter().zip(oy.iter()) {
102 let cx = ((iox - min_x) / resolution).round() as i32;
103 let cy = ((ioy - min_y) / resolution).round() as i32;
104 for ix in (cx - reach).max(0)..=(cx + reach).min(x_width - 1) {
105 let x = Self::calc_grid_position_static(ix, min_x, resolution);
106 for iy in (cy - reach).max(0)..=(cy + reach).min(y_width - 1) {
107 if obstacle_map[ix as usize][iy as usize] {
108 continue;
109 }
110 let y = Self::calc_grid_position_static(iy, min_y, resolution);
111 let d = ((iox - x).powi(2) + (ioy - y).powi(2)).sqrt();
112 if d <= robot_radius {
113 obstacle_map[ix as usize][iy as usize] = true;
114 }
115 }
116 }
117 }
118
119 Ok(GridMap {
120 resolution,
121 robot_radius,
122 min_x,
123 min_y,
124 max_x,
125 max_y,
126 x_width,
127 y_width,
128 obstacle_map,
129 })
130 }
131
132 pub fn from_obstacles(
134 obstacles: &Obstacles,
135 resolution: f64,
136 robot_radius: f64,
137 ) -> RoboticsResult<Self> {
138 let ox = obstacles.x_coords();
139 let oy = obstacles.y_coords();
140 Self::try_new(&ox, &oy, resolution, robot_radius)
141 }
142
143 pub fn calc_xy_index(&self, position: f64) -> i32 {
145 ((position - self.min_x) / self.resolution).round() as i32
146 }
147
148 pub fn calc_x_index(&self, x: f64) -> i32 {
150 ((x - self.min_x) / self.resolution).round() as i32
151 }
152
153 pub fn calc_y_index(&self, y: f64) -> i32 {
155 ((y - self.min_y) / self.resolution).round() as i32
156 }
157
158 pub fn calc_grid_position(&self, index: i32) -> f64 {
160 index as f64 * self.resolution + self.min_x
161 }
162
163 pub fn calc_x_position(&self, ix: i32) -> f64 {
165 ix as f64 * self.resolution + self.min_x
166 }
167
168 pub fn calc_y_position(&self, iy: i32) -> f64 {
170 iy as f64 * self.resolution + self.min_y
171 }
172
173 fn calc_grid_position_static(index: i32, min_position: f64, resolution: f64) -> f64 {
174 index as f64 * resolution + min_position
175 }
176
177 pub fn calc_grid_index(&self, node: &Node) -> i32 {
179 (node.y - self.min_y as i32) * self.x_width + (node.x - self.min_x as i32)
180 }
181
182 pub fn calc_index(&self, x: i32, y: i32) -> i32 {
184 (y - self.min_y as i32) * self.x_width + (x - self.min_x as i32)
185 }
186
187 pub fn verify_node(&self, node: &Node) -> bool {
189 self.is_valid(node.x, node.y)
190 }
191
192 pub fn is_valid(&self, x: i32, y: i32) -> bool {
194 let px = self.calc_x_position(x);
195 let py = self.calc_y_position(y);
196
197 if px < self.min_x || py < self.min_y || px >= self.max_x || py >= self.max_y {
198 return false;
199 }
200
201 if x < 0 || x >= self.x_width || y < 0 || y >= self.y_width {
203 return false;
204 }
205
206 !self.obstacle_map[x as usize][y as usize]
208 }
209
210 pub fn is_valid_step(&self, from_x: i32, from_y: i32, to_x: i32, to_y: i32) -> bool {
215 if !self.is_valid(from_x, from_y) || !self.is_valid(to_x, to_y) {
216 return false;
217 }
218
219 let dx = to_x - from_x;
220 let dy = to_y - from_y;
221
222 if dx.abs() > 1 || dy.abs() > 1 || (dx == 0 && dy == 0) {
223 return false;
224 }
225
226 if dx != 0 && dy != 0 {
227 self.is_valid(from_x + dx, from_y) && self.is_valid(from_x, from_y + dy)
228 } else {
229 true
230 }
231 }
232
233 pub fn is_valid_offset(&self, x: i32, y: i32, dx: i32, dy: i32) -> bool {
235 self.is_valid_step(x, y, x + dx, y + dy)
236 }
237
238 fn validate_inputs(
239 ox: &[f64],
240 oy: &[f64],
241 resolution: f64,
242 robot_radius: f64,
243 ) -> RoboticsResult<()> {
244 if ox.len() != oy.len() {
245 return Err(RoboticsError::InvalidParameter(format!(
246 "obstacle x/y coordinates must have matching lengths, got {} and {}",
247 ox.len(),
248 oy.len()
249 )));
250 }
251 if ox.is_empty() {
252 return Err(RoboticsError::InvalidParameter(
253 "grid planners require at least one obstacle point".to_string(),
254 ));
255 }
256 if !resolution.is_finite() || resolution <= 0.0 {
257 return Err(RoboticsError::InvalidParameter(format!(
258 "resolution must be positive and finite, got {}",
259 resolution
260 )));
261 }
262 if !robot_radius.is_finite() || robot_radius < 0.0 {
263 return Err(RoboticsError::InvalidParameter(format!(
264 "robot_radius must be non-negative and finite, got {}",
265 robot_radius
266 )));
267 }
268 if ox.iter().chain(oy.iter()).any(|value| !value.is_finite()) {
269 return Err(RoboticsError::InvalidParameter(
270 "obstacle coordinates must be finite".to_string(),
271 ));
272 }
273
274 Ok(())
275 }
276}
277
278pub type SearchNode = Node;
280
281#[cfg(test)]
282mod tests {
283 use super::*;
284 use rust_robotics_core::Point2D;
285
286 fn create_obstacles() -> Obstacles {
287 Obstacles::from_points(vec![
288 Point2D::new(0.0, 0.0),
289 Point2D::new(10.0, 0.0),
290 Point2D::new(0.0, 10.0),
291 Point2D::new(10.0, 10.0),
292 ])
293 }
294
295 #[test]
296 fn test_try_new_rejects_empty_obstacles() {
297 let err = GridMap::try_new(&[], &[], 1.0, 0.5).unwrap_err();
298 assert!(matches!(err, RoboticsError::InvalidParameter(_)));
299 }
300
301 #[test]
302 fn test_try_new_rejects_invalid_resolution() {
303 let obstacles = create_obstacles();
304 let ox = obstacles.x_coords();
305 let oy = obstacles.y_coords();
306
307 let err = GridMap::try_new(&ox, &oy, 0.0, 0.5).unwrap_err();
308 assert!(matches!(err, RoboticsError::InvalidParameter(_)));
309 }
310
311 #[test]
312 fn test_from_obstacles_builds_valid_map() {
313 let obstacles = create_obstacles();
314 let grid_map = GridMap::from_obstacles(&obstacles, 1.0, 0.5).unwrap();
315
316 assert_eq!(grid_map.min_x, 0.0);
317 assert_eq!(grid_map.min_y, 0.0);
318 assert_eq!(grid_map.x_width, 10);
319 assert_eq!(grid_map.y_width, 10);
320 }
321
322 #[test]
323 fn test_is_valid_step_blocks_corner_cutting() {
324 let open_obstacles = Obstacles::from_points(vec![
325 Point2D::new(0.0, 0.0),
326 Point2D::new(1.0, 0.0),
327 Point2D::new(2.0, 0.0),
328 Point2D::new(3.0, 0.0),
329 Point2D::new(0.0, 1.0),
330 Point2D::new(3.0, 1.0),
331 Point2D::new(0.0, 2.0),
332 Point2D::new(3.0, 2.0),
333 Point2D::new(0.0, 3.0),
334 Point2D::new(1.0, 3.0),
335 Point2D::new(2.0, 3.0),
336 Point2D::new(3.0, 3.0),
337 ]);
338 let open_grid_map = GridMap::from_obstacles(&open_obstacles, 1.0, 0.0).unwrap();
339
340 assert!(open_grid_map.is_valid_step(1, 1, 2, 1));
341 assert!(open_grid_map.is_valid_step(1, 1, 2, 2));
342
343 let blocked_obstacles = Obstacles::from_points(vec![
344 Point2D::new(0.0, 0.0),
345 Point2D::new(1.0, 0.0),
346 Point2D::new(2.0, 0.0),
347 Point2D::new(3.0, 0.0),
348 Point2D::new(0.0, 1.0),
349 Point2D::new(3.0, 1.0),
350 Point2D::new(0.0, 2.0),
351 Point2D::new(3.0, 2.0),
352 Point2D::new(0.0, 3.0),
353 Point2D::new(1.0, 3.0),
354 Point2D::new(2.0, 3.0),
355 Point2D::new(3.0, 3.0),
356 Point2D::new(1.0, 2.0),
357 Point2D::new(2.0, 1.0),
358 ]);
359 let grid_map = GridMap::from_obstacles(&blocked_obstacles, 1.0, 0.0).unwrap();
360
361 assert!(grid_map.is_valid(1, 1));
362 assert!(grid_map.is_valid(2, 2));
363 assert!(!grid_map.is_valid_step(1, 1, 2, 2));
364 }
365
366 #[test]
367 fn inflation_matches_brute_force_check() {
368 let ox = [0.0, 10.0, 3.3, 7.1, 5.0, 0.0, 10.0, 2.25];
369 let oy = [0.0, 0.0, 4.4, 2.9, 8.8, 10.0, 10.0, 6.75];
370 for (resolution, robot_radius) in [(1.0, 0.5), (0.5, 1.2), (0.25, 0.8), (0.3, 0.0)] {
371 let grid = GridMap::try_new(&ox, &oy, resolution, robot_radius).unwrap();
372 for ix in 0..grid.x_width {
373 let x = grid.calc_x_position(ix);
374 for iy in 0..grid.y_width {
375 let y = grid.calc_y_position(iy);
376 let expected = ox.iter().zip(&oy).any(|(&px, &py)| {
377 ((px - x).powi(2) + (py - y).powi(2)).sqrt() <= robot_radius
378 });
379 assert_eq!(
380 grid.obstacle_map[ix as usize][iy as usize], expected,
381 "cell ({ix}, {iy}) at resolution {resolution}, radius {robot_radius}"
382 );
383 }
384 }
385 }
386 }
387}