1#![allow(dead_code, clippy::too_many_arguments)]
2
3use std::f64::consts::PI;
21
22use rust_robotics_core::types::Pose2D;
23
24use crate::rrt::{AreaBounds, CircleObstacle};
25use crate::rrt_star_reeds_shepp::{RRTStarRSConfig, RRTStarRSNode, RRTStarRSPlanner};
26use rust_robotics_core::normalize_angle;
27
28#[derive(Debug, Clone)]
34pub struct UnicycleParams {
35 pub dt: f64,
37 pub wheelbase: f64,
39 pub steer_max: f64,
41 pub accel_max: f64,
43}
44
45impl Default for UnicycleParams {
46 fn default() -> Self {
47 Self {
48 dt: 0.05,
49 wheelbase: 0.9,
50 steer_max: 40.0_f64.to_radians(),
51 accel_max: 5.0,
52 }
53 }
54}
55
56#[derive(Debug, Clone, Copy)]
58pub struct VehicleState {
59 pub x: f64,
60 pub y: f64,
61 pub yaw: f64,
62 pub v: f64,
63}
64
65impl VehicleState {
66 pub fn new(x: f64, y: f64, yaw: f64, v: f64) -> Self {
67 Self { x, y, yaw, v }
68 }
69}
70
71fn unicycle_update(
73 state: &VehicleState,
74 accel: f64,
75 delta: f64,
76 params: &UnicycleParams,
77) -> VehicleState {
78 let x = state.x + state.v * state.yaw.cos() * params.dt;
79 let y = state.y + state.v * state.yaw.sin() * params.dt;
80 let yaw = normalize_angle(state.yaw + state.v / params.wheelbase * delta.tan() * params.dt);
81 let v = state.v + accel * params.dt;
82 VehicleState { x, y, yaw, v }
83}
84
85#[derive(Debug, Clone)]
91pub struct PurePursuitParams {
92 pub kp: f64,
94 pub look_ahead: f64,
96 pub max_time: f64,
98 pub goal_dis: f64,
100 pub stop_speed: f64,
102}
103
104impl Default for PurePursuitParams {
105 fn default() -> Self {
106 Self {
107 kp: 2.0,
108 look_ahead: 0.5,
109 max_time: 100.0,
110 goal_dis: 0.5,
111 stop_speed: 0.5,
112 }
113 }
114}
115
116#[derive(Debug, Clone)]
118pub struct SimulationResult {
119 pub t: Vec<f64>,
120 pub x: Vec<f64>,
121 pub y: Vec<f64>,
122 pub yaw: Vec<f64>,
123 pub v: Vec<f64>,
124 pub accel: Vec<f64>,
125 pub steer: Vec<f64>,
126 pub reached_goal: bool,
127}
128
129fn pid_control(target: f64, current: f64, kp: f64, accel_max: f64) -> f64 {
131 (kp * (target - current)).clamp(-accel_max, accel_max)
132}
133
134fn calc_target_index(
136 state: &VehicleState,
137 cx: &[f64],
138 cy: &[f64],
139 look_ahead: f64,
140) -> (usize, f64) {
141 let mut min_dist = f64::INFINITY;
142 let mut min_ind = 0;
143 for (i, (&rx, &ry)) in cx.iter().zip(cy.iter()).enumerate() {
144 let d = ((state.x - rx).powi(2) + (state.y - ry).powi(2)).sqrt();
145 if d < min_dist {
146 min_dist = d;
147 min_ind = i;
148 }
149 }
150
151 let mut cumulative = 0.0;
152 while look_ahead > cumulative && (min_ind + 1) < cx.len() {
153 let dx = cx[min_ind + 1] - cx[min_ind];
154 let dy = cy[min_ind + 1] - cy[min_ind];
155 cumulative += (dx * dx + dy * dy).sqrt();
156 min_ind += 1;
157 }
158
159 (min_ind, min_dist)
160}
161
162fn pure_pursuit_control(
164 state: &VehicleState,
165 cx: &[f64],
166 cy: &[f64],
167 prev_ind: usize,
168 look_ahead: f64,
169 wheelbase: f64,
170 steer_max: f64,
171) -> (f64, usize, f64) {
172 let (mut ind, dis) = calc_target_index(state, cx, cy, look_ahead);
173 if prev_ind >= ind {
174 ind = prev_ind;
175 }
176
177 let (tx, ty) = if ind < cx.len() {
178 (cx[ind], cy[ind])
179 } else {
180 ind = cx.len() - 1;
181 (cx[ind], cy[ind])
182 };
183
184 let mut alpha = (ty - state.y).atan2(tx - state.x) - state.yaw;
185 if state.v <= 0.0 {
186 alpha = PI - alpha;
187 }
188
189 let delta = (2.0 * wheelbase * alpha.sin() / look_ahead).atan2(1.0);
190 let delta = delta.clamp(-steer_max, steer_max);
191
192 (delta, ind, dis)
193}
194
195fn calc_speed_profile(
198 cx: &[f64],
199 cy: &[f64],
200 cyaw: &[f64],
201 target_speed: f64,
202 stop_speed: f64,
203) -> Vec<f64> {
204 let n = cx.len();
205 let mut profile = vec![target_speed; n];
206 let mut forward = true;
207 let mut is_back = false;
208
209 for i in 0..n - 1 {
210 let dx = cx[i + 1] - cx[i];
211 let dy = cy[i + 1] - cy[i];
212 let move_dir = dy.atan2(dx);
213 is_back = (move_dir - cyaw[i]).abs() >= PI / 2.0;
214
215 if dx == 0.0 && dy == 0.0 {
216 continue;
217 }
218
219 if is_back {
220 profile[i] = -target_speed;
221 } else {
222 profile[i] = target_speed;
223 }
224
225 if is_back && forward {
226 profile[i] = 0.0;
227 forward = false;
228 } else if !is_back && !forward {
229 profile[i] = 0.0;
230 forward = true;
231 }
232 }
233
234 profile[0] = 0.0;
235 if is_back {
236 profile[n - 1] = -stop_speed;
237 } else {
238 profile[n - 1] = stop_speed;
239 }
240
241 profile
242}
243
244fn extend_path(
247 cx: &[f64],
248 cy: &[f64],
249 cyaw: &[f64],
250 look_ahead: f64,
251) -> (Vec<f64>, Vec<f64>, Vec<f64>) {
252 let mut cx = cx.to_vec();
253 let mut cy = cy.to_vec();
254 let mut cyaw = cyaw.to_vec();
255
256 let dl = 0.1_f64;
257 let steps = (look_ahead / dl) as usize + 1;
258
259 let n = cx.len();
260 let move_dir = (cy[n - 1] - cy[n.saturating_sub(3)]).atan2(cx[n - 1] - cx[n.saturating_sub(3)]);
261 let is_back = (move_dir - cyaw[n - 1]).abs() >= PI / 2.0;
262
263 for _ in 0..steps {
264 let idl = if is_back { -dl } else { dl };
265 let last_yaw = *cyaw.last().unwrap();
266 cx.push(cx.last().unwrap() + idl * last_yaw.cos());
267 cy.push(cy.last().unwrap() + idl * last_yaw.sin());
268 cyaw.push(last_yaw);
269 }
270
271 (cx, cy, cyaw)
272}
273
274fn closed_loop_prediction(
276 cx: &[f64],
277 cy: &[f64],
278 _cyaw: &[f64],
279 speed_profile: &[f64],
280 goal: [f64; 3],
281 vehicle: &UnicycleParams,
282 pp: &PurePursuitParams,
283) -> SimulationResult {
284 let mut state = VehicleState::new(0.0, 0.0, 0.0, 0.0);
285 let mut time = 0.0;
286
287 let mut result = SimulationResult {
288 t: vec![0.0],
289 x: vec![state.x],
290 y: vec![state.y],
291 yaw: vec![state.yaw],
292 v: vec![state.v],
293 accel: vec![0.0],
294 steer: vec![0.0],
295 reached_goal: false,
296 };
297
298 let (mut target_ind, _) = calc_target_index(&state, cx, cy, pp.look_ahead);
299 let max_dis = 0.5_f64;
300
301 while time <= pp.max_time {
302 let (di, new_ind, dis) = pure_pursuit_control(
303 &state,
304 cx,
305 cy,
306 target_ind,
307 pp.look_ahead,
308 vehicle.wheelbase,
309 vehicle.steer_max,
310 );
311 target_ind = new_ind;
312
313 let mut target_speed = speed_profile[target_ind.min(speed_profile.len() - 1)];
314 target_speed *= (max_dis - dis.min(max_dis - 0.1)) / max_dis;
315
316 let ai = pid_control(target_speed, state.v, pp.kp, vehicle.accel_max);
317 state = unicycle_update(&state, ai, di, vehicle);
318
319 if state.v.abs() <= pp.stop_speed && target_ind <= cx.len().saturating_sub(2) {
320 target_ind += 1;
321 }
322
323 time += vehicle.dt;
324
325 let dx = state.x - goal[0];
327 let dy = state.y - goal[1];
328 if (dx * dx + dy * dy).sqrt() <= pp.goal_dis {
329 result.reached_goal = true;
330 break;
331 }
332
333 result.t.push(time);
334 result.x.push(state.x);
335 result.y.push(state.y);
336 result.yaw.push(state.yaw);
337 result.v.push(state.v);
338 result.accel.push(ai);
339 result.steer.push(di);
340 }
341
342 result
343}
344
345#[derive(Debug, Clone)]
351pub struct ClosedLoopRRTStarConfig {
352 pub rrt_config: RRTStarRSConfig,
354 pub vehicle: UnicycleParams,
356 pub pursuit: PurePursuitParams,
358 pub target_speed: f64,
360 pub yaw_threshold: f64,
362 pub xy_threshold: f64,
364 pub invalid_travel_ratio: f64,
367 pub final_yaw_tolerance: f64,
369}
370
371impl Default for ClosedLoopRRTStarConfig {
372 fn default() -> Self {
373 Self {
374 rrt_config: RRTStarRSConfig::default(),
375 vehicle: UnicycleParams::default(),
376 pursuit: PurePursuitParams::default(),
377 target_speed: 10.0 / 3.6,
378 yaw_threshold: 3.0_f64.to_radians(),
379 xy_threshold: 0.5,
380 invalid_travel_ratio: 5.0,
381 final_yaw_tolerance: 30.0_f64.to_radians(),
382 }
383 }
384}
385
386#[derive(Debug, Clone)]
388pub struct ClosedLoopRRTStarResult {
389 pub sim: SimulationResult,
391 pub geometric_poses: Vec<Pose2D>,
393}
394
395pub struct ClosedLoopRRTStarPlanner {
400 config: ClosedLoopRRTStarConfig,
401 obstacles: Vec<CircleObstacle>,
402 rand_area: AreaBounds,
403 inner: RRTStarRSPlanner,
404}
405
406impl ClosedLoopRRTStarPlanner {
407 pub fn new(
409 obstacles: Vec<CircleObstacle>,
410 rand_area: AreaBounds,
411 config: ClosedLoopRRTStarConfig,
412 ) -> Self {
413 let inner = RRTStarRSPlanner::new(
414 obstacles.clone(),
415 rand_area.clone(),
416 config.rrt_config.clone(),
417 );
418 Self {
419 config,
420 obstacles,
421 rand_area,
422 inner,
423 }
424 }
425
426 pub fn planning(&mut self, start: Pose2D, goal: Pose2D) -> Option<ClosedLoopRRTStarResult> {
430 let _ = self.inner.planning(start, goal);
432 self.select_best_feasible(goal)
433 }
434
435 pub fn plan_with_sampler<F>(
437 &mut self,
438 start: Pose2D,
439 goal: Pose2D,
440 sample_node: F,
441 ) -> Option<ClosedLoopRRTStarResult>
442 where
443 F: FnMut(&RRTStarRSPlanner) -> RRTStarRSNode,
444 {
445 let _ = self.inner.plan_with_sampler(start, goal, sample_node);
446 self.select_best_feasible(goal)
447 }
448
449 pub fn get_tree(&self) -> &[RRTStarRSNode] {
451 self.inner.get_tree()
452 }
453
454 fn select_best_feasible(&self, goal: Pose2D) -> Option<ClosedLoopRRTStarResult> {
461 let tree = self.inner.get_tree();
462 let goal_inds = self.get_goal_indexes(tree, &goal);
463
464 let mut best_time = f64::INFINITY;
465 let mut best_result: Option<ClosedLoopRRTStarResult> = None;
466
467 for &ind in &goal_inds {
468 let path = self.generate_final_course(tree, ind);
469 if path.is_empty() || path.len() < 2 {
470 continue;
471 }
472
473 let feasibility = self.check_tracking_feasible(&path, &goal);
474 if let Some(sim) = feasibility {
475 let end_time = *sim.t.last().unwrap_or(&f64::INFINITY);
476 if end_time < best_time {
477 best_time = end_time;
478 let poses: Vec<Pose2D> = path
479 .iter()
480 .map(|&(x, y, yaw)| Pose2D::new(x, y, yaw))
481 .collect();
482 best_result = Some(ClosedLoopRRTStarResult {
483 sim,
484 geometric_poses: poses,
485 });
486 }
487 }
488 }
489
490 best_result
491 }
492
493 fn get_goal_indexes(&self, tree: &[RRTStarRSNode], goal: &Pose2D) -> Vec<usize> {
495 let mut inds = Vec::new();
496 for (i, node) in tree.iter().enumerate() {
497 let dx = node.x - goal.x;
498 let dy = node.y - goal.y;
499 if (dx * dx + dy * dy).sqrt() > self.config.xy_threshold {
500 continue;
501 }
502 if angle_diff(node.yaw, goal.yaw).abs() > self.config.yaw_threshold {
503 continue;
504 }
505 inds.push(i);
506 }
507 inds
508 }
509
510 fn generate_final_course(
513 &self,
514 tree: &[RRTStarRSNode],
515 goal_index: usize,
516 ) -> Vec<(f64, f64, f64)> {
517 let mut path: Vec<(f64, f64, f64)> = Vec::new();
518
519 let mut node = &tree[goal_index];
520 while node.parent.is_some() {
521 for ((&px, &py), &pyaw) in node
522 .path_x
523 .iter()
524 .rev()
525 .zip(node.path_y.iter().rev())
526 .zip(node.path_yaw.iter().rev())
527 {
528 path.push((px, py, pyaw));
529 }
530 node = &tree[node.parent.unwrap()];
531 }
532 path.push((node.x, node.y, node.yaw));
533 path.reverse();
534 path
535 }
536
537 fn check_tracking_feasible(
541 &self,
542 path: &[(f64, f64, f64)],
543 goal: &Pose2D,
544 ) -> Option<SimulationResult> {
545 let cx: Vec<f64> = path.iter().map(|p| p.0).collect();
546 let cy: Vec<f64> = path.iter().map(|p| p.1).collect();
547 let cyaw: Vec<f64> = path.iter().map(|p| p.2).collect();
548
549 let goal_arr = [goal.x, goal.y, goal.yaw];
550
551 let (ecx, ecy, ecyaw) = extend_path(&cx, &cy, &cyaw, self.config.pursuit.look_ahead);
552
553 let speed_profile = calc_speed_profile(
554 &ecx,
555 &ecy,
556 &ecyaw,
557 self.config.target_speed,
558 self.config.pursuit.stop_speed,
559 );
560
561 let sim = closed_loop_prediction(
562 &ecx,
563 &ecy,
564 &ecyaw,
565 &speed_profile,
566 goal_arr,
567 &self.config.vehicle,
568 &self.config.pursuit,
569 );
570
571 if !sim.reached_goal {
572 return None;
573 }
574
575 if let Some(&final_yaw) = sim.yaw.last() {
577 if (normalize_angle(final_yaw) - goal.yaw).abs() >= self.config.final_yaw_tolerance {
578 return None;
579 }
580 }
581
582 let travel: f64 = sim.v.iter().map(|vi| vi.abs()).sum::<f64>() * self.config.vehicle.dt;
584 let origin_travel: f64 = path
585 .windows(2)
586 .map(|w| ((w[1].0 - w[0].0).powi(2) + (w[1].1 - w[0].1).powi(2)).sqrt())
587 .sum();
588
589 if origin_travel > 0.0 && travel / origin_travel >= self.config.invalid_travel_ratio {
590 return None;
591 }
592
593 if !self.check_sim_collision(&sim) {
595 return None;
596 }
597
598 Some(sim)
599 }
600
601 fn check_sim_collision(&self, sim: &SimulationResult) -> bool {
603 for obs in &self.obstacles {
604 for (&sx, &sy) in sim.x.iter().zip(sim.y.iter()) {
605 let dx = obs.x - sx;
606 let dy = obs.y - sy;
607 let d = (dx * dx + dy * dy).sqrt();
608 if d <= obs.radius + self.config.rrt_config.robot_radius {
609 return false;
610 }
611 }
612 }
613 true
614 }
615}
616
617fn angle_diff(a: f64, b: f64) -> f64 {
623 let mut d = a - b;
624 while d > PI {
625 d -= 2.0 * PI;
626 }
627 while d < -PI {
628 d += 2.0 * PI;
629 }
630 d
631}
632
633#[cfg(test)]
638mod tests {
639 use super::*;
640
641 fn approx_eq(a: f64, b: f64, tol: f64) -> bool {
642 (a - b).abs() < tol
643 }
644
645 #[test]
648 fn test_unicycle_straight_line() {
649 let params = UnicycleParams::default();
650 let state = VehicleState::new(0.0, 0.0, 0.0, 1.0);
651 let next = unicycle_update(&state, 0.0, 0.0, ¶ms);
652 assert!(approx_eq(next.x, params.dt, 1e-12));
653 assert!(approx_eq(next.y, 0.0, 1e-12));
654 assert!(approx_eq(next.yaw, 0.0, 1e-12));
655 assert!(approx_eq(next.v, 1.0, 1e-12));
656 }
657
658 #[test]
659 fn test_unicycle_acceleration() {
660 let params = UnicycleParams::default();
661 let state = VehicleState::new(0.0, 0.0, 0.0, 0.0);
662 let next = unicycle_update(&state, 2.0, 0.0, ¶ms);
663 assert!(approx_eq(next.v, 2.0 * params.dt, 1e-12));
664 assert!(approx_eq(next.x, 0.0, 1e-12)); }
666
667 #[test]
668 fn test_unicycle_turning() {
669 let params = UnicycleParams::default();
670 let state = VehicleState::new(0.0, 0.0, 0.0, 1.0);
671 let delta = 0.1; let next = unicycle_update(&state, 0.0, delta, ¶ms);
673 let expected_yaw = 1.0 / params.wheelbase * delta.tan() * params.dt;
675 assert!(approx_eq(next.yaw, expected_yaw, 1e-10));
676 }
677
678 #[test]
681 fn test_calc_target_index_nearest() {
682 let cx = vec![0.0, 1.0, 2.0, 3.0, 4.0];
683 let cy = vec![0.0, 0.0, 0.0, 0.0, 0.0];
684 let state = VehicleState::new(0.5, 0.0, 0.0, 1.0);
685 let (ind, dis) = calc_target_index(&state, &cx, &cy, 0.5);
686 assert!(ind <= cx.len());
688 assert!(dis < 1.0);
689 }
690
691 #[test]
692 fn test_pid_control_clamping() {
693 let a = pid_control(10.0, 0.0, 2.0, 5.0);
694 assert!(approx_eq(a, 5.0, 1e-12)); let a = pid_control(-10.0, 0.0, 2.0, 5.0);
697 assert!(approx_eq(a, -5.0, 1e-12)); }
699
700 #[test]
701 fn test_pid_control_proportional() {
702 let a = pid_control(1.0, 0.0, 2.0, 5.0);
703 assert!(approx_eq(a, 2.0, 1e-12));
704 }
705
706 #[test]
709 fn test_speed_profile_forward() {
710 let cx = vec![0.0, 1.0, 2.0, 3.0];
711 let cy = vec![0.0, 0.0, 0.0, 0.0];
712 let cyaw = vec![0.0, 0.0, 0.0, 0.0];
713 let profile = calc_speed_profile(&cx, &cy, &cyaw, 1.0, 0.5);
714 assert_eq!(profile.len(), 4);
715 assert!(approx_eq(profile[0], 0.0, 1e-12)); assert!(profile[1] > 0.0); assert!(approx_eq(profile[3], 0.5, 1e-12)); }
719
720 #[test]
721 fn test_speed_profile_backward() {
722 let cx = vec![3.0, 2.0, 1.0, 0.0];
723 let cy = vec![0.0, 0.0, 0.0, 0.0];
724 let cyaw = vec![0.0, 0.0, 0.0, 0.0];
726 let profile = calc_speed_profile(&cx, &cy, &cyaw, 1.0, 0.5);
727 assert!(approx_eq(profile[0], 0.0, 1e-12));
728 assert!(profile[1] < 0.0 || approx_eq(profile[1], 0.0, 1e-12));
730 }
731
732 #[test]
735 fn test_extend_path_length() {
736 let cx = vec![0.0, 1.0, 2.0];
737 let cy = vec![0.0, 0.0, 0.0];
738 let cyaw = vec![0.0, 0.0, 0.0];
739 let (ecx, ecy, ecyaw) = extend_path(&cx, &cy, &cyaw, 0.5);
740 assert!(ecx.len() > cx.len());
741 assert_eq!(ecx.len(), ecy.len());
742 assert_eq!(ecx.len(), ecyaw.len());
743 }
744
745 #[test]
748 fn test_pi_2_pi() {
749 assert!(approx_eq(normalize_angle(0.0), 0.0, 1e-12));
750 assert!(approx_eq(normalize_angle(PI), PI, 1e-12));
751 assert!(approx_eq(normalize_angle(-PI), -PI, 1e-12));
752 assert!(approx_eq(normalize_angle(3.0 * PI), PI, 1e-10));
753 assert!(approx_eq(normalize_angle(-3.0 * PI), -PI, 1e-10));
754 }
755
756 #[test]
757 fn test_angle_diff() {
758 assert!(approx_eq(angle_diff(0.0, 0.0), 0.0, 1e-12));
759 assert!(approx_eq(angle_diff(PI, 0.0), PI, 1e-12));
760 assert!(approx_eq(angle_diff(0.0, PI), -PI, 1e-12));
761 }
762
763 #[test]
766 fn test_closed_loop_straight_path() {
767 let cx: Vec<f64> = (0..50).map(|i| i as f64 * 0.2).collect();
768 let cy = vec![0.0; cx.len()];
769 let cyaw = vec![0.0; cx.len()];
770 let vehicle = UnicycleParams::default();
771 let pp = PurePursuitParams {
772 max_time: 20.0,
773 ..Default::default()
774 };
775 let goal = [cx[cx.len() - 6], cy[cy.len() - 6], 0.0]; let (ecx, ecy, ecyaw) = extend_path(&cx, &cy, &cyaw, pp.look_ahead);
778 let esp = calc_speed_profile(&ecx, &ecy, &ecyaw, 1.0, pp.stop_speed);
779
780 let sim = closed_loop_prediction(&ecx, &ecy, &ecyaw, &esp, goal, &vehicle, &pp);
781 assert!(sim.x.last().unwrap() > &0.0);
783 assert!(sim.t.len() > 1);
784 }
785
786 #[test]
789 fn test_config_defaults() {
790 let config = ClosedLoopRRTStarConfig::default();
791 assert!(approx_eq(config.target_speed, 10.0 / 3.6, 1e-10));
792 assert!(approx_eq(config.yaw_threshold, 3.0_f64.to_radians(), 1e-10));
793 assert!(approx_eq(config.xy_threshold, 0.5, 1e-12));
794 assert!(approx_eq(config.invalid_travel_ratio, 5.0, 1e-12));
795 }
796
797 #[test]
800 fn test_sim_collision_check_no_obstacles() {
801 let config = ClosedLoopRRTStarConfig::default();
802 let rand_area = AreaBounds::new(-2.0, 15.0, -2.0, 15.0);
803 let planner = ClosedLoopRRTStarPlanner::new(vec![], rand_area, config);
804
805 let sim = SimulationResult {
806 t: vec![0.0, 0.1],
807 x: vec![0.0, 1.0],
808 y: vec![0.0, 0.0],
809 yaw: vec![0.0, 0.0],
810 v: vec![1.0, 1.0],
811 accel: vec![0.0, 0.0],
812 steer: vec![0.0, 0.0],
813 reached_goal: true,
814 };
815 assert!(planner.check_sim_collision(&sim));
816 }
817
818 #[test]
819 fn test_sim_collision_check_with_obstacle() {
820 let obstacles = vec![CircleObstacle::new(0.5, 0.0, 0.3)];
821 let config = ClosedLoopRRTStarConfig::default();
822 let rand_area = AreaBounds::new(-2.0, 15.0, -2.0, 15.0);
823 let planner = ClosedLoopRRTStarPlanner::new(obstacles, rand_area, config);
824
825 let sim = SimulationResult {
826 t: vec![0.0, 0.1],
827 x: vec![0.0, 0.5],
828 y: vec![0.0, 0.0],
829 yaw: vec![0.0, 0.0],
830 v: vec![1.0, 1.0],
831 accel: vec![0.0, 0.0],
832 steer: vec![0.0, 0.0],
833 reached_goal: true,
834 };
835 assert!(!planner.check_sim_collision(&sim));
836 }
837
838 #[test]
841 fn test_get_goal_indexes_filters_by_xy_and_yaw() {
842 let config = ClosedLoopRRTStarConfig {
843 xy_threshold: 1.0,
844 yaw_threshold: 0.5,
845 ..Default::default()
846 };
847 let rand_area = AreaBounds::new(-5.0, 20.0, -5.0, 20.0);
848 let planner = ClosedLoopRRTStarPlanner::new(vec![], rand_area, config);
849
850 let goal = Pose2D::new(10.0, 10.0, 0.0);
851 let tree = vec![
852 RRTStarRSNode::new(0.0, 0.0, 0.0), RRTStarRSNode::new(10.0, 10.0, 0.0), RRTStarRSNode::new(10.5, 10.5, 0.0), RRTStarRSNode::new(10.0, 10.0, PI), ];
857
858 let inds = planner.get_goal_indexes(&tree, &goal);
859 assert!(inds.contains(&1));
860 assert!(inds.contains(&2));
861 assert!(!inds.contains(&0));
862 assert!(!inds.contains(&3));
863 }
864
865 #[test]
868 fn test_generate_final_course() {
869 let config = ClosedLoopRRTStarConfig::default();
870 let rand_area = AreaBounds::new(-5.0, 20.0, -5.0, 20.0);
871 let planner = ClosedLoopRRTStarPlanner::new(vec![], rand_area, config);
872
873 let mut root = RRTStarRSNode::new(0.0, 0.0, 0.0);
874 root.parent = None;
875 let mut child = RRTStarRSNode::new(5.0, 5.0, 0.5);
876 child.parent = Some(0);
877 child.path_x = vec![0.0, 2.5, 5.0];
878 child.path_y = vec![0.0, 2.5, 5.0];
879 child.path_yaw = vec![0.0, 0.25, 0.5];
880
881 let tree = vec![root, child];
882 let course = planner.generate_final_course(&tree, 1);
883 assert!(course.len() >= 3);
884 assert!(approx_eq(course[0].0, 0.0, 1e-12));
886 assert!(approx_eq(course[0].1, 0.0, 1e-12));
887 }
888
889 #[test]
892 fn test_planner_creation() {
893 let obstacles = vec![
894 CircleObstacle::new(5.0, 5.0, 1.0),
895 CircleObstacle::new(4.0, 6.0, 1.0),
896 ];
897 let rand_area = AreaBounds::new(-2.0, 20.0, -2.0, 20.0);
898 let config = ClosedLoopRRTStarConfig::default();
899 let planner = ClosedLoopRRTStarPlanner::new(obstacles, rand_area, config);
900 assert!(planner.get_tree().is_empty() || planner.get_tree().is_empty());
901 }
902
903 #[test]
906 fn test_planning_no_obstacles_deterministic() {
907 let rand_area = AreaBounds::new(-2.0, 15.0, -2.0, 15.0);
908 let config = ClosedLoopRRTStarConfig {
909 rrt_config: RRTStarRSConfig {
910 max_iter: 300,
911 goal_xy_threshold: 1.5,
912 goal_yaw_threshold: 1.0,
913 connect_circle_dist: 50.0,
914 ..Default::default()
915 },
916 xy_threshold: 1.5,
917 yaw_threshold: 1.0,
918 ..Default::default()
919 };
920 let mut planner = ClosedLoopRRTStarPlanner::new(vec![], rand_area, config);
921
922 let start = Pose2D::new(0.0, 0.0, 0.0);
923 let goal = Pose2D::new(5.0, 0.0, 0.0);
924
925 let mut call = 0;
926 let result = planner.plan_with_sampler(start, goal, |p| {
927 call += 1;
928 if call % 2 == 0 {
929 RRTStarRSNode::new(p.get_tree()[0].x + 5.0, 0.0, 0.0)
930 } else {
931 RRTStarRSNode::new(p.get_tree()[0].x + 2.5, 0.0, 0.0)
932 }
933 });
934
935 if let Some(res) = result {
939 assert!(!res.sim.x.is_empty());
940 assert!(res.sim.reached_goal);
941 assert!(!res.geometric_poses.is_empty());
942 }
943 }
944
945 #[test]
948 fn test_planning_with_obstacles_no_panic() {
949 let obstacles = vec![
950 CircleObstacle::new(5.0, 5.0, 1.0),
951 CircleObstacle::new(4.0, 6.0, 1.0),
952 CircleObstacle::new(4.0, 8.0, 1.0),
953 CircleObstacle::new(6.0, 5.0, 1.0),
954 CircleObstacle::new(7.0, 5.0, 1.0),
955 ];
956 let rand_area = AreaBounds::new(-2.0, 15.0, -2.0, 15.0);
957 let config = ClosedLoopRRTStarConfig {
958 rrt_config: RRTStarRSConfig {
959 max_iter: 100,
960 goal_xy_threshold: 1.5,
961 goal_yaw_threshold: 1.0,
962 ..Default::default()
963 },
964 xy_threshold: 1.5,
965 yaw_threshold: 1.0,
966 ..Default::default()
967 };
968 let mut planner = ClosedLoopRRTStarPlanner::new(obstacles, rand_area, config);
969
970 let start = Pose2D::new(0.0, 0.0, 0.0);
971 let goal = Pose2D::new(6.0, 7.0, PI / 2.0);
972
973 let _result = planner.planning(start, goal);
976 }
977
978 #[test]
981 fn test_extend_path_backward() {
982 let cx = vec![3.0, 2.0, 1.0];
984 let cy = vec![0.0, 0.0, 0.0];
985 let cyaw = vec![0.0, 0.0, 0.0];
986 let (ecx, _ecy, _ecyaw) = extend_path(&cx, &cy, &cyaw, 0.5);
987 assert!(ecx.last().unwrap() < &1.0);
989 }
990
991 #[test]
994 fn test_simulation_result_default_fields() {
995 let sim = SimulationResult {
996 t: vec![],
997 x: vec![],
998 y: vec![],
999 yaw: vec![],
1000 v: vec![],
1001 accel: vec![],
1002 steer: vec![],
1003 reached_goal: false,
1004 };
1005 assert!(!sim.reached_goal);
1006 assert!(sim.t.is_empty());
1007 }
1008}