rust_robotics_planning/
greedy_best_first_search.rs

1//! Greedy Best-First Search path planning algorithm
2//!
3//! Grid-based path planning using Greedy Best-First Search.
4//! Unlike A*, the priority is based solely on the heuristic h(n),
5//! ignoring the accumulated cost g(n). This makes it faster in many
6//! cases but does NOT guarantee the shortest path.
7//!
8//! See <https://en.wikipedia.org/wiki/Best-first_search>
9
10use std::cmp::Ordering;
11use std::collections::{BinaryHeap, HashMap};
12
13use crate::grid::{GridMap, Node};
14use rust_robotics_core::{Obstacles, Path2D, PathPlanner, Point2D, RoboticsError, RoboticsResult};
15
16/// Configuration for Greedy Best-First Search planner
17#[derive(Debug, Clone)]
18pub struct GreedyBestFirstConfig {
19    /// Grid resolution in meters
20    pub resolution: f64,
21    /// Robot radius for obstacle inflation
22    pub robot_radius: f64,
23}
24
25impl Default for GreedyBestFirstConfig {
26    fn default() -> Self {
27        Self {
28            resolution: 1.0,
29            robot_radius: 0.5,
30        }
31    }
32}
33
34impl GreedyBestFirstConfig {
35    pub fn validate(&self) -> RoboticsResult<()> {
36        if !self.resolution.is_finite() || self.resolution <= 0.0 {
37            return Err(RoboticsError::InvalidParameter(format!(
38                "resolution must be positive and finite, got {}",
39                self.resolution
40            )));
41        }
42        if !self.robot_radius.is_finite() || self.robot_radius < 0.0 {
43            return Err(RoboticsError::InvalidParameter(format!(
44                "robot_radius must be non-negative and finite, got {}",
45                self.robot_radius
46            )));
47        }
48
49        Ok(())
50    }
51}
52
53/// Node with priority for Greedy Best-First open set (min-heap)
54#[derive(Debug)]
55struct PriorityNode {
56    x: i32,
57    y: i32,
58    cost: f64,
59    priority: f64,
60    index: usize,
61}
62
63impl Eq for PriorityNode {}
64
65impl PartialEq for PriorityNode {
66    fn eq(&self, other: &Self) -> bool {
67        self.priority == other.priority
68    }
69}
70
71impl Ord for PriorityNode {
72    fn cmp(&self, other: &Self) -> Ordering {
73        // Reverse ordering for min-heap behavior
74        other
75            .priority
76            .partial_cmp(&self.priority)
77            .unwrap_or(Ordering::Equal)
78    }
79}
80
81impl PartialOrd for PriorityNode {
82    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
83        Some(self.cmp(other))
84    }
85}
86
87/// Greedy Best-First Search path planner
88pub struct GreedyBestFirstPlanner {
89    grid_map: GridMap,
90    #[allow(dead_code)]
91    config: GreedyBestFirstConfig,
92    motion: Vec<(i32, i32, f64)>,
93}
94
95impl GreedyBestFirstPlanner {
96    /// Create a new Greedy Best-First planner with obstacle positions
97    pub fn new(ox: &[f64], oy: &[f64], config: GreedyBestFirstConfig) -> Self {
98        Self::try_new(ox, oy, config).expect(
99            "invalid Greedy Best-First planner input: obstacle list must be non-empty and valid, and config values must be positive/finite",
100        )
101    }
102
103    /// Create a validated Greedy Best-First planner with obstacle positions
104    pub fn try_new(ox: &[f64], oy: &[f64], config: GreedyBestFirstConfig) -> RoboticsResult<Self> {
105        config.validate()?;
106        let grid_map = GridMap::try_new(ox, oy, config.resolution, config.robot_radius)?;
107        let motion = Self::get_motion_model();
108
109        Ok(GreedyBestFirstPlanner {
110            grid_map,
111            config,
112            motion,
113        })
114    }
115
116    /// Create from obstacle x/y vectors with default config
117    pub fn from_obstacles(ox: &[f64], oy: &[f64], resolution: f64, robot_radius: f64) -> Self {
118        let config = GreedyBestFirstConfig {
119            resolution,
120            robot_radius,
121        };
122        Self::new(ox, oy, config)
123    }
124
125    /// Create a validated Greedy Best-First planner from typed obstacle points
126    pub fn from_obstacle_points(
127        obstacles: &Obstacles,
128        config: GreedyBestFirstConfig,
129    ) -> RoboticsResult<Self> {
130        config.validate()?;
131        let grid_map = GridMap::from_obstacles(obstacles, config.resolution, config.robot_radius)?;
132        let motion = Self::get_motion_model();
133
134        Ok(GreedyBestFirstPlanner {
135            grid_map,
136            config,
137            motion,
138        })
139    }
140
141    /// Plan a path returning (rx, ry) vectors (legacy interface)
142    #[deprecated(note = "use plan() or plan_xy() instead")]
143    pub fn planning(&self, sx: f64, sy: f64, gx: f64, gy: f64) -> Option<(Vec<f64>, Vec<f64>)> {
144        match self.plan_xy(sx, sy, gx, gy) {
145            Ok(path) => Some((path.x_coords(), path.y_coords())),
146            Err(_) => None,
147        }
148    }
149
150    /// Plan a path without requiring the PathPlanner trait in scope
151    pub fn plan(&self, start: Point2D, goal: Point2D) -> RoboticsResult<Path2D> {
152        self.plan_impl(start, goal)
153    }
154
155    /// Plan a path from raw coordinates without requiring the PathPlanner trait in scope
156    pub fn plan_xy(&self, sx: f64, sy: f64, gx: f64, gy: f64) -> RoboticsResult<Path2D> {
157        self.plan_impl(Point2D::new(sx, sy), Point2D::new(gx, gy))
158    }
159
160    /// Get reference to the grid map
161    pub fn grid_map(&self) -> &GridMap {
162        &self.grid_map
163    }
164
165    fn calc_heuristic(n1_x: i32, n1_y: i32, n2_x: i32, n2_y: i32) -> f64 {
166        (((n1_x - n2_x).pow(2) + (n1_y - n2_y).pow(2)) as f64).sqrt()
167    }
168
169    fn get_motion_model() -> Vec<(i32, i32, f64)> {
170        // dx, dy, cost (8-connected grid)
171        vec![
172            (1, 0, 1.0),
173            (0, 1, 1.0),
174            (-1, 0, 1.0),
175            (0, -1, 1.0),
176            (-1, -1, std::f64::consts::SQRT_2),
177            (-1, 1, std::f64::consts::SQRT_2),
178            (1, -1, std::f64::consts::SQRT_2),
179            (1, 1, std::f64::consts::SQRT_2),
180        ]
181    }
182
183    fn build_path(&self, goal_index: usize, node_storage: &[Node]) -> Path2D {
184        let mut points = Vec::new();
185        let mut current_index = Some(goal_index);
186
187        while let Some(index) = current_index {
188            let node = &node_storage[index];
189            points.push(Point2D::new(
190                self.grid_map.calc_x_position(node.x),
191                self.grid_map.calc_y_position(node.y),
192            ));
193            current_index = node.parent_index;
194        }
195
196        points.reverse();
197        Path2D::from_points(points)
198    }
199
200    fn ensure_query_is_valid(&self, x: i32, y: i32, label: &str) -> RoboticsResult<()> {
201        if self.grid_map.is_valid(x, y) {
202            return Ok(());
203        }
204
205        Err(RoboticsError::PlanningError(format!(
206            "{} position is invalid",
207            label
208        )))
209    }
210
211    fn plan_impl(&self, start: Point2D, goal: Point2D) -> RoboticsResult<Path2D> {
212        let start_x = self.grid_map.calc_x_index(start.x);
213        let start_y = self.grid_map.calc_y_index(start.y);
214        let goal_x = self.grid_map.calc_x_index(goal.x);
215        let goal_y = self.grid_map.calc_y_index(goal.y);
216
217        self.ensure_query_is_valid(start_x, start_y, "Start")?;
218        self.ensure_query_is_valid(goal_x, goal_y, "Goal")?;
219
220        let mut open_set = BinaryHeap::new();
221        let mut closed_set = HashMap::new();
222        let mut node_storage: Vec<Node> = Vec::new();
223
224        // Add start node
225        node_storage.push(Node::new(start_x, start_y, 0.0, None));
226        let start_index = 0;
227
228        open_set.push(PriorityNode {
229            x: start_x,
230            y: start_y,
231            cost: 0.0,
232            // Greedy Best-First: priority = h(n) only (no g-cost)
233            priority: Self::calc_heuristic(start_x, start_y, goal_x, goal_y),
234            index: start_index,
235        });
236
237        while let Some(current) = open_set.pop() {
238            let current_grid_index = self.grid_map.calc_index(current.x, current.y);
239
240            // Check if we reached the goal
241            if current.x == goal_x && current.y == goal_y {
242                return Ok(self.build_path(current.index, &node_storage));
243            }
244
245            // Skip if already in closed set
246            if closed_set.contains_key(&current_grid_index) {
247                continue;
248            }
249
250            // Move to closed set
251            closed_set.insert(current_grid_index, current.index);
252
253            // Expand neighbors
254            for &(dx, dy, move_cost) in &self.motion {
255                let new_x = current.x + dx;
256                let new_y = current.y + dy;
257                let new_cost = current.cost + move_cost;
258                let new_grid_index = self.grid_map.calc_index(new_x, new_y);
259
260                // Skip if not valid or already visited
261                if !self.grid_map.is_valid_offset(current.x, current.y, dx, dy) {
262                    continue;
263                }
264                if closed_set.contains_key(&new_grid_index) {
265                    continue;
266                }
267
268                // Add to storage and open set
269                node_storage.push(Node::new(new_x, new_y, new_cost, Some(current.index)));
270                let new_index = node_storage.len() - 1;
271
272                // Greedy Best-First: priority = h(n) only (no g-cost added)
273                let priority = Self::calc_heuristic(new_x, new_y, goal_x, goal_y);
274                open_set.push(PriorityNode {
275                    x: new_x,
276                    y: new_y,
277                    cost: new_cost,
278                    priority,
279                    index: new_index,
280                });
281            }
282        }
283
284        Err(RoboticsError::PlanningError("No path found".to_string()))
285    }
286}
287
288impl PathPlanner for GreedyBestFirstPlanner {
289    fn plan(&self, start: Point2D, goal: Point2D) -> Result<Path2D, RoboticsError> {
290        self.plan_impl(start, goal)
291    }
292}
293
294#[cfg(test)]
295mod tests {
296    use super::*;
297
298    use rust_robotics_core::Obstacles;
299
300    fn create_simple_obstacles() -> (Vec<f64>, Vec<f64>) {
301        let mut ox = Vec::new();
302        let mut oy = Vec::new();
303
304        // Boundary
305        for i in 0..11 {
306            ox.push(i as f64);
307            oy.push(0.0);
308            ox.push(i as f64);
309            oy.push(10.0);
310            ox.push(0.0);
311            oy.push(i as f64);
312            ox.push(10.0);
313            oy.push(i as f64);
314        }
315
316        // Internal obstacle
317        for i in 4..7 {
318            ox.push(5.0);
319            oy.push(i as f64);
320        }
321
322        (ox, oy)
323    }
324
325    #[test]
326    fn test_greedy_best_first_finds_path() {
327        let (ox, oy) = create_simple_obstacles();
328        let planner = GreedyBestFirstPlanner::from_obstacles(&ox, &oy, 1.0, 0.5);
329
330        let start = Point2D::new(2.0, 2.0);
331        let goal = Point2D::new(8.0, 8.0);
332
333        let result = planner.plan(start, goal);
334        assert!(result.is_ok());
335
336        let path = result.unwrap();
337        assert!(!path.is_empty());
338    }
339
340    #[test]
341    #[allow(deprecated)]
342    fn test_greedy_best_first_legacy_interface() {
343        let (ox, oy) = create_simple_obstacles();
344        let planner = GreedyBestFirstPlanner::from_obstacles(&ox, &oy, 1.0, 0.5);
345
346        let result = planner.planning(2.0, 2.0, 8.0, 8.0);
347        assert!(result.is_some());
348
349        let (rx, ry) = result.unwrap();
350        assert!(!rx.is_empty());
351        assert_eq!(rx.len(), ry.len());
352    }
353
354    #[test]
355    fn test_greedy_best_first_from_obstacle_points() {
356        let (ox, oy) = create_simple_obstacles();
357        let obstacles = Obstacles::try_from_xy(&ox, &oy).unwrap();
358        let planner = GreedyBestFirstPlanner::from_obstacle_points(
359            &obstacles,
360            GreedyBestFirstConfig::default(),
361        )
362        .unwrap();
363
364        let path = planner.plan_xy(2.0, 2.0, 8.0, 8.0).unwrap();
365        assert!(!path.is_empty());
366    }
367
368    #[test]
369    fn test_greedy_best_first_try_new_rejects_invalid_config() {
370        let (ox, oy) = create_simple_obstacles();
371        let config = GreedyBestFirstConfig {
372            resolution: 0.0,
373            ..Default::default()
374        };
375
376        let err = match GreedyBestFirstPlanner::try_new(&ox, &oy, config) {
377            Ok(_) => panic!("expected invalid config to be rejected"),
378            Err(err) => err,
379        };
380        assert!(matches!(err, RoboticsError::InvalidParameter(_)));
381    }
382
383    #[test]
384    fn test_greedy_best_first_preserves_asymmetric_world_coordinates() {
385        let mut obstacles = Obstacles::new();
386
387        for x in 10..=20 {
388            obstacles.push(Point2D::new(x as f64, -4.0));
389            obstacles.push(Point2D::new(x as f64, 6.0));
390        }
391        for y in -4..=6 {
392            obstacles.push(Point2D::new(10.0, y as f64));
393            obstacles.push(Point2D::new(20.0, y as f64));
394        }
395
396        let planner = GreedyBestFirstPlanner::from_obstacle_points(
397            &obstacles,
398            GreedyBestFirstConfig::default(),
399        )
400        .unwrap();
401        let path = planner.plan_xy(12.0, -2.0, 18.0, 4.0).unwrap();
402
403        let first = path.points.first().unwrap();
404        let last = path.points.last().unwrap();
405        assert!((first.x - 12.0).abs() < 1e-6);
406        assert!((first.y + 2.0).abs() < 1e-6);
407        assert!((last.x - 18.0).abs() < 1e-6);
408        assert!((last.y - 4.0).abs() < 1e-6);
409    }
410
411    #[test]
412    fn test_greedy_best_first_path_not_necessarily_optimal() {
413        // Greedy Best-First may find a longer path than A* due to
414        // ignoring g-cost. We just verify it finds a valid path.
415        let (ox, oy) = create_simple_obstacles();
416        let planner = GreedyBestFirstPlanner::from_obstacles(&ox, &oy, 1.0, 0.5);
417
418        let path = planner.plan_xy(2.0, 2.0, 8.0, 8.0).unwrap();
419
420        // Path starts near start and ends near goal
421        let first = path.points.first().unwrap();
422        let last = path.points.last().unwrap();
423        assert!((first.x - 2.0).abs() < 1e-6);
424        assert!((first.y - 2.0).abs() < 1e-6);
425        assert!((last.x - 8.0).abs() < 1e-6);
426        assert!((last.y - 8.0).abs() < 1e-6);
427    }
428}
rust_robotics_planning/greedy_best_first_search.rs

rust_robotics_planning/
greedy_best_first_search.rs
