rust_robotics_planning/
breadth_first_search.rs

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

rust_robotics_planning/
breadth_first_search.rs
