Skip to main content

rust_robotics_planning/
closed_loop_rrt_star.rs

1#![allow(dead_code, clippy::too_many_arguments)]
2
3//! Closed-Loop RRT* (CL-RRT*) path planner
4//!
5//! Extends RRT\*-Reeds-Shepp by adding a forward-simulation verification step.
6//! After the tree is grown, candidate goal paths are tracked using a pure-pursuit
7//! controller on a unicycle (bicycle-kinematic) model. Only paths that are
8//! dynamically feasible --- the controller reaches the goal, the tracked path
9//! does not collide, and the travel distance is reasonable --- are accepted.
10//!
11//! The planner returns the full simulated trajectory (states over time) of the
12//! best feasible path, not just the geometric waypoints.
13//!
14//! # References
15//!
16//! * Kuwata, Y. et al. (2009). "Real-Time Motion Planning With Applications
17//!   to Autonomous Urban Driving." *IEEE T-CST*.
18//! * PythonRobotics `ClosedLoopRRTStar/` by Atsushi Sakai.
19
20use 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// ---------------------------------------------------------------------------
29// Unicycle (bicycle-kinematic) model
30// ---------------------------------------------------------------------------
31
32/// Parameters for the bicycle-kinematic (unicycle) vehicle model.
33#[derive(Debug, Clone)]
34pub struct UnicycleParams {
35    /// Simulation time step \[s\].
36    pub dt: f64,
37    /// Wheelbase \[m\].
38    pub wheelbase: f64,
39    /// Maximum steering angle \[rad\].
40    pub steer_max: f64,
41    /// Maximum longitudinal acceleration \[m/s^2\].
42    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/// Vehicle state for the unicycle model.
57#[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
71/// Advance the vehicle state by one time step.
72fn 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// ---------------------------------------------------------------------------
86// Pure-pursuit controller
87// ---------------------------------------------------------------------------
88
89/// Parameters for the pure-pursuit path tracker.
90#[derive(Debug, Clone)]
91pub struct PurePursuitParams {
92    /// Proportional speed gain.
93    pub kp: f64,
94    /// Look-ahead distance \[m\].
95    pub look_ahead: f64,
96    /// Maximum simulation time \[s\].
97    pub max_time: f64,
98    /// Goal distance threshold \[m\].
99    pub goal_dis: f64,
100    /// Speed below which the robot is considered stopped \[m/s\].
101    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/// Result of a closed-loop forward simulation.
117#[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
129/// PID speed controller (proportional only, clamped).
130fn pid_control(target: f64, current: f64, kp: f64, accel_max: f64) -> f64 {
131    (kp * (target - current)).clamp(-accel_max, accel_max)
132}
133
134/// Find the target index on the reference path for pure pursuit.
135fn 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
162/// Pure pursuit steering control.
163fn 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
195/// Build a speed profile along the reference path, inserting stop points at
196/// direction reversals (forward <-> backward).
197fn 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
244/// Extend the path beyond its end by `look_ahead` distance so the tracker
245/// does not overshoot.
246fn 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
274/// Run the closed-loop pure-pursuit simulation on a reference path.
275fn 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        // Check goal
326        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// ---------------------------------------------------------------------------
346// Closed-Loop RRT* planner
347// ---------------------------------------------------------------------------
348
349/// Configuration for the Closed-Loop RRT* planner.
350#[derive(Debug, Clone)]
351pub struct ClosedLoopRRTStarConfig {
352    /// Underlying RRT*-Reeds-Shepp configuration.
353    pub rrt_config: RRTStarRSConfig,
354    /// Vehicle (unicycle) model parameters.
355    pub vehicle: UnicycleParams,
356    /// Pure-pursuit tracking parameters.
357    pub pursuit: PurePursuitParams,
358    /// Target speed for the forward simulation \[m/s\].
359    pub target_speed: f64,
360    /// Yaw threshold for checking goal candidates from the tree \[rad\].
361    pub yaw_threshold: f64,
362    /// Position threshold for checking goal candidates from the tree \[m\].
363    pub xy_threshold: f64,
364    /// If `tracked_travel / geometric_travel` exceeds this ratio, the path is
365    /// rejected as inefficient.
366    pub invalid_travel_ratio: f64,
367    /// Yaw tolerance for the final simulated heading \[rad\].
368    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/// Result of a successful CL-RRT* planning run.
387#[derive(Debug, Clone)]
388pub struct ClosedLoopRRTStarResult {
389    /// Simulated trajectory.
390    pub sim: SimulationResult,
391    /// The geometric path from the RRT* tree (Reeds-Shepp waypoints).
392    pub geometric_poses: Vec<Pose2D>,
393}
394
395/// Closed-Loop RRT* planner.
396///
397/// Internally runs [`RRTStarRSPlanner`] to build the tree, then evaluates all
398/// candidate goal paths with a closed-loop forward simulation.
399pub struct ClosedLoopRRTStarPlanner {
400    config: ClosedLoopRRTStarConfig,
401    obstacles: Vec<CircleObstacle>,
402    rand_area: AreaBounds,
403    inner: RRTStarRSPlanner,
404}
405
406impl ClosedLoopRRTStarPlanner {
407    /// Create a new CL-RRT* planner.
408    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    /// Plan from `start` to `goal`, returning the best feasible trajectory.
427    ///
428    /// Returns `None` if no dynamically feasible path is found.
429    pub fn planning(&mut self, start: Pose2D, goal: Pose2D) -> Option<ClosedLoopRRTStarResult> {
430        // Phase 1: grow the RRT*-Reeds-Shepp tree.
431        let _ = self.inner.planning(start, goal);
432        self.select_best_feasible(goal)
433    }
434
435    /// Plan using a deterministic sampler (for testing).
436    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    /// Access the internal RRT* tree.
450    pub fn get_tree(&self) -> &[RRTStarRSNode] {
451        self.inner.get_tree()
452    }
453
454    // -----------------------------------------------------------------------
455    // Private
456    // -----------------------------------------------------------------------
457
458    /// After the tree has been built, find all goal-candidate nodes and return
459    /// the one whose closed-loop simulation has the shortest travel time.
460    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    /// Collect node indices that are within position and yaw tolerance of the goal.
494    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    /// Trace back from `goal_index` to the root, returning waypoints as
511    /// `(x, y, yaw)` from start to goal.
512    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    /// Forward-simulate a path using pure pursuit and validate feasibility.
538    ///
539    /// Returns `Some(SimulationResult)` if feasible, `None` otherwise.
540    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        // Final yaw check
576        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        // Travel ratio check
583        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        // Collision check along simulated trajectory
594        if !self.check_sim_collision(&sim) {
595            return None;
596        }
597
598        Some(sim)
599    }
600
601    /// Check that the simulated trajectory does not collide with any obstacle.
602    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
617// ---------------------------------------------------------------------------
618// Helpers
619// ---------------------------------------------------------------------------
620
621/// Shortest signed angular difference.
622fn 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// ---------------------------------------------------------------------------
634// Tests
635// ---------------------------------------------------------------------------
636
637#[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    // -- Unicycle model tests --
646
647    #[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, &params);
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, &params);
663        assert!(approx_eq(next.v, 2.0 * params.dt, 1e-12));
664        assert!(approx_eq(next.x, 0.0, 1e-12)); // v was 0 at start
665    }
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; // small steering angle
672        let next = unicycle_update(&state, 0.0, delta, &params);
673        // Yaw should increase (turning left)
674        let expected_yaw = 1.0 / params.wheelbase * delta.tan() * params.dt;
675        assert!(approx_eq(next.yaw, expected_yaw, 1e-10));
676    }
677
678    // -- Pure pursuit tests --
679
680    #[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        // Nearest is index 0 or 1, then advanced by look-ahead
687        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)); // Clamped to accel_max
695
696        let a = pid_control(-10.0, 0.0, 2.0, 5.0);
697        assert!(approx_eq(a, -5.0, 1e-12)); // Clamped to -accel_max
698    }
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    // -- Speed profile tests --
707
708    #[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)); // start is 0
716        assert!(profile[1] > 0.0); // forward
717        assert!(approx_eq(profile[3], 0.5, 1e-12)); // end is stop_speed
718    }
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        // Yaw pointing forward (+x) but moving backward (-x)
725        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        // Segments should be negative (backward)
729        assert!(profile[1] < 0.0 || approx_eq(profile[1], 0.0, 1e-12));
730    }
731
732    // -- Extend path tests --
733
734    #[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    // -- pi_2_pi tests --
746
747    #[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    // -- Closed-loop simulation tests --
764
765    #[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]; // goal before extension point
776
777        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        // Should make progress in +x direction
782        assert!(sim.x.last().unwrap() > &0.0);
783        assert!(sim.t.len() > 1);
784    }
785
786    // -- Config defaults --
787
788    #[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    // -- Collision check --
798
799    #[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    // -- Goal index selection --
839
840    #[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),   // far from goal
853            RRTStarRSNode::new(10.0, 10.0, 0.0), // at goal
854            RRTStarRSNode::new(10.5, 10.5, 0.0), // near goal, good yaw
855            RRTStarRSNode::new(10.0, 10.0, PI),  // at goal, bad yaw
856        ];
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    // -- Generate final course --
866
867    #[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        // First point should be root
885        assert!(approx_eq(course[0].0, 0.0, 1e-12));
886        assert!(approx_eq(course[0].1, 0.0, 1e-12));
887    }
888
889    // -- Integration test: planner construction --
890
891    #[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    // -- Integration test: planning on obstacle-free env --
904
905    #[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        // With no obstacles and a straight-line goal, we should find something
936        // (though the closed-loop check may still reject if simulation doesn't
937        // converge; that is acceptable).
938        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    // -- Integration test: runs without panic with obstacles --
946
947    #[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        // Random planning may or may not find a feasible path in 100 iterations;
974        // we just verify it does not panic.
975        let _result = planner.planning(start, goal);
976    }
977
978    // -- Extend path with backward motion --
979
980    #[test]
981    fn test_extend_path_backward() {
982        // Path moving in -x while yaw = 0 (backward motion)
983        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        // Extended points should continue in -x direction
988        assert!(ecx.last().unwrap() < &1.0);
989    }
990
991    // -- SimulationResult fields --
992
993    #[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}