Skip to main content

rust_robotics_planning/
grid.rs

1// Grid map for path planning algorithms
2//
3// Provides occupancy grid representation and utility functions
4// for grid-based path planning algorithms like A*, Dijkstra, D* Lite.
5
6use rust_robotics_core::{Obstacles, RoboticsError, RoboticsResult};
7
8/// Node for grid-based path search
9#[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
28/// 8-connected motion model (dx, dy, cost)
29pub 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
42/// 4-connected motion model (dx, dy, cost)
43pub 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/// Occupancy grid map for path planning
48#[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    /// Create a new grid map from obstacle points
63    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    /// Create a validated grid map from obstacle points
70    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        // Initialize obstacle map
93        let mut obstacle_map = vec![vec![false; y_width as usize]; x_width as usize];
94
95        // Generate obstacle map with robot radius inflation. Each obstacle only
96        // marks the cells in its bounding box (one extra cell of margin so float
97        // rounding at the box edge cannot drop a cell); the distance test is the
98        // same as checking every cell against every obstacle, at a fraction of
99        // the cost for large maps.
100        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    /// Create a validated grid map from obstacle points
133    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    /// Convert world position to grid index (uses min_x as reference)
144    pub fn calc_xy_index(&self, position: f64) -> i32 {
145        ((position - self.min_x) / self.resolution).round() as i32
146    }
147
148    /// Convert world x position to grid index
149    pub fn calc_x_index(&self, x: f64) -> i32 {
150        ((x - self.min_x) / self.resolution).round() as i32
151    }
152
153    /// Convert world y position to grid index
154    pub fn calc_y_index(&self, y: f64) -> i32 {
155        ((y - self.min_y) / self.resolution).round() as i32
156    }
157
158    /// Convert grid index to world position
159    pub fn calc_grid_position(&self, index: i32) -> f64 {
160        index as f64 * self.resolution + self.min_x
161    }
162
163    /// Convert grid x index to world x position
164    pub fn calc_x_position(&self, ix: i32) -> f64 {
165        ix as f64 * self.resolution + self.min_x
166    }
167
168    /// Convert grid y index to world y position
169    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    /// Calculate unique grid index for a node
178    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    /// Calculate unique grid index from grid coordinates
183    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    /// Verify if a node is valid (within bounds and not in obstacle)
188    pub fn verify_node(&self, node: &Node) -> bool {
189        self.is_valid(node.x, node.y)
190    }
191
192    /// Check if grid coordinates are valid
193    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        // Check bounds for obstacle map access
202        if x < 0 || x >= self.x_width || y < 0 || y >= self.y_width {
203            return false;
204        }
205
206        // Collision check
207        !self.obstacle_map[x as usize][y as usize]
208    }
209
210    /// Check whether a single adjacent step is valid.
211    ///
212    /// Diagonal steps are rejected when either orthogonal side cell is
213    /// blocked, matching no-corner-cutting grid semantics.
214    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    /// Check whether an adjacent `(dx, dy)` move is valid from the given cell.
234    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
278// Alias for backward compatibility
279pub 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}