rust_robotics_planning/
ida_star.rs

1//! Iterative Deepening A* (IDA*) path planning algorithm.
2//!
3//! IDA* combines the admissible heuristic of A* with repeated depth-first
4//! contour searches over increasing `f = g + h` thresholds. It uses much less
5//! memory than A* at the cost of re-expanding states across iterations.
6
7use std::collections::{HashMap, HashSet};
8use std::f64::consts::SQRT_2;
9
10use crate::grid::{get_motion_model_8, GridMap};
11use rust_robotics_core::{Obstacles, Path2D, PathPlanner, Point2D, RoboticsError, RoboticsResult};
12
13const COST_EPSILON: f64 = 1e-9;
14
15/// Configuration for IDA*.
16#[derive(Debug, Clone)]
17pub struct IDAStarConfig {
18    /// Grid resolution in meters.
19    pub resolution: f64,
20    /// Robot radius for obstacle inflation.
21    pub robot_radius: f64,
22    /// Heuristic weight (1.0 = admissible, >1.0 = weighted search).
23    pub heuristic_weight: f64,
24    /// Depth of optimistic local backup used to raise the initial threshold.
25    pub initial_lookahead_depth: usize,
26    /// Maximum number of threshold iterations.
27    pub max_iterations: usize,
28    /// Optional hard cap on expanded nodes for one query.
29    pub max_expanded_nodes: Option<usize>,
30}
31
32impl Default for IDAStarConfig {
33    fn default() -> Self {
34        Self {
35            resolution: 1.0,
36            robot_radius: 0.5,
37            heuristic_weight: 1.0,
38            initial_lookahead_depth: 2,
39            max_iterations: 10_000,
40            max_expanded_nodes: None,
41        }
42    }
43}
44
45impl IDAStarConfig {
46    pub fn validate(&self) -> RoboticsResult<()> {
47        if !self.resolution.is_finite() || self.resolution <= 0.0 {
48            return Err(RoboticsError::InvalidParameter(format!(
49                "resolution must be positive and finite, got {}",
50                self.resolution
51            )));
52        }
53        if !self.robot_radius.is_finite() || self.robot_radius < 0.0 {
54            return Err(RoboticsError::InvalidParameter(format!(
55                "robot_radius must be non-negative and finite, got {}",
56                self.robot_radius
57            )));
58        }
59        if !self.heuristic_weight.is_finite() || self.heuristic_weight <= 0.0 {
60            return Err(RoboticsError::InvalidParameter(format!(
61                "heuristic_weight must be positive and finite, got {}",
62                self.heuristic_weight
63            )));
64        }
65        if self.max_iterations == 0 {
66            return Err(RoboticsError::InvalidParameter(
67                "max_iterations must be greater than zero".to_string(),
68            ));
69        }
70        if matches!(self.max_expanded_nodes, Some(0)) {
71            return Err(RoboticsError::InvalidParameter(
72                "max_expanded_nodes must be greater than zero when provided".to_string(),
73            ));
74        }
75
76        Ok(())
77    }
78}
79
80enum SearchOutcome {
81    Found,
82    NextThreshold(f64),
83    ExpansionBudgetExceeded,
84}
85
86struct SearchContext<'a> {
87    goal: (i32, i32),
88    threshold: f64,
89    path: &'a mut Vec<(i32, i32)>,
90    visited: &'a mut HashSet<i32>,
91    best_g_by_grid: &'a mut HashMap<i32, f64>,
92    expanded_grid_ids: &'a mut HashSet<i32>,
93    stats: &'a mut IDAStarSearchStats,
94}
95
96/// IDA* planner on an 8-connected occupancy grid.
97#[derive(Debug)]
98pub struct IDAStarPlanner {
99    grid_map: GridMap,
100    config: IDAStarConfig,
101    motion: Vec<(i32, i32, f64)>,
102}
103
104/// Per-contour (per-threshold-iteration) statistics for IDA* diagnostics.
105///
106/// Each contour corresponds to one depth-first search pass at a fixed
107/// `f`-cost threshold. Tracking per-contour effort reveals how re-expansion
108/// grows across successive thresholds and where the budget is consumed.
109#[derive(Debug, Clone, Copy, Default, PartialEq)]
110pub struct ContourStats {
111    /// The `f`-cost threshold used for this contour.
112    pub threshold: f64,
113    /// Nodes expanded during this contour (includes re-expansions).
114    pub expanded: usize,
115    /// Nodes expanded here that were never expanded in any prior contour.
116    pub new_unique: usize,
117    /// Nodes expanded here that were already expanded in a prior contour.
118    pub reexpanded: usize,
119    /// Nodes generated (pushed onto the DFS stack) during this contour.
120    pub generated: usize,
121    /// Nodes pruned by `f > threshold` during this contour.
122    pub threshold_prunes: usize,
123    /// Nodes pruned by the current-path cycle check during this contour.
124    pub cycle_prunes: usize,
125    /// Nodes pruned by transposition (cost) comparison during this contour.
126    pub transposition_prunes: usize,
127    /// Maximum recursion depth reached during this contour.
128    pub max_depth: usize,
129}
130
131/// Search-effort statistics collected during one IDA* query.
132#[derive(Debug, Clone, Copy, Default, PartialEq, Eq)]
133pub struct IDAStarSearchStats {
134    pub threshold_iterations: usize,
135    pub expanded_nodes: usize,
136    pub unique_expanded_nodes: usize,
137    pub reexpanded_nodes: usize,
138    pub generated_nodes: usize,
139    pub threshold_prunes: usize,
140    pub cycle_prunes: usize,
141    pub transposition_prunes: usize,
142    pub max_depth: usize,
143}
144
145#[derive(Debug, Clone, Copy, PartialEq, Eq)]
146pub enum IDAStarPlanOutcome {
147    Exact,
148    MaxIterations,
149    MaxExpandedNodes,
150    NoPath,
151}
152
153#[derive(Debug, Clone)]
154pub struct IDAStarPlanReport {
155    pub outcome: IDAStarPlanOutcome,
156    pub path: Option<Path2D>,
157    pub stats: IDAStarSearchStats,
158    pub initial_threshold: f64,
159    pub last_searched_threshold: f64,
160    pub next_threshold: Option<f64>,
161    /// Per-contour effort breakdown, one entry per threshold iteration.
162    pub contour_history: Vec<ContourStats>,
163}
164
165impl IDAStarPlanner {
166    /// Create a new planner from obstacle x/y vectors.
167    pub fn new(ox: &[f64], oy: &[f64], config: IDAStarConfig) -> Self {
168        Self::try_new(ox, oy, config).expect(
169            "invalid IDA* planner input: obstacle list must be non-empty and valid, and config values must be positive/finite",
170        )
171    }
172
173    /// Create a validated planner from obstacle x/y vectors.
174    pub fn try_new(ox: &[f64], oy: &[f64], config: IDAStarConfig) -> RoboticsResult<Self> {
175        config.validate()?;
176        let grid_map = GridMap::try_new(ox, oy, config.resolution, config.robot_radius)?;
177
178        Ok(Self {
179            grid_map,
180            config,
181            motion: get_motion_model_8(),
182        })
183    }
184
185    /// Create from obstacle x/y vectors with explicit scalar parameters.
186    pub fn from_obstacles(ox: &[f64], oy: &[f64], resolution: f64, robot_radius: f64) -> Self {
187        let config = IDAStarConfig {
188            resolution,
189            robot_radius,
190            ..Default::default()
191        };
192        Self::new(ox, oy, config)
193    }
194
195    /// Create from typed obstacle points.
196    pub fn from_obstacle_points(
197        obstacles: &Obstacles,
198        config: IDAStarConfig,
199    ) -> RoboticsResult<Self> {
200        config.validate()?;
201        let grid_map = GridMap::from_obstacles(obstacles, config.resolution, config.robot_radius)?;
202
203        Ok(Self {
204            grid_map,
205            config,
206            motion: get_motion_model_8(),
207        })
208    }
209
210    /// Plan without importing the trait explicitly.
211    pub fn plan(&self, start: Point2D, goal: Point2D) -> RoboticsResult<Path2D> {
212        self.plan_with_stats(start, goal).map(|(path, _stats)| path)
213    }
214
215    /// Plan from raw coordinates without importing the trait explicitly.
216    pub fn plan_xy(&self, sx: f64, sy: f64, gx: f64, gy: f64) -> RoboticsResult<Path2D> {
217        self.plan_with_stats(Point2D::new(sx, sy), Point2D::new(gx, gy))
218            .map(|(path, _stats)| path)
219    }
220
221    /// Plan a path and return per-query search-effort statistics.
222    pub fn plan_with_stats(
223        &self,
224        start: Point2D,
225        goal: Point2D,
226    ) -> RoboticsResult<(Path2D, IDAStarSearchStats)> {
227        let report = self.plan_with_report(start, goal)?;
228        match report.outcome {
229            IDAStarPlanOutcome::Exact => Ok((
230                report
231                    .path
232                    .expect("exact IDA* report should always include a path"),
233                report.stats,
234            )),
235            IDAStarPlanOutcome::MaxExpandedNodes => Err(RoboticsError::PlanningError(
236                "IDA* exceeded max_expanded_nodes before finding a path".to_string(),
237            )),
238            IDAStarPlanOutcome::MaxIterations => Err(RoboticsError::PlanningError(
239                "IDA* exceeded max_iterations before finding a path".to_string(),
240            )),
241            IDAStarPlanOutcome::NoPath => {
242                Err(RoboticsError::PlanningError("No path found".to_string()))
243            }
244        }
245    }
246
247    /// Plan a path and return the query outcome plus search-effort diagnostics,
248    /// even when the bounded search stops before reaching an exact path.
249    pub fn plan_with_report(
250        &self,
251        start: Point2D,
252        goal: Point2D,
253    ) -> RoboticsResult<IDAStarPlanReport> {
254        self.plan_impl(start, goal)
255    }
256
257    /// Plan a path returning `(rx, ry)` vectors.
258    #[deprecated(note = "use plan() or plan_xy() instead")]
259    pub fn planning(&self, sx: f64, sy: f64, gx: f64, gy: f64) -> Option<(Vec<f64>, Vec<f64>)> {
260        match self.plan_xy(sx, sy, gx, gy) {
261            Ok(path) => Some((path.x_coords(), path.y_coords())),
262            Err(_) => None,
263        }
264    }
265
266    pub fn grid_map(&self) -> &GridMap {
267        &self.grid_map
268    }
269
270    fn calc_heuristic(&self, x: i32, y: i32, goal_x: i32, goal_y: i32) -> f64 {
271        let dx = (x - goal_x).abs() as f64;
272        let dy = (y - goal_y).abs() as f64;
273        let diagonal = dx.min(dy);
274        let straight = dx.max(dy) - diagonal;
275
276        // Octile distance is a tighter admissible lower bound than Euclidean
277        // for an 8-connected grid with unit cardinal and sqrt(2) diagonal cost.
278        self.config.heuristic_weight * (diagonal * SQRT_2 + straight)
279    }
280
281    fn lookahead_lower_bound(
282        &self,
283        current: (i32, i32),
284        goal: (i32, i32),
285        depth: usize,
286        cache: &mut HashMap<(i32, usize), f64>,
287    ) -> f64 {
288        let (current_x, current_y) = current;
289        let (goal_x, goal_y) = goal;
290        if current_x == goal_x && current_y == goal_y {
291            return 0.0;
292        }
293        if depth == 0 {
294            return self.calc_heuristic(current_x, current_y, goal_x, goal_y);
295        }
296
297        let cache_key = (self.grid_map.calc_index(current_x, current_y), depth);
298        if let Some(&cached) = cache.get(&cache_key) {
299            return cached;
300        }
301
302        let mut best = f64::INFINITY;
303        for &(dx, dy, move_cost) in &self.motion {
304            if !self.grid_map.is_valid_offset(current_x, current_y, dx, dy) {
305                continue;
306            }
307
308            let next_x = current_x + dx;
309            let next_y = current_y + dy;
310            let candidate =
311                move_cost + self.lookahead_lower_bound((next_x, next_y), goal, depth - 1, cache);
312            best = best.min(candidate);
313        }
314
315        let value = if best.is_finite() {
316            best
317        } else {
318            self.calc_heuristic(current_x, current_y, goal_x, goal_y)
319        };
320        cache.insert(cache_key, value);
321        value
322    }
323
324    fn calc_initial_threshold(&self, start_x: i32, start_y: i32, goal_x: i32, goal_y: i32) -> f64 {
325        let mut cache = HashMap::new();
326        self.lookahead_lower_bound(
327            (start_x, start_y),
328            (goal_x, goal_y),
329            self.config.initial_lookahead_depth,
330            &mut cache,
331        )
332    }
333
334    fn ensure_query_is_valid(&self, x: i32, y: i32, label: &str) -> RoboticsResult<()> {
335        if self.grid_map.is_valid(x, y) {
336            return Ok(());
337        }
338
339        Err(RoboticsError::PlanningError(format!(
340            "{} position is invalid",
341            label
342        )))
343    }
344
345    fn build_path(&self, path_cells: &[(i32, i32)]) -> Path2D {
346        let points = path_cells
347            .iter()
348            .map(|&(x, y)| {
349                Point2D::new(
350                    self.grid_map.calc_x_position(x),
351                    self.grid_map.calc_y_position(y),
352                )
353            })
354            .collect();
355        Path2D::from_points(points)
356    }
357
358    fn search_contour(
359        &self,
360        current: (i32, i32),
361        g_cost: f64,
362        depth: usize,
363        ctx: &mut SearchContext<'_>,
364    ) -> SearchOutcome {
365        let (current_x, current_y) = current;
366        let (goal_x, goal_y) = ctx.goal;
367        ctx.stats.max_depth = ctx.stats.max_depth.max(depth);
368        let f_cost = g_cost + self.calc_heuristic(current_x, current_y, goal_x, goal_y);
369        if f_cost > ctx.threshold + COST_EPSILON {
370            ctx.stats.threshold_prunes += 1;
371            return SearchOutcome::NextThreshold(f_cost);
372        }
373
374        if self
375            .config
376            .max_expanded_nodes
377            .is_some_and(|limit| ctx.stats.expanded_nodes >= limit)
378        {
379            return SearchOutcome::ExpansionBudgetExceeded;
380        }
381
382        let current_grid_index = self.grid_map.calc_index(current_x, current_y);
383        ctx.stats.expanded_nodes += 1;
384        if ctx.expanded_grid_ids.insert(current_grid_index) {
385            ctx.stats.unique_expanded_nodes += 1;
386        } else {
387            ctx.stats.reexpanded_nodes += 1;
388        }
389
390        if current_x == goal_x && current_y == goal_y {
391            return SearchOutcome::Found;
392        }
393
394        let mut min_exceeded = f64::INFINITY;
395        let mut ordered_neighbors = Vec::with_capacity(self.motion.len());
396
397        for &(dx, dy, move_cost) in &self.motion {
398            if !self.grid_map.is_valid_offset(current_x, current_y, dx, dy) {
399                continue;
400            }
401
402            let next_x = current_x + dx;
403            let next_y = current_y + dy;
404            let next_cost = g_cost + move_cost;
405            let next_f_cost = next_cost + self.calc_heuristic(next_x, next_y, goal_x, goal_y);
406            ordered_neighbors.push((next_f_cost, next_x, next_y, next_cost));
407        }
408
409        ordered_neighbors.sort_by(|lhs, rhs| {
410            lhs.0.total_cmp(&rhs.0).then_with(|| {
411                self.calc_heuristic(lhs.1, lhs.2, goal_x, goal_y)
412                    .total_cmp(&self.calc_heuristic(rhs.1, rhs.2, goal_x, goal_y))
413            })
414        });
415
416        for (_next_f_cost, next_x, next_y, next_cost) in ordered_neighbors {
417            let next_grid_index = self.grid_map.calc_index(next_x, next_y);
418            if ctx.visited.contains(&next_grid_index) {
419                ctx.stats.cycle_prunes += 1;
420                continue;
421            }
422
423            if ctx
424                .best_g_by_grid
425                .get(&next_grid_index)
426                .is_some_and(|&best_g| next_cost >= best_g - COST_EPSILON)
427            {
428                ctx.stats.transposition_prunes += 1;
429                continue;
430            }
431
432            ctx.stats.generated_nodes += 1;
433            ctx.best_g_by_grid.insert(next_grid_index, next_cost);
434            ctx.visited.insert(next_grid_index);
435            ctx.path.push((next_x, next_y));
436
437            match self.search_contour((next_x, next_y), next_cost, depth + 1, ctx) {
438                SearchOutcome::Found => return SearchOutcome::Found,
439                SearchOutcome::ExpansionBudgetExceeded => {
440                    return SearchOutcome::ExpansionBudgetExceeded
441                }
442                SearchOutcome::NextThreshold(limit) => {
443                    min_exceeded = min_exceeded.min(limit);
444                }
445            }
446
447            ctx.path.pop();
448            ctx.visited.remove(&next_grid_index);
449        }
450
451        SearchOutcome::NextThreshold(min_exceeded)
452    }
453
454    fn plan_impl(&self, start: Point2D, goal: Point2D) -> RoboticsResult<IDAStarPlanReport> {
455        let start_x = self.grid_map.calc_x_index(start.x);
456        let start_y = self.grid_map.calc_y_index(start.y);
457        let goal_x = self.grid_map.calc_x_index(goal.x);
458        let goal_y = self.grid_map.calc_y_index(goal.y);
459
460        self.ensure_query_is_valid(start_x, start_y, "Start")?;
461        self.ensure_query_is_valid(goal_x, goal_y, "Goal")?;
462
463        let initial_threshold = self.calc_initial_threshold(start_x, start_y, goal_x, goal_y);
464        let mut threshold = initial_threshold;
465        let mut last_searched_threshold = threshold;
466        let mut next_threshold = None;
467        let start_grid_index = self.grid_map.calc_index(start_x, start_y);
468        let mut stats = IDAStarSearchStats::default();
469        let mut expanded_grid_ids = HashSet::new();
470        let mut contour_history = Vec::new();
471
472        for _ in 0..self.config.max_iterations {
473            last_searched_threshold = threshold;
474            let mut path = vec![(start_x, start_y)];
475            let mut visited = HashSet::new();
476            let mut best_g_by_grid = HashMap::new();
477            visited.insert(start_grid_index);
478            best_g_by_grid.insert(start_grid_index, 0.0);
479            stats.threshold_iterations += 1;
480
481            let snap = stats;
482            let outcome = {
483                let mut ctx = SearchContext {
484                    goal: (goal_x, goal_y),
485                    threshold,
486                    path: &mut path,
487                    visited: &mut visited,
488                    best_g_by_grid: &mut best_g_by_grid,
489                    expanded_grid_ids: &mut expanded_grid_ids,
490                    stats: &mut stats,
491                };
492                self.search_contour((start_x, start_y), 0.0, 1, &mut ctx)
493            };
494
495            let contour_expanded = stats.expanded_nodes - snap.expanded_nodes;
496            let contour_reexpanded = stats.reexpanded_nodes - snap.reexpanded_nodes;
497            contour_history.push(ContourStats {
498                threshold,
499                expanded: contour_expanded,
500                new_unique: contour_expanded - contour_reexpanded,
501                reexpanded: contour_reexpanded,
502                generated: stats.generated_nodes - snap.generated_nodes,
503                threshold_prunes: stats.threshold_prunes - snap.threshold_prunes,
504                cycle_prunes: stats.cycle_prunes - snap.cycle_prunes,
505                transposition_prunes: stats.transposition_prunes - snap.transposition_prunes,
506                max_depth: stats.max_depth,
507            });
508
509            match outcome {
510                SearchOutcome::Found => {
511                    return Ok(IDAStarPlanReport {
512                        outcome: IDAStarPlanOutcome::Exact,
513                        path: Some(self.build_path(&path)),
514                        stats,
515                        initial_threshold,
516                        last_searched_threshold,
517                        next_threshold: None,
518                        contour_history,
519                    });
520                }
521                SearchOutcome::ExpansionBudgetExceeded => {
522                    return Ok(IDAStarPlanReport {
523                        outcome: IDAStarPlanOutcome::MaxExpandedNodes,
524                        path: None,
525                        stats,
526                        initial_threshold,
527                        last_searched_threshold,
528                        next_threshold: None,
529                        contour_history,
530                    });
531                }
532                SearchOutcome::NextThreshold(candidate_threshold)
533                    if candidate_threshold.is_finite() =>
534                {
535                    next_threshold = Some(candidate_threshold);
536                    threshold = candidate_threshold;
537                }
538                SearchOutcome::NextThreshold(_) => {
539                    return Ok(IDAStarPlanReport {
540                        outcome: IDAStarPlanOutcome::NoPath,
541                        path: None,
542                        stats,
543                        initial_threshold,
544                        last_searched_threshold,
545                        next_threshold: None,
546                        contour_history,
547                    });
548                }
549            }
550        }
551
552        Ok(IDAStarPlanReport {
553            outcome: IDAStarPlanOutcome::MaxIterations,
554            path: None,
555            stats,
556            initial_threshold,
557            last_searched_threshold,
558            next_threshold,
559            contour_history,
560        })
561    }
562}
563
564impl PathPlanner for IDAStarPlanner {
565    fn plan(&self, start: Point2D, goal: Point2D) -> Result<Path2D, RoboticsError> {
566        IDAStarPlanner::plan(self, start, goal)
567    }
568}
569
570#[cfg(test)]
571mod tests {
572    use super::*;
573    use crate::a_star::{AStarConfig, AStarPlanner};
574    use crate::moving_ai::{MovingAiMap, MovingAiScenario};
575
576    fn create_simple_obstacles() -> (Vec<f64>, Vec<f64>) {
577        let mut ox = Vec::new();
578        let mut oy = Vec::new();
579
580        for i in 0..11 {
581            ox.push(i as f64);
582            oy.push(0.0);
583            ox.push(i as f64);
584            oy.push(10.0);
585            ox.push(0.0);
586            oy.push(i as f64);
587            ox.push(10.0);
588            oy.push(i as f64);
589        }
590
591        for i in 4..7 {
592            ox.push(5.0);
593            oy.push(i as f64);
594        }
595
596        (ox, oy)
597    }
598
599    #[test]
600    fn test_ida_star_finds_path() {
601        let (ox, oy) = create_simple_obstacles();
602        let planner = IDAStarPlanner::from_obstacles(&ox, &oy, 1.0, 0.5);
603
604        let path = planner
605            .plan(Point2D::new(2.0, 2.0), Point2D::new(8.0, 8.0))
606            .expect("IDA* should find a path on the simple map");
607
608        assert!(!path.is_empty());
609    }
610
611    #[test]
612    fn test_ida_star_try_new_rejects_invalid_config() {
613        let (ox, oy) = create_simple_obstacles();
614        let config = IDAStarConfig {
615            max_iterations: 0,
616            ..Default::default()
617        };
618
619        let err = IDAStarPlanner::try_new(&ox, &oy, config)
620            .expect_err("invalid max_iterations should be rejected");
621        assert!(matches!(err, RoboticsError::InvalidParameter(_)));
622    }
623
624    #[test]
625    fn test_ida_star_try_new_rejects_zero_expansion_budget() {
626        let (ox, oy) = create_simple_obstacles();
627        let config = IDAStarConfig {
628            max_expanded_nodes: Some(0),
629            ..Default::default()
630        };
631
632        let err = IDAStarPlanner::try_new(&ox, &oy, config)
633            .expect_err("zero expansion budget should be rejected");
634        assert!(matches!(err, RoboticsError::InvalidParameter(_)));
635    }
636
637    #[test]
638    fn test_ida_star_matches_simple_map_length() {
639        let (ox, oy) = create_simple_obstacles();
640        let planner = IDAStarPlanner::from_obstacles(&ox, &oy, 1.0, 0.5);
641        let a_star = AStarPlanner::new(
642            &ox,
643            &oy,
644            AStarConfig {
645                resolution: 1.0,
646                robot_radius: 0.5,
647                heuristic_weight: 1.0,
648            },
649        );
650
651        let path = planner
652            .plan(Point2D::new(2.0, 2.0), Point2D::new(8.0, 8.0))
653            .expect("IDA* should find a path on the simple map");
654        let reference = a_star
655            .plan(Point2D::new(2.0, 2.0), Point2D::new(8.0, 8.0))
656            .expect("A* should find the same reference path on the simple map");
657
658        assert!(
659            (path.total_length() - reference.total_length()).abs() < 1e-6,
660            "simple-map path length {} should match A* reference length {}",
661            path.total_length(),
662            reference.total_length()
663        );
664    }
665
666    #[test]
667    #[ignore = "long-running MovingAI benchmark"]
668    fn test_ida_star_matches_sample_moving_ai_optimal_length() {
669        let map = MovingAiMap::parse_str(include_str!("testdata/moving_ai/sample.map"))
670            .expect("sample MovingAI map should parse");
671        let scenario =
672            MovingAiScenario::parse_str(include_str!("testdata/moving_ai/sample.map.scen"))
673                .expect("sample MovingAI scenarios should parse")
674                .into_iter()
675                .next()
676                .expect("sample MovingAI scenario should exist");
677
678        let planner = IDAStarPlanner::from_obstacle_points(
679            &map.to_obstacles(),
680            IDAStarConfig {
681                resolution: 1.0,
682                robot_radius: 0.5,
683                heuristic_weight: 1.0,
684                initial_lookahead_depth: 2,
685                max_iterations: 10_000,
686                max_expanded_nodes: None,
687            },
688        )
689        .expect("IDA* planner should build from sample obstacles");
690
691        let start = map
692            .planning_point(scenario.start_x, scenario.start_y)
693            .expect("sample start should be valid");
694        let goal = map
695            .planning_point(scenario.goal_x, scenario.goal_y)
696            .expect("sample goal should be valid");
697
698        let path = planner
699            .plan(start, goal)
700            .expect("IDA* should solve the sample scenario");
701
702        assert!(
703            (path.total_length() - scenario.optimal_length).abs() < 1e-6,
704            "IDA* path length {} should match sample MovingAI optimal {} when corner cutting is disabled",
705            path.total_length(),
706            scenario.optimal_length
707        );
708    }
709
710    #[test]
711    fn test_ida_star_report_keeps_failure_effort_on_iteration_limit() {
712        let (ox, oy) = create_simple_obstacles();
713        let planner = IDAStarPlanner::try_new(
714            &ox,
715            &oy,
716            IDAStarConfig {
717                max_iterations: 1,
718                max_expanded_nodes: Some(10_000),
719                ..Default::default()
720            },
721        )
722        .expect("bounded IDA* planner should build");
723
724        let report = planner
725            .plan_with_report(Point2D::new(2.0, 2.0), Point2D::new(8.0, 8.0))
726            .expect("valid query should return an IDA* report");
727
728        assert_eq!(report.outcome, IDAStarPlanOutcome::MaxIterations);
729        assert!(report.path.is_none());
730        assert_eq!(report.stats.threshold_iterations, 1);
731        assert!(report.stats.expanded_nodes > 0);
732        assert!(report.stats.unique_expanded_nodes > 0);
733        assert!(report.next_threshold.is_some());
734        assert!(report.next_threshold.unwrap() > report.last_searched_threshold);
735    }
736
737    #[test]
738    fn test_ida_star_report_keeps_failure_effort_on_expansion_limit() {
739        let (ox, oy) = create_simple_obstacles();
740        let planner = IDAStarPlanner::try_new(
741            &ox,
742            &oy,
743            IDAStarConfig {
744                max_iterations: 10_000,
745                max_expanded_nodes: Some(1),
746                ..Default::default()
747            },
748        )
749        .expect("bounded IDA* planner should build");
750
751        let report = planner
752            .plan_with_report(Point2D::new(2.0, 2.0), Point2D::new(8.0, 8.0))
753            .expect("valid query should return an IDA* report");
754
755        assert_eq!(report.outcome, IDAStarPlanOutcome::MaxExpandedNodes);
756        assert!(report.path.is_none());
757        assert_eq!(report.stats.expanded_nodes, 1);
758        assert_eq!(report.stats.unique_expanded_nodes, 1);
759        assert_eq!(report.stats.reexpanded_nodes, 0);
760        assert!(report.next_threshold.is_none());
761    }
762}
rust_robotics_planning/ida_star.rs

rust_robotics_planning/
ida_star.rs
