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
96        for ix in 0..x_width {
97            let x = Self::calc_grid_position_static(ix, min_x, resolution);
98            for iy in 0..y_width {
99                let y = Self::calc_grid_position_static(iy, min_y, resolution);
100
101                for (&iox, &ioy) in ox.iter().zip(oy.iter()) {
102                    let d = ((iox - x).powi(2) + (ioy - y).powi(2)).sqrt();
103                    if d <= robot_radius {
104                        obstacle_map[ix as usize][iy as usize] = true;
105                        break;
106                    }
107                }
108            }
109        }
110
111        Ok(GridMap {
112            resolution,
113            robot_radius,
114            min_x,
115            min_y,
116            max_x,
117            max_y,
118            x_width,
119            y_width,
120            obstacle_map,
121        })
122    }
123
124    /// Create a validated grid map from obstacle points
125    pub fn from_obstacles(
126        obstacles: &Obstacles,
127        resolution: f64,
128        robot_radius: f64,
129    ) -> RoboticsResult<Self> {
130        let ox = obstacles.x_coords();
131        let oy = obstacles.y_coords();
132        Self::try_new(&ox, &oy, resolution, robot_radius)
133    }
134
135    /// Convert world position to grid index (uses min_x as reference)
136    pub fn calc_xy_index(&self, position: f64) -> i32 {
137        ((position - self.min_x) / self.resolution).round() as i32
138    }
139
140    /// Convert world x position to grid index
141    pub fn calc_x_index(&self, x: f64) -> i32 {
142        ((x - self.min_x) / self.resolution).round() as i32
143    }
144
145    /// Convert world y position to grid index
146    pub fn calc_y_index(&self, y: f64) -> i32 {
147        ((y - self.min_y) / self.resolution).round() as i32
148    }
149
150    /// Convert grid index to world position
151    pub fn calc_grid_position(&self, index: i32) -> f64 {
152        index as f64 * self.resolution + self.min_x
153    }
154
155    /// Convert grid x index to world x position
156    pub fn calc_x_position(&self, ix: i32) -> f64 {
157        ix as f64 * self.resolution + self.min_x
158    }
159
160    /// Convert grid y index to world y position
161    pub fn calc_y_position(&self, iy: i32) -> f64 {
162        iy as f64 * self.resolution + self.min_y
163    }
164
165    fn calc_grid_position_static(index: i32, min_position: f64, resolution: f64) -> f64 {
166        index as f64 * resolution + min_position
167    }
168
169    /// Calculate unique grid index for a node
170    pub fn calc_grid_index(&self, node: &Node) -> i32 {
171        (node.y - self.min_y as i32) * self.x_width + (node.x - self.min_x as i32)
172    }
173
174    /// Calculate unique grid index from grid coordinates
175    pub fn calc_index(&self, x: i32, y: i32) -> i32 {
176        (y - self.min_y as i32) * self.x_width + (x - self.min_x as i32)
177    }
178
179    /// Verify if a node is valid (within bounds and not in obstacle)
180    pub fn verify_node(&self, node: &Node) -> bool {
181        self.is_valid(node.x, node.y)
182    }
183
184    /// Check if grid coordinates are valid
185    pub fn is_valid(&self, x: i32, y: i32) -> bool {
186        let px = self.calc_x_position(x);
187        let py = self.calc_y_position(y);
188
189        if px < self.min_x || py < self.min_y || px >= self.max_x || py >= self.max_y {
190            return false;
191        }
192
193        // Check bounds for obstacle map access
194        if x < 0 || x >= self.x_width || y < 0 || y >= self.y_width {
195            return false;
196        }
197
198        // Collision check
199        !self.obstacle_map[x as usize][y as usize]
200    }
201
202    /// Check whether a single adjacent step is valid.
203    ///
204    /// Diagonal steps are rejected when either orthogonal side cell is
205    /// blocked, matching no-corner-cutting grid semantics.
206    pub fn is_valid_step(&self, from_x: i32, from_y: i32, to_x: i32, to_y: i32) -> bool {
207        if !self.is_valid(from_x, from_y) || !self.is_valid(to_x, to_y) {
208            return false;
209        }
210
211        let dx = to_x - from_x;
212        let dy = to_y - from_y;
213
214        if dx.abs() > 1 || dy.abs() > 1 || (dx == 0 && dy == 0) {
215            return false;
216        }
217
218        if dx != 0 && dy != 0 {
219            self.is_valid(from_x + dx, from_y) && self.is_valid(from_x, from_y + dy)
220        } else {
221            true
222        }
223    }
224
225    /// Check whether an adjacent `(dx, dy)` move is valid from the given cell.
226    pub fn is_valid_offset(&self, x: i32, y: i32, dx: i32, dy: i32) -> bool {
227        self.is_valid_step(x, y, x + dx, y + dy)
228    }
229
230    fn validate_inputs(
231        ox: &[f64],
232        oy: &[f64],
233        resolution: f64,
234        robot_radius: f64,
235    ) -> RoboticsResult<()> {
236        if ox.len() != oy.len() {
237            return Err(RoboticsError::InvalidParameter(format!(
238                "obstacle x/y coordinates must have matching lengths, got {} and {}",
239                ox.len(),
240                oy.len()
241            )));
242        }
243        if ox.is_empty() {
244            return Err(RoboticsError::InvalidParameter(
245                "grid planners require at least one obstacle point".to_string(),
246            ));
247        }
248        if !resolution.is_finite() || resolution <= 0.0 {
249            return Err(RoboticsError::InvalidParameter(format!(
250                "resolution must be positive and finite, got {}",
251                resolution
252            )));
253        }
254        if !robot_radius.is_finite() || robot_radius < 0.0 {
255            return Err(RoboticsError::InvalidParameter(format!(
256                "robot_radius must be non-negative and finite, got {}",
257                robot_radius
258            )));
259        }
260        if ox.iter().chain(oy.iter()).any(|value| !value.is_finite()) {
261            return Err(RoboticsError::InvalidParameter(
262                "obstacle coordinates must be finite".to_string(),
263            ));
264        }
265
266        Ok(())
267    }
268}
269
270// Alias for backward compatibility
271pub type SearchNode = Node;
272
273#[cfg(test)]
274mod tests {
275    use super::*;
276    use rust_robotics_core::Point2D;
277
278    fn create_obstacles() -> Obstacles {
279        Obstacles::from_points(vec![
280            Point2D::new(0.0, 0.0),
281            Point2D::new(10.0, 0.0),
282            Point2D::new(0.0, 10.0),
283            Point2D::new(10.0, 10.0),
284        ])
285    }
286
287    #[test]
288    fn test_try_new_rejects_empty_obstacles() {
289        let err = GridMap::try_new(&[], &[], 1.0, 0.5).unwrap_err();
290        assert!(matches!(err, RoboticsError::InvalidParameter(_)));
291    }
292
293    #[test]
294    fn test_try_new_rejects_invalid_resolution() {
295        let obstacles = create_obstacles();
296        let ox = obstacles.x_coords();
297        let oy = obstacles.y_coords();
298
299        let err = GridMap::try_new(&ox, &oy, 0.0, 0.5).unwrap_err();
300        assert!(matches!(err, RoboticsError::InvalidParameter(_)));
301    }
302
303    #[test]
304    fn test_from_obstacles_builds_valid_map() {
305        let obstacles = create_obstacles();
306        let grid_map = GridMap::from_obstacles(&obstacles, 1.0, 0.5).unwrap();
307
308        assert_eq!(grid_map.min_x, 0.0);
309        assert_eq!(grid_map.min_y, 0.0);
310        assert_eq!(grid_map.x_width, 10);
311        assert_eq!(grid_map.y_width, 10);
312    }
313
314    #[test]
315    fn test_is_valid_step_blocks_corner_cutting() {
316        let open_obstacles = Obstacles::from_points(vec![
317            Point2D::new(0.0, 0.0),
318            Point2D::new(1.0, 0.0),
319            Point2D::new(2.0, 0.0),
320            Point2D::new(3.0, 0.0),
321            Point2D::new(0.0, 1.0),
322            Point2D::new(3.0, 1.0),
323            Point2D::new(0.0, 2.0),
324            Point2D::new(3.0, 2.0),
325            Point2D::new(0.0, 3.0),
326            Point2D::new(1.0, 3.0),
327            Point2D::new(2.0, 3.0),
328            Point2D::new(3.0, 3.0),
329        ]);
330        let open_grid_map = GridMap::from_obstacles(&open_obstacles, 1.0, 0.0).unwrap();
331
332        assert!(open_grid_map.is_valid_step(1, 1, 2, 1));
333        assert!(open_grid_map.is_valid_step(1, 1, 2, 2));
334
335        let blocked_obstacles = Obstacles::from_points(vec![
336            Point2D::new(0.0, 0.0),
337            Point2D::new(1.0, 0.0),
338            Point2D::new(2.0, 0.0),
339            Point2D::new(3.0, 0.0),
340            Point2D::new(0.0, 1.0),
341            Point2D::new(3.0, 1.0),
342            Point2D::new(0.0, 2.0),
343            Point2D::new(3.0, 2.0),
344            Point2D::new(0.0, 3.0),
345            Point2D::new(1.0, 3.0),
346            Point2D::new(2.0, 3.0),
347            Point2D::new(3.0, 3.0),
348            Point2D::new(1.0, 2.0),
349            Point2D::new(2.0, 1.0),
350        ]);
351        let grid_map = GridMap::from_obstacles(&blocked_obstacles, 1.0, 0.0).unwrap();
352
353        assert!(grid_map.is_valid(1, 1));
354        assert!(grid_map.is_valid(2, 2));
355        assert!(!grid_map.is_valid_step(1, 1, 2, 2));
356    }
357}
rust_robotics_planning/grid.rs

rust_robotics_planning/
grid.rs
