Skip to main content

rust_robotics_planning/
hybrid_a_star.rs

1//! Hybrid A* path planning algorithm
2//!
3//! Plans paths for car-like robots with non-holonomic constraints
4//! by combining A* grid search with continuous-space vehicle motion
5//! primitives and Reeds-Shepp analytic expansion.
6
7use std::cmp::Ordering;
8use std::collections::{BinaryHeap, HashMap};
9use std::f64::consts::PI;
10
11use crate::grid::GridMap;
12use crate::reeds_shepp_path;
13use rust_robotics_core::normalize_angle;
14use rust_robotics_core::{RoboticsError, RoboticsResult};
15
16/// Configuration for Hybrid A* planner
17#[derive(Debug, Clone)]
18pub struct HybridAStarConfig {
19    /// Grid resolution in meters
20    pub xy_resolution: f64,
21    /// Yaw angle resolution in radians
22    pub yaw_resolution: f64,
23    /// Robot wheelbase (distance between axles)
24    pub wheelbase: f64,
25    /// Maximum steering angle in radians
26    pub max_steer: f64,
27    /// Number of discrete steering inputs
28    pub n_steer: usize,
29    /// Distance traveled per expansion step
30    pub step_size: f64,
31    /// Robot radius for collision checking
32    pub robot_radius: f64,
33    /// Maximum curvature (1 / minimum turning radius)
34    pub max_curvature: f64,
35    /// Cost multiplier for switching direction
36    pub switch_back_cost: f64,
37    /// Cost multiplier for steering angle
38    pub steer_cost: f64,
39    /// Cost multiplier for changing steering
40    pub steer_change_cost: f64,
41    /// Heuristic weight for holonomic-with-obstacles heuristic
42    pub h_cost: f64,
43    /// Interval for attempting Reeds-Shepp analytic expansion
44    pub analytic_expansion_interval: usize,
45}
46
47impl Default for HybridAStarConfig {
48    fn default() -> Self {
49        let wheelbase = 3.0;
50        let max_steer: f64 = 0.6;
51        let max_curvature = (max_steer).tan() / wheelbase;
52        Self {
53            xy_resolution: 2.0,
54            yaw_resolution: PI / 36.0, // 5 degrees
55            wheelbase,
56            max_steer,
57            n_steer: 20,
58            step_size: 1.5,
59            robot_radius: 1.5,
60            max_curvature,
61            switch_back_cost: 100.0,
62            steer_cost: 1.0,
63            steer_change_cost: 5.0,
64            h_cost: 1.0,
65            analytic_expansion_interval: 5,
66        }
67    }
68}
69
70impl HybridAStarConfig {
71    pub fn validate(&self) -> RoboticsResult<()> {
72        if !self.xy_resolution.is_finite() || self.xy_resolution <= 0.0 {
73            return Err(RoboticsError::InvalidParameter(format!(
74                "xy_resolution must be positive and finite, got {}",
75                self.xy_resolution
76            )));
77        }
78        if !self.yaw_resolution.is_finite() || self.yaw_resolution <= 0.0 {
79            return Err(RoboticsError::InvalidParameter(format!(
80                "yaw_resolution must be positive and finite, got {}",
81                self.yaw_resolution
82            )));
83        }
84        if !self.wheelbase.is_finite() || self.wheelbase <= 0.0 {
85            return Err(RoboticsError::InvalidParameter(format!(
86                "wheelbase must be positive and finite, got {}",
87                self.wheelbase
88            )));
89        }
90        if self.n_steer == 0 {
91            return Err(RoboticsError::InvalidParameter(
92                "n_steer must be at least 1".to_string(),
93            ));
94        }
95        Ok(())
96    }
97}
98
99/// A node in the Hybrid A* search tree
100#[derive(Debug, Clone)]
101struct HybridNode {
102    /// Grid x index
103    x_index: i32,
104    /// Grid y index
105    y_index: i32,
106    /// Yaw index (discretized)
107    yaw_index: i32,
108    /// Direction: true = forward, false = backward
109    direction: bool,
110    /// Continuous x positions along this node's arc
111    x_list: Vec<f64>,
112    /// Continuous y positions along this node's arc
113    y_list: Vec<f64>,
114    /// Continuous yaw angles along this node's arc
115    yaw_list: Vec<f64>,
116    /// Direction list (1 = forward, -1 = backward) for each point
117    directions: Vec<i32>,
118    /// Steering angle used
119    steer: f64,
120    /// Accumulated cost
121    cost: f64,
122    /// Parent node index in storage
123    parent_index: Option<usize>,
124}
125
126/// Priority wrapper for BinaryHeap (min-heap)
127#[derive(Debug)]
128struct PriorityNode {
129    index: usize,
130    cost: f64,
131}
132
133impl Eq for PriorityNode {}
134
135impl PartialEq for PriorityNode {
136    fn eq(&self, other: &Self) -> bool {
137        self.cost == other.cost
138    }
139}
140
141impl Ord for PriorityNode {
142    fn cmp(&self, other: &Self) -> Ordering {
143        other
144            .cost
145            .partial_cmp(&self.cost)
146            .unwrap_or(Ordering::Equal)
147    }
148}
149
150impl PartialOrd for PriorityNode {
151    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
152        Some(self.cmp(other))
153    }
154}
155
156/// Result of Hybrid A* planning
157#[derive(Debug, Clone)]
158pub struct HybridAStarPath {
159    /// X coordinates along the path
160    pub x: Vec<f64>,
161    /// Y coordinates along the path
162    pub y: Vec<f64>,
163    /// Yaw angles along the path
164    pub yaw: Vec<f64>,
165    /// Direction at each point (1 = forward, -1 = backward)
166    pub directions: Vec<i32>,
167}
168
169impl HybridAStarPath {
170    pub fn is_empty(&self) -> bool {
171        self.x.is_empty()
172    }
173
174    pub fn len(&self) -> usize {
175        self.x.len()
176    }
177}
178
179/// Rectangular footprint of a car, measured from the rear axle (the point
180/// the planner's poses refer to).
181#[derive(Debug, Clone, Copy, PartialEq)]
182pub struct VehicleFootprint {
183    /// Rear axle to front bumper \[m\].
184    pub front: f64,
185    /// Rear axle to rear bumper \[m\].
186    pub rear: f64,
187    /// Overall width \[m\].
188    pub width: f64,
189}
190
191impl VehicleFootprint {
192    /// Circles `(offset along the heading from the rear axle, radius)` that
193    /// together cover the rectangle: one per width-sized slice of its length.
194    ///
195    /// At most 16 circles; a degenerate footprint (non-positive width or
196    /// length) gets a single circle.
197    pub fn covering_circles(&self) -> Vec<(f64, f64)> {
198        let length = (self.front + self.rear).max(0.0);
199        let n = if self.width > 0.0 && length > 0.0 {
200            ((length / self.width).ceil() as usize).clamp(1, 16)
201        } else {
202            1
203        };
204        let slice = length / n as f64;
205        let radius = (slice / 2.0).hypot(self.width / 2.0);
206        (0..n)
207            .map(|i| (-self.rear + (i as f64 + 0.5) * slice, radius))
208            .collect()
209    }
210
211    fn validate(&self) -> RoboticsResult<()> {
212        let ok = |v: f64| v.is_finite() && v >= 0.0;
213        if !ok(self.front) || !ok(self.rear) || !(self.width.is_finite() && self.width > 0.0) {
214            return Err(RoboticsError::InvalidParameter(format!(
215                "invalid vehicle footprint {self:?}"
216            )));
217        }
218        if self.front + self.rear <= 0.0 {
219            return Err(RoboticsError::InvalidParameter(
220                "vehicle footprint must have a positive length".to_string(),
221            ));
222        }
223        Ok(())
224    }
225}
226
227/// Hybrid A* path planner
228pub struct HybridAStarPlanner {
229    config: HybridAStarConfig,
230    grid_map: GridMap,
231    /// Offsets along the heading of the collision circles' centers; just
232    /// the rear axle (`[0.0]`) for a round robot.
233    circle_offsets: Vec<f64>,
234    /// Finer grid for collision checks of a vehicle footprint (the search
235    /// grid is too coarse to place a car's corners).
236    collision_map: Option<GridMap>,
237    /// Search budget: node expansions before giving up.
238    max_expansions: usize,
239    /// Precomputed holonomic heuristic (cost from each grid cell to goal)
240    h_map: Vec<Vec<f64>>,
241    goal_x: i32,
242    goal_y: i32,
243}
244
245impl HybridAStarPlanner {
246    /// Create a new planner from obstacle coordinates
247    pub fn new(ox: &[f64], oy: &[f64], config: HybridAStarConfig) -> RoboticsResult<Self> {
248        config.validate()?;
249        let grid_map = GridMap::try_new(ox, oy, config.xy_resolution, config.robot_radius)?;
250
251        Ok(Self {
252            config,
253            grid_map,
254            circle_offsets: vec![0.0],
255            collision_map: None,
256            max_expansions: usize::MAX,
257            h_map: Vec::new(),
258            goal_x: 0,
259            goal_y: 0,
260        })
261    }
262
263    /// A planner for a car with a rectangular `footprint`: every pose is
264    /// checked with circles covering the car instead of one circle of
265    /// `config.robot_radius` at the rear axle (which is ignored).
266    pub fn with_vehicle(
267        ox: &[f64],
268        oy: &[f64],
269        config: HybridAStarConfig,
270        footprint: VehicleFootprint,
271    ) -> RoboticsResult<Self> {
272        footprint.validate()?;
273        let circles = footprint.covering_circles();
274        let radius = circles[0].1;
275        let mut planner = Self::new(
276            ox,
277            oy,
278            HybridAStarConfig {
279                robot_radius: radius,
280                ..config
281            },
282        )?;
283        // Circle centers are looked up in the cell they round to, up to half
284        // a cell diagonal away: check them on a finer grid inflated by that
285        // much more, so the car's corners never reach an obstacle.
286        let resolution = (planner.config.xy_resolution / 5.0).max(0.05);
287        planner.collision_map = Some(GridMap::try_new(
288            ox,
289            oy,
290            resolution,
291            radius + resolution * std::f64::consts::SQRT_2 / 2.0,
292        )?);
293        planner.circle_offsets = circles.into_iter().map(|(offset, _)| offset).collect();
294        Ok(planner)
295    }
296
297    /// Gives up (with an error) after `max_expansions` node expansions, so an
298    /// unreachable goal fails fast instead of searching the whole state
299    /// space.
300    pub fn with_max_expansions(mut self, max_expansions: usize) -> Self {
301        self.max_expansions = max_expansions.max(1);
302        self
303    }
304
305    /// Plan a path from start pose to goal pose
306    pub fn plan(
307        &mut self,
308        sx: f64,
309        sy: f64,
310        syaw: f64,
311        gx: f64,
312        gy: f64,
313        gyaw: f64,
314    ) -> RoboticsResult<HybridAStarPath> {
315        // The start and goal themselves must be free, or nothing can be
316        // planned (checked before the costly heuristic map).
317        if !self.verify_path(&[sx], &[sy], &[syaw]) {
318            return Err(RoboticsError::PlanningError(
319                "Hybrid A*: the start pose collides".to_string(),
320            ));
321        }
322        if !self.verify_path(&[gx], &[gy], &[gyaw]) {
323            return Err(RoboticsError::PlanningError(
324                "Hybrid A*: the goal pose collides".to_string(),
325            ));
326        }
327
328        // Compute grid indices for goal
329        self.goal_x = self.grid_map.calc_x_index(gx);
330        self.goal_y = self.grid_map.calc_y_index(gy);
331
332        // Compute holonomic heuristic map (Dijkstra from goal)
333        self.h_map = self.compute_holonomic_heuristic();
334
335        let start_yaw_index = self.calc_yaw_index(syaw);
336        let start_node = HybridNode {
337            x_index: self.grid_map.calc_x_index(sx),
338            y_index: self.grid_map.calc_y_index(sy),
339            yaw_index: start_yaw_index,
340            direction: true,
341            x_list: vec![sx],
342            y_list: vec![sy],
343            yaw_list: vec![syaw],
344            directions: vec![1],
345            steer: 0.0,
346            cost: 0.0,
347            parent_index: None,
348        };
349
350        let goal_yaw_index = self.calc_yaw_index(gyaw);
351        let goal_node = HybridNode {
352            x_index: self.goal_x,
353            y_index: self.goal_y,
354            yaw_index: goal_yaw_index,
355            direction: true,
356            x_list: vec![gx],
357            y_list: vec![gy],
358            yaw_list: vec![gyaw],
359            directions: vec![1],
360            steer: 0.0,
361            cost: 0.0,
362            parent_index: None,
363        };
364
365        let mut open_set = BinaryHeap::new();
366        let mut closed_set: HashMap<i64, usize> = HashMap::new();
367        let mut node_storage: Vec<HybridNode> = Vec::new();
368
369        node_storage.push(start_node.clone());
370        let start_idx = 0;
371        let start_key = self.calc_hybrid_index(&node_storage[start_idx]);
372
373        let h = self.calc_heuristic(&node_storage[start_idx]);
374        open_set.push(PriorityNode {
375            index: start_idx,
376            cost: node_storage[start_idx].cost + h,
377        });
378
379        let mut open_map: HashMap<i64, usize> = HashMap::new();
380        open_map.insert(start_key, start_idx);
381
382        let mut iteration = 0;
383
384        while let Some(current_priority) = open_set.pop() {
385            if iteration >= self.max_expansions {
386                return Err(RoboticsError::PlanningError(
387                    "Hybrid A*: search budget exhausted".to_string(),
388                ));
389            }
390            let current_idx = current_priority.index;
391            let current_key = self.calc_hybrid_index(&node_storage[current_idx]);
392
393            if closed_set.contains_key(&current_key) {
394                continue;
395            }
396
397            closed_set.insert(current_key, current_idx);
398            open_map.remove(&current_key);
399
400            // Check goal reached
401            if self.is_goal(&node_storage[current_idx], &goal_node) {
402                return Ok(self.build_path(current_idx, &node_storage, None));
403            }
404
405            // Try analytic expansion (Reeds-Shepp)
406            iteration += 1;
407            if iteration % self.config.analytic_expansion_interval == 0 {
408                if let Some(rs) = self.analytic_expansion(&node_storage[current_idx], &goal_node) {
409                    return Ok(self.build_path(current_idx, &node_storage, Some(&rs)));
410                }
411            }
412
413            // Expand neighbors with motion primitives
414            let neighbors = self.get_neighbors(&node_storage[current_idx], current_idx);
415            for neighbor in neighbors {
416                let neighbor_key = self.calc_hybrid_index(&neighbor);
417
418                if closed_set.contains_key(&neighbor_key) {
419                    continue;
420                }
421
422                if let Some(&existing_idx) = open_map.get(&neighbor_key) {
423                    if neighbor.cost < node_storage[existing_idx].cost {
424                        node_storage.push(neighbor);
425                        let new_idx = node_storage.len() - 1;
426                        open_map.insert(neighbor_key, new_idx);
427                        let h = self.calc_heuristic(&node_storage[new_idx]);
428                        open_set.push(PriorityNode {
429                            index: new_idx,
430                            cost: node_storage[new_idx].cost + h,
431                        });
432                    }
433                } else {
434                    node_storage.push(neighbor);
435                    let new_idx = node_storage.len() - 1;
436                    open_map.insert(neighbor_key, new_idx);
437                    let h = self.calc_heuristic(&node_storage[new_idx]);
438                    open_set.push(PriorityNode {
439                        index: new_idx,
440                        cost: node_storage[new_idx].cost + h,
441                    });
442                }
443            }
444        }
445
446        Err(RoboticsError::PlanningError(
447            "Hybrid A*: no path found".to_string(),
448        ))
449    }
450
451    /// Discretize yaw to grid index
452    fn calc_yaw_index(&self, yaw: f64) -> i32 {
453        let normalized = normalize_angle(yaw);
454        ((normalized + PI) / self.config.yaw_resolution).round() as i32
455    }
456
457    /// Compute a unique index for the hybrid state (x, y, yaw)
458    fn calc_hybrid_index(&self, node: &HybridNode) -> i64 {
459        let n_yaw = ((2.0 * PI) / self.config.yaw_resolution).round() as i64 + 1;
460        (node.yaw_index as i64)
461            + n_yaw * (node.y_index as i64)
462            + n_yaw * (self.grid_map.y_width as i64) * (node.x_index as i64)
463    }
464
465    /// Check if node is at goal within grid tolerance
466    fn is_goal(&self, node: &HybridNode, goal: &HybridNode) -> bool {
467        node.x_index == goal.x_index
468            && node.y_index == goal.y_index
469            && node.yaw_index == goal.yaw_index
470    }
471
472    /// Get heuristic cost from holonomic A* map
473    fn calc_heuristic(&self, node: &HybridNode) -> f64 {
474        let ix = node.x_index;
475        let iy = node.y_index;
476
477        if ix >= 0 && ix < self.grid_map.x_width && iy >= 0 && iy < self.grid_map.y_width {
478            self.h_map[ix as usize][iy as usize] * self.config.h_cost
479        } else {
480            // Fallback: Euclidean distance
481            let dx = (node.x_index - self.goal_x) as f64;
482            let dy = (node.y_index - self.goal_y) as f64;
483            (dx * dx + dy * dy).sqrt() * self.config.h_cost
484        }
485    }
486
487    /// Generate neighbor nodes using bicycle model motion primitives
488    fn get_neighbors(&self, current: &HybridNode, parent_idx: usize) -> Vec<HybridNode> {
489        let mut neighbors = Vec::new();
490
491        let n_steer = self.config.n_steer as f64;
492        let steer_step = 2.0 * self.config.max_steer / n_steer;
493
494        // For each steering angle
495        for i in 0..=self.config.n_steer {
496            let steer = -self.config.max_steer + i as f64 * steer_step;
497
498            // Forward and backward
499            for &direction in &[1.0_f64, -1.0] {
500                let current_x = *current.x_list.last().unwrap();
501                let current_y = *current.y_list.last().unwrap();
502                let current_yaw = *current.yaw_list.last().unwrap();
503
504                if let Some(node) = self.simulate_motion(
505                    current_x,
506                    current_y,
507                    current_yaw,
508                    steer,
509                    direction,
510                    current,
511                    parent_idx,
512                ) {
513                    neighbors.push(node);
514                }
515            }
516        }
517
518        neighbors
519    }
520
521    /// Simulate bicycle model forward by one step
522    #[allow(clippy::too_many_arguments)]
523    fn simulate_motion(
524        &self,
525        x: f64,
526        y: f64,
527        yaw: f64,
528        steer: f64,
529        direction: f64,
530        parent: &HybridNode,
531        parent_idx: usize,
532    ) -> Option<HybridNode> {
533        let arc_length = self.config.xy_resolution * 1.5;
534        let n_steps = (arc_length / self.config.step_size).ceil() as usize;
535        let d = direction * self.config.step_size;
536
537        let mut cx = x;
538        let mut cy = y;
539        let mut cyaw = yaw;
540
541        let mut x_list = vec![cx];
542        let mut y_list = vec![cy];
543        let mut yaw_list = vec![cyaw];
544
545        for _ in 0..n_steps {
546            cx += d * cyaw.cos();
547            cy += d * cyaw.sin();
548            cyaw += d * steer.tan() / self.config.wheelbase;
549            cyaw = normalize_angle(cyaw);
550
551            x_list.push(cx);
552            y_list.push(cy);
553            yaw_list.push(cyaw);
554        }
555
556        // Collision check
557        if !self.verify_path(&x_list, &y_list, &yaw_list) {
558            return None;
559        }
560
561        let dir_val = if direction > 0.0 { 1 } else { -1 };
562        let directions = vec![dir_val; x_list.len()];
563
564        // Compute cost
565        let mut cost = parent.cost;
566
567        // Distance cost
568        cost += arc_length;
569
570        // Steering cost
571        cost += self.config.steer_cost * steer.abs();
572
573        // Steering change cost
574        cost += self.config.steer_change_cost * (steer - parent.steer).abs();
575
576        // Direction switch cost
577        let parent_dir = if parent.direction { 1.0 } else { -1.0 };
578        if direction != parent_dir {
579            cost += self.config.switch_back_cost;
580        }
581
582        let x_idx = self.grid_map.calc_x_index(cx);
583        let y_idx = self.grid_map.calc_y_index(cy);
584        let yaw_idx = self.calc_yaw_index(cyaw);
585
586        Some(HybridNode {
587            x_index: x_idx,
588            y_index: y_idx,
589            yaw_index: yaw_idx,
590            direction: direction > 0.0,
591            x_list,
592            y_list,
593            yaw_list,
594            directions,
595            steer,
596            cost,
597            parent_index: Some(parent_idx),
598        })
599    }
600
601    /// Verify that all poses in a path are collision-free (every collision
602    /// circle of the vehicle at every pose).
603    fn verify_path(&self, x_list: &[f64], y_list: &[f64], yaw_list: &[f64]) -> bool {
604        let map = self.collision_map.as_ref().unwrap_or(&self.grid_map);
605        for ((&x, &y), &yaw) in x_list.iter().zip(y_list).zip(yaw_list) {
606            let (s, c) = yaw.sin_cos();
607            for &offset in &self.circle_offsets {
608                let ix = map.calc_x_index(x + offset * c);
609                let iy = map.calc_y_index(y + offset * s);
610                if !map.is_valid(ix, iy) {
611                    return false;
612                }
613            }
614        }
615        true
616    }
617
618    /// Attempt analytic expansion using Reeds-Shepp path
619    fn analytic_expansion(&self, current: &HybridNode, goal: &HybridNode) -> Option<HybridNode> {
620        let sx = *current.x_list.last().unwrap();
621        let sy = *current.y_list.last().unwrap();
622        let syaw = *current.yaw_list.last().unwrap();
623
624        let gx = *goal.x_list.last().unwrap();
625        let gy = *goal.y_list.last().unwrap();
626        let gyaw = *goal.yaw_list.last().unwrap();
627
628        // Cheapest collision-free Reeds-Shepp path, counting each gear
629        // change like the search does.
630        let mut candidates = reeds_shepp_path::reeds_shepp_paths(
631            sx,
632            sy,
633            syaw,
634            gx,
635            gy,
636            gyaw,
637            self.config.max_curvature,
638            self.config.step_size,
639        );
640        let arriving = if current.direction { 1 } else { -1 };
641        let cost = |path: &reeds_shepp_path::Path| {
642            let switches = path.directions.windows(2).filter(|w| w[0] != w[1]).count()
643                + usize::from(path.directions.first().is_some_and(|&d| d != arriving));
644            path.l + self.config.switch_back_cost * switches as f64
645        };
646        candidates.sort_by(|a, b| cost(a).total_cmp(&cost(b)));
647        let path = candidates
648            .into_iter()
649            .find(|path| !path.x.is_empty() && self.verify_path(&path.x, &path.y, &path.yaw))?;
650        let (path_x, path_y, path_yaw, directions) = (path.x, path.y, path.yaw, path.directions);
651
652        let last_x = *path_x.last().unwrap();
653        let last_y = *path_y.last().unwrap();
654        let last_yaw = *path_yaw.last().unwrap();
655
656        Some(HybridNode {
657            x_index: self.grid_map.calc_x_index(last_x),
658            y_index: self.grid_map.calc_y_index(last_y),
659            yaw_index: self.calc_yaw_index(last_yaw),
660            direction: true,
661            x_list: path_x,
662            y_list: path_y,
663            yaw_list: path_yaw,
664            directions,
665            steer: 0.0,
666            cost: 0.0,
667            parent_index: None,
668        })
669    }
670
671    /// Build the final path by tracing back from goal to start
672    fn build_path(
673        &self,
674        final_idx: usize,
675        storage: &[HybridNode],
676        rs_segment: Option<&HybridNode>,
677    ) -> HybridAStarPath {
678        #[allow(clippy::type_complexity)]
679        let mut segments: Vec<(Vec<f64>, Vec<f64>, Vec<f64>, Vec<i32>)> = Vec::new();
680
681        let mut current_idx = Some(final_idx);
682        while let Some(idx) = current_idx {
683            let node = &storage[idx];
684            segments.push((
685                node.x_list.clone(),
686                node.y_list.clone(),
687                node.yaw_list.clone(),
688                node.directions.clone(),
689            ));
690            current_idx = node.parent_index;
691        }
692
693        segments.reverse();
694
695        let mut x = Vec::new();
696        let mut y = Vec::new();
697        let mut yaw = Vec::new();
698        let mut directions = Vec::new();
699
700        for (sx, sy, syaw, sd) in &segments {
701            x.extend_from_slice(sx);
702            y.extend_from_slice(sy);
703            yaw.extend_from_slice(syaw);
704            directions.extend_from_slice(sd);
705        }
706
707        // Append Reeds-Shepp segment if present
708        if let Some(rs) = rs_segment {
709            x.extend_from_slice(&rs.x_list);
710            y.extend_from_slice(&rs.y_list);
711            yaw.extend_from_slice(&rs.yaw_list);
712            directions.extend_from_slice(&rs.directions);
713        }
714
715        HybridAStarPath {
716            x,
717            y,
718            yaw,
719            directions,
720        }
721    }
722
723    /// Compute holonomic heuristic using Dijkstra from goal on the grid
724    fn compute_holonomic_heuristic(&self) -> Vec<Vec<f64>> {
725        let xw = self.grid_map.x_width as usize;
726        let yw = self.grid_map.y_width as usize;
727
728        let mut cost_map = vec![vec![f64::INFINITY; yw]; xw];
729
730        let gx = self.goal_x;
731        let gy = self.goal_y;
732
733        if gx < 0 || gx >= self.grid_map.x_width || gy < 0 || gy >= self.grid_map.y_width {
734            return cost_map;
735        }
736
737        cost_map[gx as usize][gy as usize] = 0.0;
738
739        let mut open_set = BinaryHeap::new();
740        open_set.push(DijkstraNode {
741            x: gx,
742            y: gy,
743            cost: 0.0,
744        });
745
746        let motions: [(i32, i32, f64); 8] = [
747            (1, 0, 1.0),
748            (0, 1, 1.0),
749            (-1, 0, 1.0),
750            (0, -1, 1.0),
751            (-1, -1, std::f64::consts::SQRT_2),
752            (-1, 1, std::f64::consts::SQRT_2),
753            (1, -1, std::f64::consts::SQRT_2),
754            (1, 1, std::f64::consts::SQRT_2),
755        ];
756
757        while let Some(current) = open_set.pop() {
758            if current.cost > cost_map[current.x as usize][current.y as usize] {
759                continue;
760            }
761
762            for &(dx, dy, mc) in &motions {
763                let nx = current.x + dx;
764                let ny = current.y + dy;
765
766                if nx < 0 || nx >= self.grid_map.x_width || ny < 0 || ny >= self.grid_map.y_width {
767                    continue;
768                }
769
770                if self.grid_map.obstacle_map[nx as usize][ny as usize] {
771                    continue;
772                }
773
774                let new_cost = current.cost + mc;
775                if new_cost < cost_map[nx as usize][ny as usize] {
776                    cost_map[nx as usize][ny as usize] = new_cost;
777                    open_set.push(DijkstraNode {
778                        x: nx,
779                        y: ny,
780                        cost: new_cost,
781                    });
782                }
783            }
784        }
785
786        cost_map
787    }
788}
789
790/// Dijkstra node for holonomic heuristic computation
791#[derive(Debug)]
792struct DijkstraNode {
793    x: i32,
794    y: i32,
795    cost: f64,
796}
797
798impl Eq for DijkstraNode {}
799
800impl PartialEq for DijkstraNode {
801    fn eq(&self, other: &Self) -> bool {
802        self.cost == other.cost
803    }
804}
805
806impl Ord for DijkstraNode {
807    fn cmp(&self, other: &Self) -> Ordering {
808        other
809            .cost
810            .partial_cmp(&self.cost)
811            .unwrap_or(Ordering::Equal)
812    }
813}
814
815impl PartialOrd for DijkstraNode {
816    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
817        Some(self.cmp(other))
818    }
819}
820
821#[cfg(test)]
822mod tests {
823    use super::*;
824
825    /// Create a rectangular boundary with obstacles
826    fn create_boundary_obstacles(
827        x_min: f64,
828        x_max: f64,
829        y_min: f64,
830        y_max: f64,
831        step: f64,
832    ) -> (Vec<f64>, Vec<f64>) {
833        let mut ox = Vec::new();
834        let mut oy = Vec::new();
835
836        let mut x = x_min;
837        while x <= x_max {
838            ox.push(x);
839            oy.push(y_min);
840            ox.push(x);
841            oy.push(y_max);
842            x += step;
843        }
844
845        let mut y = y_min;
846        while y <= y_max {
847            ox.push(x_min);
848            oy.push(y);
849            ox.push(x_max);
850            oy.push(y);
851            y += step;
852        }
853
854        (ox, oy)
855    }
856
857    #[test]
858    fn test_open_space_path() {
859        let (ox, oy) = create_boundary_obstacles(0.0, 50.0, 0.0, 50.0, 1.0);
860
861        let config = HybridAStarConfig {
862            xy_resolution: 2.0,
863            yaw_resolution: PI / 18.0,
864            robot_radius: 1.0,
865            ..Default::default()
866        };
867
868        let mut planner = HybridAStarPlanner::new(&ox, &oy, config).unwrap();
869
870        let result = planner.plan(10.0, 10.0, 0.0, 40.0, 40.0, 0.0);
871        assert!(result.is_ok(), "Planning failed: {:?}", result.err());
872
873        let path = result.unwrap();
874        assert!(!path.is_empty());
875        assert_eq!(path.x.len(), path.y.len());
876        assert_eq!(path.x.len(), path.yaw.len());
877
878        // Check path starts near start
879        let start_dist = ((path.x[0] - 10.0).powi(2) + (path.y[0] - 10.0).powi(2)).sqrt();
880        assert!(
881            start_dist < 5.0,
882            "Path start too far from start: {}",
883            start_dist
884        );
885
886        let last = path.x.len() - 1;
887        let goal_dist = ((path.x[last] - 40.0).powi(2) + (path.y[last] - 40.0).powi(2)).sqrt();
888        assert!(
889            goal_dist < 10.0,
890            "Path end too far from goal: {}",
891            goal_dist
892        );
893    }
894
895    #[test]
896    fn test_path_around_obstacle() {
897        let (mut ox, mut oy) = create_boundary_obstacles(0.0, 50.0, 0.0, 50.0, 1.0);
898
899        // Add a wall in the middle
900        let mut y = 10.0;
901        while y <= 40.0 {
902            ox.push(25.0);
903            oy.push(y);
904            y += 1.0;
905        }
906
907        let config = HybridAStarConfig {
908            xy_resolution: 2.0,
909            yaw_resolution: PI / 18.0,
910            robot_radius: 1.0,
911            ..Default::default()
912        };
913
914        let mut planner = HybridAStarPlanner::new(&ox, &oy, config).unwrap();
915
916        let result = planner.plan(10.0, 25.0, 0.0, 40.0, 25.0, 0.0);
917        assert!(
918            result.is_ok(),
919            "Planning with obstacle failed: {:?}",
920            result.err()
921        );
922
923        let path = result.unwrap();
924        assert!(!path.is_empty());
925        assert!(path.len() > 5, "Path should have multiple waypoints");
926    }
927
928    #[test]
929    fn test_goal_tolerance() {
930        let (ox, oy) = create_boundary_obstacles(0.0, 50.0, 0.0, 50.0, 1.0);
931
932        let config = HybridAStarConfig {
933            xy_resolution: 2.0,
934            yaw_resolution: PI / 18.0,
935            robot_radius: 1.0,
936            ..Default::default()
937        };
938
939        let mut planner = HybridAStarPlanner::new(&ox, &oy, config).unwrap();
940
941        let result = planner.plan(10.0, 10.0, 0.0, 35.0, 35.0, PI / 2.0);
942        assert!(result.is_ok(), "Planning failed: {:?}", result.err());
943
944        let path = result.unwrap();
945        let last = path.x.len() - 1;
946        let goal_dist = ((path.x[last] - 35.0).powi(2) + (path.y[last] - 35.0).powi(2)).sqrt();
947        assert!(
948            goal_dist < 15.0,
949            "Path end too far from goal: dist={}",
950            goal_dist
951        );
952    }
953
954    /// A narrow parking street: walls, a row of parked cars with one gap.
955    fn parking_street() -> (Vec<f64>, Vec<f64>) {
956        let (mut ox, mut oy) = create_boundary_obstacles(0.0, 30.0, 0.0, 14.0, 0.25);
957        // Parked cars 4.6 m long along the bottom curb, a 7 m gap at x 12..19.
958        for x0 in [1.0, 6.0, 20.0, 25.0] {
959            let mut x = x0;
960            while x <= x0 + 4.6 {
961                let mut y = 0.5;
962                while y <= 2.4 {
963                    ox.push(x);
964                    oy.push(y);
965                    y += 0.25;
966                }
967                x += 0.25;
968            }
969        }
970        (ox, oy)
971    }
972
973    fn car() -> VehicleFootprint {
974        VehicleFootprint {
975            front: 3.6,
976            rear: 1.0,
977            width: 1.9,
978        }
979    }
980
981    fn parking_config() -> HybridAStarConfig {
982        HybridAStarConfig {
983            xy_resolution: 0.5,
984            yaw_resolution: PI / 36.0,
985            wheelbase: 2.7,
986            max_steer: 0.6,
987            n_steer: 6,
988            step_size: 0.25,
989            max_curvature: 0.6_f64.tan() / 2.7,
990            switch_back_cost: 5.0,
991            analytic_expansion_interval: 3,
992            ..HybridAStarConfig::default()
993        }
994    }
995
996    /// Distance from `p` to the nearest obstacle point.
997    fn clearance(ox: &[f64], oy: &[f64], p: (f64, f64)) -> f64 {
998        ox.iter()
999            .zip(oy)
1000            .map(|(x, y)| (x - p.0).hypot(y - p.1))
1001            .fold(f64::INFINITY, f64::min)
1002    }
1003
1004    #[test]
1005    fn degenerate_footprints_do_not_blow_up() {
1006        let flat = VehicleFootprint {
1007            front: 3.6,
1008            rear: 1.0,
1009            width: 0.0,
1010        };
1011        assert_eq!(flat.covering_circles().len(), 1);
1012        let thin = VehicleFootprint {
1013            width: 1e-6,
1014            ..flat
1015        };
1016        assert!(thin.covering_circles().len() <= 16);
1017        let (ox, oy) = parking_street();
1018        assert!(HybridAStarPlanner::with_vehicle(&ox, &oy, parking_config(), flat).is_err());
1019    }
1020
1021    #[test]
1022    fn covering_circles_cover_the_car() {
1023        let car = car();
1024        let circles = car.covering_circles();
1025        // Every corner and edge point of the rectangle is inside a circle.
1026        for k in 0..=40 {
1027            let along = -car.rear + (car.front + car.rear) * k as f64 / 40.0;
1028            for side in [-car.width / 2.0, car.width / 2.0] {
1029                assert!(
1030                    circles
1031                        .iter()
1032                        .any(|&(c, r)| (along - c).hypot(side) <= r + 1e-9),
1033                    "({along}, {side}) uncovered"
1034                );
1035            }
1036        }
1037    }
1038
1039    #[test]
1040    fn parallel_parks_into_the_gap_without_touching_parked_cars() {
1041        let (ox, oy) = parking_street();
1042        let mut planner = HybridAStarPlanner::with_vehicle(&ox, &oy, parking_config(), car())
1043            .unwrap()
1044            .with_max_expansions(20_000);
1045        // Driving along the street, then into the gap between parked cars.
1046        let path = planner.plan(5.0, 7.0, 0.0, 14.0, 1.5, 0.0).expect("path");
1047        let end = path.len() - 1;
1048        assert!((path.x[end] - 14.0).abs() < 0.6 && (path.y[end] - 1.5).abs() < 0.6);
1049        // The car's corners stay clear of every obstacle point.
1050        let car = car();
1051        for i in 0..path.len() {
1052            let (s, c) = path.yaw[i].sin_cos();
1053            for (along, side) in [
1054                (car.front, car.width / 2.0),
1055                (car.front, -car.width / 2.0),
1056                (-car.rear, car.width / 2.0),
1057                (-car.rear, -car.width / 2.0),
1058            ] {
1059                let corner = (
1060                    path.x[i] + along * c - side * s,
1061                    path.y[i] + along * s + side * c,
1062                );
1063                assert!(
1064                    clearance(&ox, &oy, corner) > 0.05,
1065                    "corner {corner:?} hits at pose {i}"
1066                );
1067            }
1068        }
1069        // Directions agree with the motion: every step goes along the
1070        // heading for +1 and against it for -1.
1071        for i in 0..end {
1072            let along = (path.x[i + 1] - path.x[i]) * path.yaw[i].cos()
1073                + (path.y[i + 1] - path.y[i]) * path.yaw[i].sin();
1074            if along.abs() > 1e-6 {
1075                assert_eq!(
1076                    along.signum() as i32,
1077                    path.directions[i],
1078                    "direction at {i}"
1079                );
1080            }
1081        }
1082    }
1083
1084    #[test]
1085    fn a_straight_goal_ahead_needs_no_reversing() {
1086        let (ox, oy) = parking_street();
1087        let mut planner = HybridAStarPlanner::with_vehicle(&ox, &oy, parking_config(), car())
1088            .unwrap()
1089            .with_max_expansions(20_000);
1090        let path = planner.plan(3.0, 8.0, 0.0, 25.0, 8.0, 0.0).expect("path");
1091        assert!(
1092            path.directions.iter().all(|&d| d == 1),
1093            "{:?}",
1094            path.directions
1095        );
1096    }
1097
1098    #[test]
1099    fn an_unreachable_goal_fails_within_the_budget() {
1100        let (ox, oy) = parking_street();
1101        let mut planner = HybridAStarPlanner::with_vehicle(&ox, &oy, parking_config(), car())
1102            .unwrap()
1103            .with_max_expansions(500);
1104        // Inside a parked car.
1105        assert!(planner.plan(5.0, 7.0, 0.0, 3.0, 1.5, 0.0).is_err());
1106        // Free but walled-in pose: a gap far too short for the car.
1107        let mut too_tight = parking_street();
1108        for y in [1.0, 2.0] {
1109            too_tight.0.push(16.0);
1110            too_tight.1.push(y);
1111        }
1112        let mut planner =
1113            HybridAStarPlanner::with_vehicle(&too_tight.0, &too_tight.1, parking_config(), car())
1114                .unwrap()
1115                .with_max_expansions(500);
1116        assert!(planner.plan(5.0, 7.0, 0.0, 14.0, 1.5, 0.0).is_err());
1117    }
1118
1119    #[test]
1120    fn test_invalid_config() {
1121        let (ox, oy) = create_boundary_obstacles(0.0, 50.0, 0.0, 50.0, 1.0);
1122
1123        let config = HybridAStarConfig {
1124            xy_resolution: 0.0,
1125            ..Default::default()
1126        };
1127
1128        let result = HybridAStarPlanner::new(&ox, &oy, config);
1129        assert!(result.is_err());
1130    }
1131
1132    #[test]
1133    fn test_path_result_consistency() {
1134        let (ox, oy) = create_boundary_obstacles(0.0, 50.0, 0.0, 50.0, 1.0);
1135
1136        let config = HybridAStarConfig {
1137            xy_resolution: 2.0,
1138            yaw_resolution: PI / 18.0,
1139            robot_radius: 1.0,
1140            ..Default::default()
1141        };
1142
1143        let mut planner = HybridAStarPlanner::new(&ox, &oy, config).unwrap();
1144
1145        let result = planner.plan(10.0, 10.0, 0.0, 30.0, 30.0, 0.0);
1146        assert!(result.is_ok());
1147
1148        let path = result.unwrap();
1149        // All vectors should have same length
1150        assert_eq!(path.x.len(), path.y.len());
1151        assert_eq!(path.x.len(), path.yaw.len());
1152        assert_eq!(path.x.len(), path.directions.len());
1153
1154        // All yaw values should be in [-pi, pi]
1155        for &yaw in &path.yaw {
1156            assert!(
1157                (-PI - 0.01..=PI + 0.01).contains(&yaw),
1158                "Yaw out of range: {}",
1159                yaw
1160            );
1161        }
1162
1163        // All directions should be 1 or -1
1164        for &d in &path.directions {
1165            assert!(d == 1 || d == -1, "Invalid direction: {}", d);
1166        }
1167    }
1168}