1use 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#[derive(Debug, Clone)]
18pub struct HybridAStarConfig {
19 pub xy_resolution: f64,
21 pub yaw_resolution: f64,
23 pub wheelbase: f64,
25 pub max_steer: f64,
27 pub n_steer: usize,
29 pub step_size: f64,
31 pub robot_radius: f64,
33 pub max_curvature: f64,
35 pub switch_back_cost: f64,
37 pub steer_cost: f64,
39 pub steer_change_cost: f64,
41 pub h_cost: f64,
43 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, 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#[derive(Debug, Clone)]
101struct HybridNode {
102 x_index: i32,
104 y_index: i32,
106 yaw_index: i32,
108 direction: bool,
110 x_list: Vec<f64>,
112 y_list: Vec<f64>,
114 yaw_list: Vec<f64>,
116 directions: Vec<i32>,
118 steer: f64,
120 cost: f64,
122 parent_index: Option<usize>,
124}
125
126#[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#[derive(Debug, Clone)]
158pub struct HybridAStarPath {
159 pub x: Vec<f64>,
161 pub y: Vec<f64>,
163 pub yaw: Vec<f64>,
165 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#[derive(Debug, Clone, Copy, PartialEq)]
182pub struct VehicleFootprint {
183 pub front: f64,
185 pub rear: f64,
187 pub width: f64,
189}
190
191impl VehicleFootprint {
192 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
227pub struct HybridAStarPlanner {
229 config: HybridAStarConfig,
230 grid_map: GridMap,
231 circle_offsets: Vec<f64>,
234 collision_map: Option<GridMap>,
237 max_expansions: usize,
239 h_map: Vec<Vec<f64>>,
241 goal_x: i32,
242 goal_y: i32,
243}
244
245impl HybridAStarPlanner {
246 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 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 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 pub fn with_max_expansions(mut self, max_expansions: usize) -> Self {
301 self.max_expansions = max_expansions.max(1);
302 self
303 }
304
305 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 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 self.goal_x = self.grid_map.calc_x_index(gx);
330 self.goal_y = self.grid_map.calc_y_index(gy);
331
332 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(¤t_key) {
394 continue;
395 }
396
397 closed_set.insert(current_key, current_idx);
398 open_map.remove(¤t_key);
399
400 if self.is_goal(&node_storage[current_idx], &goal_node) {
402 return Ok(self.build_path(current_idx, &node_storage, None));
403 }
404
405 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 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 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 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 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 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 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 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 i in 0..=self.config.n_steer {
496 let steer = -self.config.max_steer + i as f64 * steer_step;
497
498 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 #[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 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 let mut cost = parent.cost;
566
567 cost += arc_length;
569
570 cost += self.config.steer_cost * steer.abs();
572
573 cost += self.config.steer_change_cost * (steer - parent.steer).abs();
575
576 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 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 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 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 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 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 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#[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 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 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 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 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 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 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 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 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 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 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 assert!(planner.plan(5.0, 7.0, 0.0, 3.0, 1.5, 0.0).is_err());
1106 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 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 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 for &d in &path.directions {
1165 assert!(d == 1 || d == -1, "Invalid direction: {}", d);
1166 }
1167 }
1168}