Skip to main content

rust_robotics_planning/
a_star.rs

1//! A* path planning algorithm
2//!
3//! Grid-based path planning using A* search algorithm with
4//! configurable heuristic weight.
5
6use std::cmp::Ordering;
7use std::collections::{BinaryHeap, HashMap};
8
9use crate::grid::{GridMap, Node};
10use rust_robotics_core::{Obstacles, Path2D, PathPlanner, Point2D, RoboticsError, RoboticsResult};
11
12/// Configuration for A* planner
13#[derive(Debug, Clone)]
14pub struct AStarConfig {
15    /// Grid resolution in meters
16    pub resolution: f64,
17    /// Robot radius for obstacle inflation
18    pub robot_radius: f64,
19    /// Heuristic weight (1.0 = optimal, >1.0 = faster but suboptimal,
20    /// 0.0 = uniform-cost search, i.e. Dijkstra)
21    pub heuristic_weight: f64,
22}
23
24impl Default for AStarConfig {
25    fn default() -> Self {
26        Self {
27            resolution: 1.0,
28            robot_radius: 0.5,
29            heuristic_weight: 1.0,
30        }
31    }
32}
33
34impl AStarConfig {
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        if !self.heuristic_weight.is_finite() || self.heuristic_weight < 0.0 {
49            return Err(RoboticsError::InvalidParameter(format!(
50                "heuristic_weight must be non-negative and finite, got {}",
51                self.heuristic_weight
52            )));
53        }
54
55        Ok(())
56    }
57}
58
59/// Node with priority for A* open set (min-heap)
60#[derive(Debug)]
61struct PriorityNode {
62    x: i32,
63    y: i32,
64    cost: f64,
65    priority: f64,
66    index: usize,
67}
68
69impl Eq for PriorityNode {}
70
71impl PartialEq for PriorityNode {
72    fn eq(&self, other: &Self) -> bool {
73        self.priority == other.priority
74    }
75}
76
77impl Ord for PriorityNode {
78    fn cmp(&self, other: &Self) -> Ordering {
79        // Reverse ordering for min-heap behavior
80        other
81            .priority
82            .partial_cmp(&self.priority)
83            .unwrap_or(Ordering::Equal)
84    }
85}
86
87impl PartialOrd for PriorityNode {
88    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
89        Some(self.cmp(other))
90    }
91}
92
93/// A* path planner
94pub struct AStarPlanner {
95    grid_map: GridMap,
96    config: AStarConfig,
97    motion: Vec<(i32, i32, f64)>,
98}
99
100/// Search-effort statistics collected during one A* query.
101#[derive(Debug, Clone, Copy, Default, PartialEq, Eq)]
102pub struct AStarSearchStats {
103    pub expanded_nodes: usize,
104    pub generated_nodes: usize,
105    pub skipped_closed_nodes: usize,
106    pub max_frontier_len: usize,
107}
108
109impl AStarPlanner {
110    /// Create a new A* planner with obstacle positions
111    pub fn new(ox: &[f64], oy: &[f64], config: AStarConfig) -> Self {
112        Self::try_new(ox, oy, config).expect(
113            "invalid A* planner input: obstacle list must be non-empty and valid, and config values must be positive/finite",
114        )
115    }
116
117    /// Create a validated A* planner with obstacle positions
118    pub fn try_new(ox: &[f64], oy: &[f64], config: AStarConfig) -> RoboticsResult<Self> {
119        config.validate()?;
120        let grid_map = GridMap::try_new(ox, oy, config.resolution, config.robot_radius)?;
121        let motion = Self::get_motion_model();
122
123        Ok(AStarPlanner {
124            grid_map,
125            config,
126            motion,
127        })
128    }
129
130    /// Create from obstacle x/y vectors with default config
131    pub fn from_obstacles(ox: &[f64], oy: &[f64], resolution: f64, robot_radius: f64) -> Self {
132        let config = AStarConfig {
133            resolution,
134            robot_radius,
135            ..Default::default()
136        };
137        Self::new(ox, oy, config)
138    }
139
140    /// Create a validated A* planner from typed obstacle points
141    pub fn from_obstacle_points(
142        obstacles: &Obstacles,
143        config: AStarConfig,
144    ) -> RoboticsResult<Self> {
145        config.validate()?;
146        let grid_map = GridMap::from_obstacles(obstacles, config.resolution, config.robot_radius)?;
147        let motion = Self::get_motion_model();
148
149        Ok(AStarPlanner {
150            grid_map,
151            config,
152            motion,
153        })
154    }
155
156    /// Plan a path returning (rx, ry) vectors (legacy interface)
157    #[deprecated(note = "use plan() or plan_xy() instead")]
158    pub fn planning(&self, sx: f64, sy: f64, gx: f64, gy: f64) -> Option<(Vec<f64>, Vec<f64>)> {
159        match self.plan_xy(sx, sy, gx, gy) {
160            Ok(path) => Some((path.x_coords(), path.y_coords())),
161            Err(_) => None,
162        }
163    }
164
165    /// Plan a path without requiring the PathPlanner trait in scope
166    pub fn plan(&self, start: Point2D, goal: Point2D) -> RoboticsResult<Path2D> {
167        self.plan_impl(start, goal).map(|(path, _stats)| path)
168    }
169
170    /// Plan a path from raw coordinates without requiring the PathPlanner trait in scope
171    pub fn plan_xy(&self, sx: f64, sy: f64, gx: f64, gy: f64) -> RoboticsResult<Path2D> {
172        self.plan_impl(Point2D::new(sx, sy), Point2D::new(gx, gy))
173            .map(|(path, _stats)| path)
174    }
175
176    /// Plan a path and return per-query search-effort statistics.
177    pub fn plan_with_stats(
178        &self,
179        start: Point2D,
180        goal: Point2D,
181    ) -> RoboticsResult<(Path2D, AStarSearchStats)> {
182        self.plan_impl(start, goal)
183    }
184
185    /// Plans a path while observing each expanded cell in search order.
186    ///
187    /// The callback receives world coordinates, once per expanded cell,
188    /// including the start and (when reached) the goal. It is also called
189    /// during unsuccessful searches; invalid queries emit no cells. The
190    /// ordinary planning methods do not allocate or retain this history.
191    pub fn plan_with_observer(
192        &self,
193        start: Point2D,
194        goal: Point2D,
195        mut on_expand: impl FnMut(Point2D),
196    ) -> RoboticsResult<Path2D> {
197        self.search(start, goal, &mut on_expand)
198            .map(|(path, _stats)| path)
199    }
200
201    /// Get reference to the grid map
202    pub fn grid_map(&self) -> &GridMap {
203        &self.grid_map
204    }
205
206    fn calc_heuristic(&self, n1_x: i32, n1_y: i32, n2_x: i32, n2_y: i32) -> f64 {
207        self.config.heuristic_weight * (((n1_x - n2_x).pow(2) + (n1_y - n2_y).pow(2)) as f64).sqrt()
208    }
209
210    fn get_motion_model() -> Vec<(i32, i32, f64)> {
211        // dx, dy, cost (8-connected grid)
212        vec![
213            (1, 0, 1.0),
214            (0, 1, 1.0),
215            (-1, 0, 1.0),
216            (0, -1, 1.0),
217            (-1, -1, std::f64::consts::SQRT_2),
218            (-1, 1, std::f64::consts::SQRT_2),
219            (1, -1, std::f64::consts::SQRT_2),
220            (1, 1, std::f64::consts::SQRT_2),
221        ]
222    }
223
224    fn build_path(&self, goal_index: usize, node_storage: &[Node]) -> Path2D {
225        let mut points = Vec::new();
226        let mut current_index = Some(goal_index);
227
228        while let Some(index) = current_index {
229            let node = &node_storage[index];
230            points.push(Point2D::new(
231                self.grid_map.calc_x_position(node.x),
232                self.grid_map.calc_y_position(node.y),
233            ));
234            current_index = node.parent_index;
235        }
236
237        points.reverse();
238        Path2D::from_points(points)
239    }
240
241    fn ensure_query_is_valid(&self, x: i32, y: i32, label: &str) -> RoboticsResult<()> {
242        if self.grid_map.is_valid(x, y) {
243            return Ok(());
244        }
245
246        Err(RoboticsError::PlanningError(format!(
247            "{} position is invalid",
248            label
249        )))
250    }
251
252    fn plan_impl(
253        &self,
254        start: Point2D,
255        goal: Point2D,
256    ) -> RoboticsResult<(Path2D, AStarSearchStats)> {
257        self.search(start, goal, &mut |_| {})
258    }
259
260    fn search(
261        &self,
262        start: Point2D,
263        goal: Point2D,
264        on_expand: &mut impl FnMut(Point2D),
265    ) -> RoboticsResult<(Path2D, AStarSearchStats)> {
266        let start_x = self.grid_map.calc_x_index(start.x);
267        let start_y = self.grid_map.calc_y_index(start.y);
268        let goal_x = self.grid_map.calc_x_index(goal.x);
269        let goal_y = self.grid_map.calc_y_index(goal.y);
270
271        self.ensure_query_is_valid(start_x, start_y, "Start")?;
272        self.ensure_query_is_valid(goal_x, goal_y, "Goal")?;
273
274        let mut open_set = BinaryHeap::new();
275        let mut closed_set = HashMap::new();
276        let mut node_storage: Vec<Node> = Vec::new();
277        let mut stats = AStarSearchStats::default();
278
279        // Add start node
280        node_storage.push(Node::new(start_x, start_y, 0.0, None));
281        let start_index = 0;
282
283        open_set.push(PriorityNode {
284            x: start_x,
285            y: start_y,
286            cost: 0.0,
287            priority: self.calc_heuristic(start_x, start_y, goal_x, goal_y),
288            index: start_index,
289        });
290        stats.max_frontier_len = open_set.len();
291
292        while let Some(current) = open_set.pop() {
293            let current_grid_index = self.grid_map.calc_index(current.x, current.y);
294
295            // Skip if already in closed set
296            if closed_set.contains_key(&current_grid_index) {
297                stats.skipped_closed_nodes += 1;
298                continue;
299            }
300
301            stats.expanded_nodes += 1;
302            on_expand(Point2D::new(
303                self.grid_map.calc_x_position(current.x),
304                self.grid_map.calc_y_position(current.y),
305            ));
306
307            // Check if we reached the goal
308            if current.x == goal_x && current.y == goal_y {
309                return Ok((self.build_path(current.index, &node_storage), stats));
310            }
311
312            // Move to closed set
313            closed_set.insert(current_grid_index, current.index);
314
315            // Expand neighbors
316            for &(dx, dy, move_cost) in &self.motion {
317                let new_x = current.x + dx;
318                let new_y = current.y + dy;
319                let new_cost = current.cost + move_cost;
320                let new_grid_index = self.grid_map.calc_index(new_x, new_y);
321
322                // Skip if not valid or already visited
323                if !self.grid_map.is_valid_offset(current.x, current.y, dx, dy) {
324                    continue;
325                }
326                if closed_set.contains_key(&new_grid_index) {
327                    continue;
328                }
329
330                // Add to storage and open set
331                node_storage.push(Node::new(new_x, new_y, new_cost, Some(current.index)));
332                let new_index = node_storage.len() - 1;
333                stats.generated_nodes += 1;
334
335                let priority = new_cost + self.calc_heuristic(new_x, new_y, goal_x, goal_y);
336                open_set.push(PriorityNode {
337                    x: new_x,
338                    y: new_y,
339                    cost: new_cost,
340                    priority,
341                    index: new_index,
342                });
343                stats.max_frontier_len = stats.max_frontier_len.max(open_set.len());
344            }
345        }
346
347        Err(RoboticsError::PlanningError("No path found".to_string()))
348    }
349}
350
351impl PathPlanner for AStarPlanner {
352    fn plan(&self, start: Point2D, goal: Point2D) -> Result<Path2D, RoboticsError> {
353        self.plan_impl(start, goal).map(|(path, _stats)| path)
354    }
355}
356
357#[cfg(test)]
358mod tests {
359    use super::*;
360    use crate::moving_ai::{MovingAiMap, MovingAiScenario};
361
362    use rust_robotics_core::Obstacles;
363
364    fn create_simple_obstacles() -> (Vec<f64>, Vec<f64>) {
365        let mut ox = Vec::new();
366        let mut oy = Vec::new();
367
368        // Boundary
369        for i in 0..11 {
370            ox.push(i as f64);
371            oy.push(0.0);
372            ox.push(i as f64);
373            oy.push(10.0);
374            ox.push(0.0);
375            oy.push(i as f64);
376            ox.push(10.0);
377            oy.push(i as f64);
378        }
379
380        // Internal obstacle
381        for i in 4..7 {
382            ox.push(5.0);
383            oy.push(i as f64);
384        }
385
386        (ox, oy)
387    }
388
389    #[test]
390    fn test_a_star_finds_path() {
391        let (ox, oy) = create_simple_obstacles();
392        let planner = AStarPlanner::from_obstacles(&ox, &oy, 1.0, 0.5);
393
394        let start = Point2D::new(2.0, 2.0);
395        let goal = Point2D::new(8.0, 8.0);
396
397        let result = planner.plan(start, goal);
398        assert!(result.is_ok());
399
400        let path = result.unwrap();
401        assert!(!path.is_empty());
402    }
403
404    #[test]
405    #[allow(deprecated)]
406    fn test_a_star_legacy_interface() {
407        let (ox, oy) = create_simple_obstacles();
408        let planner = AStarPlanner::from_obstacles(&ox, &oy, 1.0, 0.5);
409
410        let result = planner.planning(2.0, 2.0, 8.0, 8.0);
411        assert!(result.is_some());
412
413        let (rx, ry) = result.unwrap();
414        assert!(!rx.is_empty());
415        assert_eq!(rx.len(), ry.len());
416    }
417
418    #[test]
419    fn test_a_star_from_obstacle_points() {
420        let (ox, oy) = create_simple_obstacles();
421        let obstacles = Obstacles::try_from_xy(&ox, &oy).unwrap();
422        let planner =
423            AStarPlanner::from_obstacle_points(&obstacles, AStarConfig::default()).unwrap();
424
425        let path = planner.plan_xy(2.0, 2.0, 8.0, 8.0).unwrap();
426        assert!(!path.is_empty());
427    }
428
429    #[test]
430    fn test_a_star_try_new_rejects_invalid_config() {
431        let (ox, oy) = create_simple_obstacles();
432        let config = AStarConfig {
433            heuristic_weight: -1.0,
434            ..Default::default()
435        };
436
437        let err = match AStarPlanner::try_new(&ox, &oy, config) {
438            Ok(_) => panic!("expected invalid config to be rejected"),
439            Err(err) => err,
440        };
441        assert!(matches!(err, RoboticsError::InvalidParameter(_)));
442
443        // A zero weight is valid: uniform-cost search (Dijkstra).
444        let dijkstra = AStarConfig {
445            heuristic_weight: 0.0,
446            ..Default::default()
447        };
448        assert!(AStarPlanner::try_new(&ox, &oy, dijkstra).is_ok());
449    }
450
451    #[test]
452    fn test_a_star_preserves_asymmetric_world_coordinates() {
453        let mut obstacles = Obstacles::new();
454
455        for x in 10..=20 {
456            obstacles.push(Point2D::new(x as f64, -4.0));
457            obstacles.push(Point2D::new(x as f64, 6.0));
458        }
459        for y in -4..=6 {
460            obstacles.push(Point2D::new(10.0, y as f64));
461            obstacles.push(Point2D::new(20.0, y as f64));
462        }
463
464        let planner =
465            AStarPlanner::from_obstacle_points(&obstacles, AStarConfig::default()).unwrap();
466        let path = planner.plan_xy(12.0, -2.0, 18.0, 4.0).unwrap();
467
468        let first = path.points.first().unwrap();
469        let last = path.points.last().unwrap();
470        assert!((first.x - 12.0).abs() < 1e-6);
471        assert!((first.y + 2.0).abs() < 1e-6);
472        assert!((last.x - 18.0).abs() < 1e-6);
473        assert!((last.y - 4.0).abs() < 1e-6);
474    }
475
476    #[test]
477    #[ignore = "long-running MovingAI benchmark"]
478    fn test_a_star_matches_moving_ai_arena2_bucket_80_optimal_length() {
479        let map = MovingAiMap::parse_str(include_str!("../benchdata/moving_ai/dao/arena2.map"))
480            .expect("arena2 MovingAI map should parse");
481        let scenario =
482            MovingAiScenario::parse_str(include_str!("../benchdata/moving_ai/dao/arena2.map.scen"))
483                .expect("arena2 MovingAI scenarios should parse")
484                .into_iter()
485                .find(|row| row.bucket == 80)
486                .expect("arena2 MovingAI bucket 80 scenario should exist");
487
488        let planner = AStarPlanner::from_obstacle_points(
489            &map.to_obstacles(),
490            AStarConfig {
491                resolution: 1.0,
492                robot_radius: 0.5,
493                heuristic_weight: 1.0,
494            },
495        )
496        .expect("A* planner should build from arena2 obstacles");
497
498        let start = map
499            .planning_point(scenario.start_x, scenario.start_y)
500            .expect("arena2 start should be valid");
501        let goal = map
502            .planning_point(scenario.goal_x, scenario.goal_y)
503            .expect("arena2 goal should be valid");
504
505        let path = planner
506            .plan(start, goal)
507            .expect("A* should solve the arena2 bucket 80 scenario");
508
509        assert!(
510            (path.total_length() - scenario.optimal_length).abs() < 1e-6,
511            "A* path length {} should match MovingAI optimal {} when corner cutting is disabled",
512            path.total_length(),
513            scenario.optimal_length
514        );
515    }
516}