rust_robotics_planning/
depth_first_search.rs

1//! Depth-First Search path planning algorithm
2//!
3//! Grid-based path planning using depth-first search with a LIFO stack.
4//! DFS does **not** guarantee the shortest path, but will find a valid
5//! path if one exists.
6
7use std::collections::HashSet;
8
9use crate::grid::{GridMap, Node};
10use rust_robotics_core::{Obstacles, Path2D, PathPlanner, Point2D, RoboticsError, RoboticsResult};
11
12/// Configuration for DFS planner
13#[derive(Debug, Clone)]
14pub struct DFSConfig {
15    /// Grid resolution in meters
16    pub resolution: f64,
17    /// Robot radius for obstacle inflation
18    pub robot_radius: f64,
19}
20
21impl Default for DFSConfig {
22    fn default() -> Self {
23        Self {
24            resolution: 1.0,
25            robot_radius: 0.5,
26        }
27    }
28}
29
30impl DFSConfig {
31    pub fn validate(&self) -> RoboticsResult<()> {
32        if !self.resolution.is_finite() || self.resolution <= 0.0 {
33            return Err(RoboticsError::InvalidParameter(format!(
34                "resolution must be positive and finite, got {}",
35                self.resolution
36            )));
37        }
38        if !self.robot_radius.is_finite() || self.robot_radius < 0.0 {
39            return Err(RoboticsError::InvalidParameter(format!(
40                "robot_radius must be non-negative and finite, got {}",
41                self.robot_radius
42            )));
43        }
44
45        Ok(())
46    }
47}
48
49/// Depth-First Search path planner
50pub struct DFSPlanner {
51    grid_map: GridMap,
52    #[allow(dead_code)]
53    config: DFSConfig,
54    motion: Vec<(i32, i32, f64)>,
55}
56
57impl DFSPlanner {
58    /// Create a new DFS planner with obstacle positions
59    pub fn new(ox: &[f64], oy: &[f64], config: DFSConfig) -> Self {
60        Self::try_new(ox, oy, config).expect(
61            "invalid DFS planner input: obstacle list must be non-empty and valid, and config values must be positive/finite",
62        )
63    }
64
65    /// Create a validated DFS planner with obstacle positions
66    pub fn try_new(ox: &[f64], oy: &[f64], config: DFSConfig) -> RoboticsResult<Self> {
67        config.validate()?;
68        let grid_map = GridMap::try_new(ox, oy, config.resolution, config.robot_radius)?;
69        let motion = Self::get_motion_model();
70
71        Ok(DFSPlanner {
72            grid_map,
73            config,
74            motion,
75        })
76    }
77
78    /// Create from obstacle x/y vectors with default config
79    pub fn from_obstacles(ox: &[f64], oy: &[f64], resolution: f64, robot_radius: f64) -> Self {
80        let config = DFSConfig {
81            resolution,
82            robot_radius,
83        };
84        Self::new(ox, oy, config)
85    }
86
87    /// Create a validated DFS planner from typed obstacle points
88    pub fn from_obstacle_points(obstacles: &Obstacles, config: DFSConfig) -> RoboticsResult<Self> {
89        config.validate()?;
90        let grid_map = GridMap::from_obstacles(obstacles, config.resolution, config.robot_radius)?;
91        let motion = Self::get_motion_model();
92
93        Ok(DFSPlanner {
94            grid_map,
95            config,
96            motion,
97        })
98    }
99
100    /// Plan a path returning (rx, ry) vectors (legacy interface)
101    #[deprecated(note = "use plan() or plan_xy() instead")]
102    pub fn planning(&self, sx: f64, sy: f64, gx: f64, gy: f64) -> Option<(Vec<f64>, Vec<f64>)> {
103        match self.plan_xy(sx, sy, gx, gy) {
104            Ok(path) => Some((path.x_coords(), path.y_coords())),
105            Err(_) => None,
106        }
107    }
108
109    /// Plan a path without requiring the PathPlanner trait in scope
110    pub fn plan(&self, start: Point2D, goal: Point2D) -> RoboticsResult<Path2D> {
111        self.plan_impl(start, goal)
112    }
113
114    /// Plan a path from raw coordinates without requiring the PathPlanner trait in scope
115    pub fn plan_xy(&self, sx: f64, sy: f64, gx: f64, gy: f64) -> RoboticsResult<Path2D> {
116        self.plan_impl(Point2D::new(sx, sy), Point2D::new(gx, gy))
117    }
118
119    /// Get reference to the grid map
120    pub fn grid_map(&self) -> &GridMap {
121        &self.grid_map
122    }
123
124    fn get_motion_model() -> Vec<(i32, i32, f64)> {
125        // dx, dy, cost (8-connected grid)
126        vec![
127            (1, 0, 1.0),
128            (0, 1, 1.0),
129            (-1, 0, 1.0),
130            (0, -1, 1.0),
131            (-1, -1, std::f64::consts::SQRT_2),
132            (-1, 1, std::f64::consts::SQRT_2),
133            (1, -1, std::f64::consts::SQRT_2),
134            (1, 1, std::f64::consts::SQRT_2),
135        ]
136    }
137
138    fn build_path(&self, goal_index: usize, node_storage: &[Node]) -> Path2D {
139        let mut points = Vec::new();
140        let mut current_index = Some(goal_index);
141
142        while let Some(index) = current_index {
143            let node = &node_storage[index];
144            points.push(Point2D::new(
145                self.grid_map.calc_x_position(node.x),
146                self.grid_map.calc_y_position(node.y),
147            ));
148            current_index = node.parent_index;
149        }
150
151        points.reverse();
152        Path2D::from_points(points)
153    }
154
155    fn ensure_query_is_valid(&self, x: i32, y: i32, label: &str) -> RoboticsResult<()> {
156        if self.grid_map.is_valid(x, y) {
157            return Ok(());
158        }
159
160        Err(RoboticsError::PlanningError(format!(
161            "{} position is invalid",
162            label
163        )))
164    }
165
166    fn plan_impl(&self, start: Point2D, goal: Point2D) -> RoboticsResult<Path2D> {
167        let start_x = self.grid_map.calc_x_index(start.x);
168        let start_y = self.grid_map.calc_y_index(start.y);
169        let goal_x = self.grid_map.calc_x_index(goal.x);
170        let goal_y = self.grid_map.calc_y_index(goal.y);
171
172        self.ensure_query_is_valid(start_x, start_y, "Start")?;
173        self.ensure_query_is_valid(goal_x, goal_y, "Goal")?;
174
175        // LIFO stack for DFS (Vec with push/pop)
176        let mut stack: Vec<usize> = Vec::new();
177        let mut closed_set: HashSet<i32> = HashSet::new();
178        let mut node_storage: Vec<Node> = Vec::new();
179
180        // Add start node
181        node_storage.push(Node::new(start_x, start_y, 0.0, None));
182        let start_storage_index = 0;
183        stack.push(start_storage_index);
184
185        while let Some(current_storage_index) = stack.pop() {
186            let current_x = node_storage[current_storage_index].x;
187            let current_y = node_storage[current_storage_index].y;
188            let current_cost = node_storage[current_storage_index].cost;
189            let current_grid_index = self.grid_map.calc_index(current_x, current_y);
190
191            // Check if we reached the goal
192            if current_x == goal_x && current_y == goal_y {
193                return Ok(self.build_path(current_storage_index, &node_storage));
194            }
195
196            // Skip if already visited
197            if closed_set.contains(&current_grid_index) {
198                continue;
199            }
200
201            // Mark as visited
202            closed_set.insert(current_grid_index);
203
204            // Expand neighbors (push to stack for LIFO ordering)
205            for &(dx, dy, move_cost) in &self.motion {
206                let new_x = current_x + dx;
207                let new_y = current_y + dy;
208                let new_cost = current_cost + move_cost;
209                let new_grid_index = self.grid_map.calc_index(new_x, new_y);
210
211                // Skip if not valid or already visited
212                if !self.grid_map.is_valid_offset(current_x, current_y, dx, dy) {
213                    continue;
214                }
215                if closed_set.contains(&new_grid_index) {
216                    continue;
217                }
218
219                // Add to storage and stack
220                node_storage.push(Node::new(
221                    new_x,
222                    new_y,
223                    new_cost,
224                    Some(current_storage_index),
225                ));
226                let new_storage_index = node_storage.len() - 1;
227                stack.push(new_storage_index);
228            }
229        }
230
231        Err(RoboticsError::PlanningError("No path found".to_string()))
232    }
233}
234
235impl PathPlanner for DFSPlanner {
236    fn plan(&self, start: Point2D, goal: Point2D) -> Result<Path2D, RoboticsError> {
237        self.plan_impl(start, goal)
238    }
239}
240
241#[cfg(test)]
242mod tests {
243    use super::*;
244
245    use rust_robotics_core::Obstacles;
246
247    fn create_simple_obstacles() -> (Vec<f64>, Vec<f64>) {
248        let mut ox = Vec::new();
249        let mut oy = Vec::new();
250
251        // Boundary
252        for i in 0..11 {
253            ox.push(i as f64);
254            oy.push(0.0);
255            ox.push(i as f64);
256            oy.push(10.0);
257            ox.push(0.0);
258            oy.push(i as f64);
259            ox.push(10.0);
260            oy.push(i as f64);
261        }
262
263        // Internal obstacle
264        for i in 4..7 {
265            ox.push(5.0);
266            oy.push(i as f64);
267        }
268
269        (ox, oy)
270    }
271
272    #[test]
273    fn test_dfs_finds_path() {
274        let (ox, oy) = create_simple_obstacles();
275        let planner = DFSPlanner::from_obstacles(&ox, &oy, 1.0, 0.5);
276
277        let start = Point2D::new(2.0, 2.0);
278        let goal = Point2D::new(8.0, 8.0);
279
280        let result = planner.plan(start, goal);
281        assert!(result.is_ok());
282
283        let path = result.unwrap();
284        assert!(!path.is_empty());
285    }
286
287    #[test]
288    #[allow(deprecated)]
289    fn test_dfs_legacy_interface() {
290        let (ox, oy) = create_simple_obstacles();
291        let planner = DFSPlanner::from_obstacles(&ox, &oy, 1.0, 0.5);
292
293        let result = planner.planning(2.0, 2.0, 8.0, 8.0);
294        assert!(result.is_some());
295
296        let (rx, ry) = result.unwrap();
297        assert!(!rx.is_empty());
298        assert_eq!(rx.len(), ry.len());
299    }
300
301    #[test]
302    fn test_dfs_from_obstacle_points() {
303        let (ox, oy) = create_simple_obstacles();
304        let obstacles = Obstacles::try_from_xy(&ox, &oy).unwrap();
305        let planner = DFSPlanner::from_obstacle_points(&obstacles, DFSConfig::default()).unwrap();
306
307        let path = planner.plan_xy(2.0, 2.0, 8.0, 8.0).unwrap();
308        assert!(!path.is_empty());
309    }
310
311    #[test]
312    fn test_dfs_try_new_rejects_invalid_config() {
313        let (ox, oy) = create_simple_obstacles();
314        let config = DFSConfig {
315            resolution: 0.0,
316            ..Default::default()
317        };
318
319        let err = match DFSPlanner::try_new(&ox, &oy, config) {
320            Ok(_) => panic!("expected invalid config to be rejected"),
321            Err(err) => err,
322        };
323        assert!(matches!(err, RoboticsError::InvalidParameter(_)));
324    }
325
326    #[test]
327    fn test_dfs_preserves_asymmetric_world_coordinates() {
328        let mut obstacles = Obstacles::new();
329
330        for x in 10..=20 {
331            obstacles.push(Point2D::new(x as f64, -4.0));
332            obstacles.push(Point2D::new(x as f64, 6.0));
333        }
334        for y in -4..=6 {
335            obstacles.push(Point2D::new(10.0, y as f64));
336            obstacles.push(Point2D::new(20.0, y as f64));
337        }
338
339        let planner = DFSPlanner::from_obstacle_points(&obstacles, DFSConfig::default()).unwrap();
340        let path = planner.plan_xy(12.0, -2.0, 18.0, 4.0).unwrap();
341
342        let first = path.points.first().unwrap();
343        let last = path.points.last().unwrap();
344        assert!((first.x - 12.0).abs() < 1e-6);
345        assert!((first.y + 2.0).abs() < 1e-6);
346        assert!((last.x - 18.0).abs() < 1e-6);
347        assert!((last.y - 4.0).abs() < 1e-6);
348    }
349
350    #[test]
351    fn test_dfs_path_starts_at_start_and_ends_at_goal() {
352        let (ox, oy) = create_simple_obstacles();
353        let planner = DFSPlanner::from_obstacles(&ox, &oy, 1.0, 0.5);
354
355        let path = planner.plan_xy(2.0, 2.0, 8.0, 8.0).unwrap();
356        let first = path.points.first().unwrap();
357        let last = path.points.last().unwrap();
358
359        assert!((first.x - 2.0).abs() < 1e-6);
360        assert!((first.y - 2.0).abs() < 1e-6);
361        assert!((last.x - 8.0).abs() < 1e-6);
362        assert!((last.y - 8.0).abs() < 1e-6);
363    }
364}
rust_robotics_planning/depth_first_search.rs

rust_robotics_planning/
depth_first_search.rs
