rust_robotics_planning/
rrt_star_reeds_shepp.rs

1#![allow(dead_code, clippy::too_many_arguments)]
2
3//! RRT*-Reeds-Shepp path planner
4//!
5//! Combines the RRT* (optimal RRT) algorithm with Reeds-Shepp curve
6//! connections for non-holonomic vehicle path planning. Unlike the Dubins
7//! variant, the vehicle can move both forward and backward.
8//!
9//! The key additions over plain RRT-Reeds-Shepp are:
10//! - **choose_parent**: picks the lowest-cost parent among nearby nodes.
11//! - **rewire**: redirects nearby nodes through the new node when beneficial.
12//! - **try_goal_path**: after adding each node, attempts a direct connection
13//!   to the goal.
14//!
15//! # References
16//!
17//! * Karaman, S. and Frazzoli, E. (2011). "Sampling-based algorithms for
18//!   optimal motion planning." *IJRR*.
19//! * Reeds, J.A. and Shepp, L.A. (1990). "Optimal paths for a car that goes
20//!   both forwards and backwards."
21
22use std::f64::consts::PI;
23
24use rand::Rng;
25
26use rust_robotics_core::types::Pose2D;
27
28use crate::reeds_shepp_path::reeds_shepp_path_planning;
29use crate::rrt::{AreaBounds, CircleObstacle};
30
31/// A node in the RRT*-Reeds-Shepp tree, carrying position and heading.
32#[derive(Debug, Clone)]
33pub struct RRTStarRSNode {
34    pub x: f64,
35    pub y: f64,
36    pub yaw: f64,
37    pub path_x: Vec<f64>,
38    pub path_y: Vec<f64>,
39    pub path_yaw: Vec<f64>,
40    pub cost: f64,
41    pub parent: Option<usize>,
42}
43
44impl RRTStarRSNode {
45    pub fn new(x: f64, y: f64, yaw: f64) -> Self {
46        Self {
47            x,
48            y,
49            yaw,
50            path_x: Vec::new(),
51            path_y: Vec::new(),
52            path_yaw: Vec::new(),
53            cost: 0.0,
54            parent: None,
55        }
56    }
57}
58
59/// Configuration for the RRT*-Reeds-Shepp planner.
60#[derive(Debug, Clone)]
61pub struct RRTStarRSConfig {
62    /// Maximum curvature = 1 / minimum turning radius \[1/m\].
63    pub curvature: f64,
64    /// Step size for sampling points along Reeds-Shepp paths \[m\].
65    pub step_size: f64,
66    /// Goal-biased sampling rate (0..100). Higher means more goal samples.
67    pub goal_sample_rate: i32,
68    /// Maximum number of RRT iterations.
69    pub max_iter: usize,
70    /// Distance threshold to consider a node at the goal position \[m\].
71    pub goal_xy_threshold: f64,
72    /// Yaw threshold to consider a node at the goal heading \[rad\].
73    pub goal_yaw_threshold: f64,
74    /// Robot radius for collision checking \[m\].
75    pub robot_radius: f64,
76    /// Near-node search radius parameter.
77    pub connect_circle_dist: f64,
78}
79
80impl Default for RRTStarRSConfig {
81    fn default() -> Self {
82        Self {
83            curvature: 1.0,
84            step_size: 0.2,
85            goal_sample_rate: 10,
86            max_iter: 200,
87            goal_xy_threshold: 0.5,
88            goal_yaw_threshold: 1.0_f64.to_radians(),
89            robot_radius: 0.0,
90            connect_circle_dist: 50.0,
91        }
92    }
93}
94
95/// Result of a successful RRT*-Reeds-Shepp planning run.
96#[derive(Debug, Clone)]
97pub struct RRTStarRSPath {
98    /// Sampled waypoints along the path: (x, y, yaw).
99    pub poses: Vec<Pose2D>,
100}
101
102/// RRT* path planner using Reeds-Shepp curves for steering.
103pub struct RRTStarRSPlanner {
104    config: RRTStarRSConfig,
105    obstacles: Vec<CircleObstacle>,
106    rand_area: AreaBounds,
107    node_list: Vec<RRTStarRSNode>,
108    start: RRTStarRSNode,
109    goal: RRTStarRSNode,
110}
111
112impl RRTStarRSPlanner {
113    /// Create a new RRT*-Reeds-Shepp planner.
114    pub fn new(
115        obstacles: Vec<CircleObstacle>,
116        rand_area: AreaBounds,
117        config: RRTStarRSConfig,
118    ) -> Self {
119        Self {
120            config,
121            obstacles,
122            rand_area,
123            node_list: Vec::new(),
124            start: RRTStarRSNode::new(0.0, 0.0, 0.0),
125            goal: RRTStarRSNode::new(0.0, 0.0, 0.0),
126        }
127    }
128
129    /// Plan a path from `start` to `goal`.
130    ///
131    /// Returns `None` if no path is found within `max_iter` iterations.
132    pub fn planning(&mut self, start: Pose2D, goal: Pose2D) -> Option<RRTStarRSPath> {
133        self.start = RRTStarRSNode::new(start.x, start.y, start.yaw);
134        self.goal = RRTStarRSNode::new(goal.x, goal.y, goal.yaw);
135        self.node_list = vec![self.start.clone()];
136
137        for _ in 0..self.config.max_iter {
138            let rnd = self.get_random_node();
139            let nearest_ind = self.get_nearest_node_index(&rnd);
140
141            if let Some(new_node) =
142                self.steer(&self.node_list[nearest_ind].clone(), &rnd, nearest_ind)
143            {
144                if self.check_collision(&new_node) {
145                    let near_inds = self.find_near_nodes(&new_node);
146                    if let Some(best_node) = self.choose_parent(new_node, &near_inds) {
147                        let new_index = self.node_list.len();
148                        self.node_list.push(best_node);
149                        self.rewire(new_index, &near_inds);
150                        self.try_goal_path(&self.node_list[new_index].clone());
151                    }
152                }
153            }
154
155            if let Some(goal_idx) = self.search_best_goal_node() {
156                return Some(self.generate_final_course(goal_idx));
157            }
158        }
159
160        self.search_best_goal_node()
161            .map(|idx| self.generate_final_course(idx))
162    }
163
164    /// Plan with a deterministic node sampler (for testing).
165    pub fn plan_with_sampler<F>(
166        &mut self,
167        start: Pose2D,
168        goal: Pose2D,
169        mut sample_node: F,
170    ) -> Option<RRTStarRSPath>
171    where
172        F: FnMut(&Self) -> RRTStarRSNode,
173    {
174        self.start = RRTStarRSNode::new(start.x, start.y, start.yaw);
175        self.goal = RRTStarRSNode::new(goal.x, goal.y, goal.yaw);
176        self.node_list = vec![self.start.clone()];
177
178        for _ in 0..self.config.max_iter {
179            let rnd = sample_node(self);
180            let nearest_ind = self.get_nearest_node_index(&rnd);
181
182            if let Some(new_node) =
183                self.steer(&self.node_list[nearest_ind].clone(), &rnd, nearest_ind)
184            {
185                if self.check_collision(&new_node) {
186                    let near_inds = self.find_near_nodes(&new_node);
187                    if let Some(best_node) = self.choose_parent(new_node, &near_inds) {
188                        let new_index = self.node_list.len();
189                        self.node_list.push(best_node);
190                        self.rewire(new_index, &near_inds);
191                        self.try_goal_path(&self.node_list[new_index].clone());
192                    }
193                }
194            }
195
196            if let Some(goal_idx) = self.search_best_goal_node() {
197                return Some(self.generate_final_course(goal_idx));
198            }
199        }
200
201        self.search_best_goal_node()
202            .map(|idx| self.generate_final_course(idx))
203    }
204
205    /// Access the internal tree for inspection or visualization.
206    pub fn get_tree(&self) -> &[RRTStarRSNode] {
207        &self.node_list
208    }
209
210    // -----------------------------------------------------------------------
211    // Private helpers
212    // -----------------------------------------------------------------------
213
214    fn get_random_node(&self) -> RRTStarRSNode {
215        let mut rng = rand::rng();
216        if rng.random_range(0..=100) > self.config.goal_sample_rate {
217            RRTStarRSNode::new(
218                rng.random_range(self.rand_area.xmin..=self.rand_area.xmax),
219                rng.random_range(self.rand_area.ymin..=self.rand_area.ymax),
220                rng.random_range(-PI..=PI),
221            )
222        } else {
223            RRTStarRSNode::new(self.goal.x, self.goal.y, self.goal.yaw)
224        }
225    }
226
227    fn get_nearest_node_index(&self, rnd_node: &RRTStarRSNode) -> usize {
228        let mut min_dist = f64::INFINITY;
229        let mut min_ind = 0;
230        for (i, node) in self.node_list.iter().enumerate() {
231            let dist = (node.x - rnd_node.x).powi(2) + (node.y - rnd_node.y).powi(2);
232            if dist < min_dist {
233                min_dist = dist;
234                min_ind = i;
235            }
236        }
237        min_ind
238    }
239
240    /// Steer from `from_node` toward `to_node` using a Reeds-Shepp curve.
241    fn steer(
242        &self,
243        from_node: &RRTStarRSNode,
244        to_node: &RRTStarRSNode,
245        from_index: usize,
246    ) -> Option<RRTStarRSNode> {
247        let result = reeds_shepp_path_planning(
248            from_node.x,
249            from_node.y,
250            from_node.yaw,
251            to_node.x,
252            to_node.y,
253            to_node.yaw,
254            self.config.curvature,
255            self.config.step_size,
256        )?;
257
258        let (px, py, pyaw, _modes, lengths) = result;
259
260        if px.is_empty() {
261            return None;
262        }
263
264        let mut new_node = from_node.clone();
265        new_node.x = *px.last().unwrap();
266        new_node.y = *py.last().unwrap();
267        new_node.yaw = *pyaw.last().unwrap();
268        new_node.path_x = px;
269        new_node.path_y = py;
270        new_node.path_yaw = pyaw;
271        new_node.cost += lengths.iter().map(|l| l.abs()).sum::<f64>();
272        new_node.parent = Some(from_index);
273
274        Some(new_node)
275    }
276
277    /// Compute the cost of steering from `from_node` to `to_node` via
278    /// Reeds-Shepp.
279    fn calc_new_cost(&self, from_node: &RRTStarRSNode, to_node: &RRTStarRSNode) -> f64 {
280        match reeds_shepp_path_planning(
281            from_node.x,
282            from_node.y,
283            from_node.yaw,
284            to_node.x,
285            to_node.y,
286            to_node.yaw,
287            self.config.curvature,
288            self.config.step_size,
289        ) {
290            Some((_px, _py, _pyaw, _modes, lengths)) => {
291                from_node.cost + lengths.iter().map(|l| l.abs()).sum::<f64>()
292            }
293            None => f64::INFINITY,
294        }
295    }
296
297    /// Check that a node's path does not collide with any obstacle.
298    fn check_collision(&self, node: &RRTStarRSNode) -> bool {
299        for obs in &self.obstacles {
300            for (&px, &py) in node.path_x.iter().zip(node.path_y.iter()) {
301                let dx = obs.x - px;
302                let dy = obs.y - py;
303                let d = (dx * dx + dy * dy).sqrt();
304                if d <= obs.radius + self.config.robot_radius {
305                    return false;
306                }
307            }
308        }
309        true
310    }
311
312    /// Find nodes within a dynamic radius around `new_node`.
313    fn find_near_nodes(&self, new_node: &RRTStarRSNode) -> Vec<usize> {
314        let nnode = self.node_list.len() + 1;
315        let r = self.config.connect_circle_dist * ((nnode as f64).ln() / nnode as f64).sqrt();
316
317        self.node_list
318            .iter()
319            .enumerate()
320            .filter_map(|(i, node)| {
321                let dist_sq = (node.x - new_node.x).powi(2) + (node.y - new_node.y).powi(2);
322                if dist_sq <= r * r {
323                    Some(i)
324                } else {
325                    None
326                }
327            })
328            .collect()
329    }
330
331    /// Choose the best parent for `new_node` from the near nodes.
332    fn choose_parent(&self, new_node: RRTStarRSNode, near_inds: &[usize]) -> Option<RRTStarRSNode> {
333        if near_inds.is_empty() {
334            return Some(new_node);
335        }
336
337        let mut best_cost = f64::INFINITY;
338        let mut best_index: Option<usize> = None;
339
340        for &i in near_inds {
341            let near_node = &self.node_list[i];
342            if let Some(t_node) = self.steer(near_node, &new_node, i) {
343                if self.check_collision(&t_node) {
344                    let cost = self.calc_new_cost(near_node, &new_node);
345                    if cost < best_cost {
346                        best_cost = cost;
347                        best_index = Some(i);
348                    }
349                }
350            }
351        }
352
353        let parent_ind = best_index?;
354        let mut result = self.steer(&self.node_list[parent_ind], &new_node, parent_ind)?;
355        result.cost = best_cost;
356        Some(result)
357    }
358
359    /// Rewire nearby nodes through `new_node_ind` if it reduces cost.
360    fn rewire(&mut self, new_node_ind: usize, near_inds: &[usize]) {
361        for &i in near_inds {
362            let near_node = self.node_list[i].clone();
363            let new_node = &self.node_list[new_node_ind];
364
365            let edge_cost = self.calc_new_cost(new_node, &near_node);
366            if edge_cost >= near_node.cost {
367                continue;
368            }
369
370            if let Some(mut edge_node) = self.steer(
371                &self.node_list[new_node_ind].clone(),
372                &near_node,
373                new_node_ind,
374            ) {
375                if self.check_collision(&edge_node) {
376                    edge_node.cost = edge_cost;
377                    self.node_list[i] = edge_node;
378                    self.propagate_cost_to_leaves(i);
379                }
380            }
381        }
382    }
383
384    /// After adding a new node, try to connect it directly to the goal.
385    fn try_goal_path(&mut self, node: &RRTStarRSNode) {
386        let goal_node = RRTStarRSNode::new(self.goal.x, self.goal.y, self.goal.yaw);
387        let node_index = self.node_list.len() - 1;
388
389        if let Some(new_node) = self.steer(node, &goal_node, node_index) {
390            if self.check_collision(&new_node) {
391                self.node_list.push(new_node);
392            }
393        }
394    }
395
396    /// Propagate cost updates to all descendants of `parent_ind`.
397    fn propagate_cost_to_leaves(&mut self, parent_ind: usize) {
398        for i in 0..self.node_list.len() {
399            if let Some(p) = self.node_list[i].parent {
400                if p == parent_ind {
401                    self.node_list[i].cost = self.calc_new_cost(
402                        &self.node_list[parent_ind].clone(),
403                        &self.node_list[i].clone(),
404                    );
405                    self.propagate_cost_to_leaves(i);
406                }
407            }
408        }
409    }
410
411    /// Search for the best (lowest-cost) node near the goal.
412    fn search_best_goal_node(&self) -> Option<usize> {
413        let mut candidates: Vec<usize> = Vec::new();
414
415        for (i, node) in self.node_list.iter().enumerate() {
416            let dx = node.x - self.goal.x;
417            let dy = node.y - self.goal.y;
418            let dist = (dx * dx + dy * dy).sqrt();
419            if dist > self.config.goal_xy_threshold {
420                continue;
421            }
422            let dyaw = angle_diff(node.yaw, self.goal.yaw).abs();
423            if dyaw > self.config.goal_yaw_threshold {
424                continue;
425            }
426            candidates.push(i);
427        }
428
429        if candidates.is_empty() {
430            return None;
431        }
432
433        candidates.into_iter().min_by(|&a, &b| {
434            self.node_list[a]
435                .cost
436                .partial_cmp(&self.node_list[b].cost)
437                .unwrap_or(std::cmp::Ordering::Equal)
438        })
439    }
440
441    /// Trace back from a goal node to the start, collecting all waypoints.
442    fn generate_final_course(&self, goal_index: usize) -> RRTStarRSPath {
443        let mut poses: Vec<Pose2D> = vec![Pose2D::new(self.goal.x, self.goal.y, self.goal.yaw)];
444
445        let mut node = &self.node_list[goal_index];
446        while node.parent.is_some() {
447            for ((&px, &py), &pyaw) in node
448                .path_x
449                .iter()
450                .rev()
451                .zip(node.path_y.iter().rev())
452                .zip(node.path_yaw.iter().rev())
453            {
454                poses.push(Pose2D::new(px, py, pyaw));
455            }
456            node = &self.node_list[node.parent.unwrap()];
457        }
458
459        poses.push(Pose2D::new(self.start.x, self.start.y, self.start.yaw));
460        poses.reverse();
461
462        RRTStarRSPath { poses }
463    }
464}
465
466// ---------------------------------------------------------------------------
467// Helpers
468// ---------------------------------------------------------------------------
469
470/// Compute the shortest signed angular difference.
471fn angle_diff(a: f64, b: f64) -> f64 {
472    let mut d = a - b;
473    while d > PI {
474        d -= 2.0 * PI;
475    }
476    while d < -PI {
477        d += 2.0 * PI;
478    }
479    d
480}
481
482// ---------------------------------------------------------------------------
483// Tests
484// ---------------------------------------------------------------------------
485
486#[cfg(test)]
487mod tests {
488    use super::*;
489
490    fn approx_eq(a: f64, b: f64, tol: f64) -> bool {
491        (a - b).abs() < tol
492    }
493
494    fn create_obstacle_list() -> Vec<CircleObstacle> {
495        vec![
496            CircleObstacle::new(5.0, 5.0, 1.0),
497            CircleObstacle::new(4.0, 6.0, 1.0),
498            CircleObstacle::new(4.0, 8.0, 1.0),
499            CircleObstacle::new(4.0, 10.0, 1.0),
500            CircleObstacle::new(6.0, 5.0, 1.0),
501            CircleObstacle::new(7.0, 5.0, 1.0),
502            CircleObstacle::new(8.0, 6.0, 1.0),
503            CircleObstacle::new(8.0, 8.0, 1.0),
504            CircleObstacle::new(8.0, 10.0, 1.0),
505        ]
506    }
507
508    #[test]
509    fn test_config_default() {
510        let config = RRTStarRSConfig::default();
511        assert_eq!(config.curvature, 1.0);
512        assert_eq!(config.step_size, 0.2);
513        assert_eq!(config.max_iter, 200);
514        assert_eq!(config.goal_sample_rate, 10);
515        assert_eq!(config.connect_circle_dist, 50.0);
516        assert!(approx_eq(config.goal_xy_threshold, 0.5, 1e-12));
517    }
518
519    #[test]
520    fn test_node_creation() {
521        let node = RRTStarRSNode::new(1.0, 2.0, 0.5);
522        assert_eq!(node.x, 1.0);
523        assert_eq!(node.y, 2.0);
524        assert_eq!(node.yaw, 0.5);
525        assert_eq!(node.cost, 0.0);
526        assert!(node.parent.is_none());
527        assert!(node.path_x.is_empty());
528    }
529
530    #[test]
531    fn test_angle_diff() {
532        assert!(approx_eq(angle_diff(0.0, 0.0), 0.0, 1e-12));
533        assert!(approx_eq(angle_diff(PI, 0.0), PI, 1e-12));
534        assert!(approx_eq(angle_diff(0.0, PI), -PI, 1e-12));
535        assert!(approx_eq(angle_diff(3.0 * PI / 2.0, -PI / 2.0), 0.0, 1e-12));
536    }
537
538    #[test]
539    fn test_collision_check_no_obstacles() {
540        let config = RRTStarRSConfig::default();
541        let rand_area = AreaBounds::new(-2.0, 15.0, -2.0, 15.0);
542        let planner = RRTStarRSPlanner::new(vec![], rand_area, config);
543
544        let mut node = RRTStarRSNode::new(1.0, 1.0, 0.0);
545        node.path_x = vec![0.0, 0.5, 1.0];
546        node.path_y = vec![0.0, 0.5, 1.0];
547        assert!(planner.check_collision(&node));
548    }
549
550    #[test]
551    fn test_collision_check_with_obstacle() {
552        let obstacles = vec![CircleObstacle::new(0.5, 0.5, 0.3)];
553        let config = RRTStarRSConfig::default();
554        let rand_area = AreaBounds::new(-2.0, 15.0, -2.0, 15.0);
555        let planner = RRTStarRSPlanner::new(obstacles, rand_area, config);
556
557        let mut node = RRTStarRSNode::new(1.0, 1.0, 0.0);
558        node.path_x = vec![0.0, 0.5, 1.0];
559        node.path_y = vec![0.0, 0.5, 1.0];
560        assert!(!planner.check_collision(&node));
561    }
562
563    #[test]
564    fn test_steer_produces_reeds_shepp_path() {
565        let config = RRTStarRSConfig::default();
566        let rand_area = AreaBounds::new(-2.0, 15.0, -2.0, 15.0);
567        let mut planner = RRTStarRSPlanner::new(vec![], rand_area, config);
568        planner.start = RRTStarRSNode::new(0.0, 0.0, 0.0);
569        planner.goal = RRTStarRSNode::new(10.0, 10.0, 0.0);
570        planner.node_list = vec![planner.start.clone()];
571
572        let from_node = RRTStarRSNode::new(0.0, 0.0, 0.0);
573        let to_node = RRTStarRSNode::new(5.0, 5.0, PI / 2.0);
574
575        let result = planner.steer(&from_node, &to_node, 0);
576        assert!(result.is_some());
577
578        let new_node = result.unwrap();
579        assert!(!new_node.path_x.is_empty());
580        assert_eq!(new_node.path_x.len(), new_node.path_y.len());
581        assert_eq!(new_node.path_x.len(), new_node.path_yaw.len());
582        assert!(new_node.cost > 0.0);
583        assert!(new_node.parent.is_some());
584    }
585
586    #[test]
587    fn test_find_near_nodes() {
588        let config = RRTStarRSConfig {
589            connect_circle_dist: 50.0,
590            ..Default::default()
591        };
592        let rand_area = AreaBounds::new(-2.0, 15.0, -2.0, 15.0);
593        let mut planner = RRTStarRSPlanner::new(vec![], rand_area, config);
594        planner.node_list = vec![
595            RRTStarRSNode::new(0.0, 0.0, 0.0),
596            RRTStarRSNode::new(1.0, 1.0, 0.0),
597            RRTStarRSNode::new(100.0, 100.0, 0.0),
598        ];
599
600        let query = RRTStarRSNode::new(0.5, 0.5, 0.0);
601        let near = planner.find_near_nodes(&query);
602        assert!(near.contains(&0));
603        assert!(near.contains(&1));
604        assert!(!near.contains(&2));
605    }
606
607    #[test]
608    fn test_search_best_goal_node() {
609        let config = RRTStarRSConfig {
610            goal_xy_threshold: 1.0,
611            goal_yaw_threshold: 0.5,
612            ..Default::default()
613        };
614        let rand_area = AreaBounds::new(-2.0, 15.0, -2.0, 15.0);
615        let mut planner = RRTStarRSPlanner::new(vec![], rand_area, config);
616        planner.goal = RRTStarRSNode::new(10.0, 10.0, 0.0);
617
618        // No nodes near goal
619        planner.node_list = vec![RRTStarRSNode::new(0.0, 0.0, 0.0)];
620        assert!(planner.search_best_goal_node().is_none());
621
622        // Node near goal in position but wrong yaw
623        let mut n = RRTStarRSNode::new(10.0, 10.0, PI);
624        n.cost = 5.0;
625        planner.node_list.push(n);
626        assert!(planner.search_best_goal_node().is_none());
627
628        // Node near goal in both position and yaw
629        let mut n = RRTStarRSNode::new(10.2, 10.2, 0.1);
630        n.cost = 8.0;
631        planner.node_list.push(n);
632        assert_eq!(planner.search_best_goal_node(), Some(2));
633    }
634
635    #[test]
636    fn test_deterministic_planning_no_obstacles() {
637        let config = RRTStarRSConfig {
638            curvature: 1.0,
639            step_size: 0.2,
640            goal_sample_rate: 10,
641            max_iter: 500,
642            goal_xy_threshold: 1.5,
643            goal_yaw_threshold: 0.5,
644            robot_radius: 0.0,
645            connect_circle_dist: 50.0,
646        };
647        let rand_area = AreaBounds::new(-2.0, 15.0, -2.0, 15.0);
648        let mut planner = RRTStarRSPlanner::new(vec![], rand_area, config);
649
650        let start = Pose2D::new(0.0, 0.0, 0.0);
651        let goal = Pose2D::new(5.0, 5.0, 0.0);
652
653        // Alternate between a midpoint and the goal to avoid the edge case
654        // where from_node == to_node in Reeds-Shepp planning.
655        let mut call = 0;
656        let result = planner.plan_with_sampler(start, goal, |p| {
657            call += 1;
658            if call % 2 == 0 {
659                RRTStarRSNode::new(p.goal.x, p.goal.y, p.goal.yaw)
660            } else {
661                RRTStarRSNode::new(p.goal.x * 0.5, p.goal.y * 0.5, p.goal.yaw)
662            }
663        });
664
665        assert!(result.is_some(), "Should find a path with no obstacles");
666        let path = result.unwrap();
667        assert!(path.poses.len() >= 2);
668
669        let first = &path.poses[0];
670        let last = &path.poses[path.poses.len() - 1];
671        assert!(approx_eq(first.x, 0.0, 0.5));
672        assert!(approx_eq(first.y, 0.0, 0.5));
673        assert!(approx_eq(last.x, 5.0, 1.5));
674        assert!(approx_eq(last.y, 5.0, 1.5));
675    }
676
677    #[test]
678    fn test_choose_parent_picks_lower_cost() {
679        let config = RRTStarRSConfig {
680            curvature: 1.0,
681            step_size: 0.2,
682            connect_circle_dist: 100.0,
683            ..Default::default()
684        };
685        let rand_area = AreaBounds::new(-5.0, 20.0, -5.0, 20.0);
686        let mut planner = RRTStarRSPlanner::new(vec![], rand_area, config);
687        planner.start = RRTStarRSNode::new(0.0, 0.0, 0.0);
688        planner.goal = RRTStarRSNode::new(20.0, 0.0, 0.0);
689
690        // Node 0: start (cost 0)
691        // Node 1: cost 100 (expensive parent)
692        let mut expensive = RRTStarRSNode::new(5.0, 0.0, 0.0);
693        expensive.cost = 100.0;
694        expensive.parent = Some(0);
695
696        planner.node_list = vec![planner.start.clone(), expensive];
697
698        let new_node = RRTStarRSNode::new(3.0, 0.0, 0.0);
699        let near_inds = vec![0, 1];
700        let result = planner.choose_parent(new_node, &near_inds);
701        assert!(result.is_some());
702        let chosen = result.unwrap();
703        assert_eq!(chosen.parent, Some(0));
704    }
705
706    #[test]
707    fn test_planning_with_obstacles_runs_without_panic() {
708        let obstacles = create_obstacle_list();
709        let config = RRTStarRSConfig {
710            curvature: 1.0,
711            step_size: 0.2,
712            goal_sample_rate: 10,
713            max_iter: 200,
714            goal_xy_threshold: 1.5,
715            goal_yaw_threshold: 1.0,
716            robot_radius: 0.0,
717            connect_circle_dist: 50.0,
718        };
719        let rand_area = AreaBounds::new(-2.0, 15.0, -2.0, 15.0);
720        let mut planner = RRTStarRSPlanner::new(obstacles, rand_area, config);
721
722        let start = Pose2D::new(0.0, 0.0, 0.0);
723        let goal = Pose2D::new(6.0, 7.0, PI / 2.0);
724
725        let result = planner.planning(start, goal);
726        if let Some(path) = result {
727            assert!(path.poses.len() >= 2);
728            for pose in &path.poses {
729                assert!(pose.x.is_finite());
730                assert!(pose.y.is_finite());
731                assert!(pose.yaw.is_finite());
732            }
733        }
734    }
735
736    #[test]
737    fn test_generate_final_course() {
738        let config = RRTStarRSConfig::default();
739        let rand_area = AreaBounds::new(-2.0, 15.0, -2.0, 15.0);
740        let mut planner = RRTStarRSPlanner::new(vec![], rand_area, config);
741        planner.start = RRTStarRSNode::new(0.0, 0.0, 0.0);
742        planner.goal = RRTStarRSNode::new(10.0, 10.0, 0.0);
743
744        let root = RRTStarRSNode::new(0.0, 0.0, 0.0);
745        let mut child = RRTStarRSNode::new(5.0, 5.0, 0.5);
746        child.parent = Some(0);
747        child.path_x = vec![0.0, 2.5, 5.0];
748        child.path_y = vec![0.0, 2.5, 5.0];
749        child.path_yaw = vec![0.0, 0.25, 0.5];
750
751        planner.node_list = vec![root, child];
752
753        let course = planner.generate_final_course(1);
754        assert!(course.poses.len() >= 3);
755        assert!(approx_eq(course.poses[0].x, 0.0, 1e-12));
756        let last = course.poses.last().unwrap();
757        assert!(approx_eq(last.x, 10.0, 1e-12));
758        assert!(approx_eq(last.y, 10.0, 1e-12));
759    }
760
761    #[test]
762    fn test_try_goal_path_adds_node() {
763        let config = RRTStarRSConfig {
764            curvature: 1.0,
765            step_size: 0.2,
766            goal_xy_threshold: 1.0,
767            goal_yaw_threshold: 1.0,
768            ..Default::default()
769        };
770        let rand_area = AreaBounds::new(-5.0, 20.0, -5.0, 20.0);
771        let mut planner = RRTStarRSPlanner::new(vec![], rand_area, config);
772        planner.start = RRTStarRSNode::new(0.0, 0.0, 0.0);
773        planner.goal = RRTStarRSNode::new(5.0, 0.0, 0.0);
774        planner.node_list = vec![planner.start.clone()];
775
776        // Add a node close to the goal
777        let near_goal = RRTStarRSNode::new(3.0, 0.0, 0.0);
778        if let Some(n) = planner.steer(&planner.node_list[0].clone(), &near_goal, 0) {
779            planner.node_list.push(n);
780        }
781
782        let count_before = planner.node_list.len();
783        let last_node = planner.node_list.last().unwrap().clone();
784        planner.try_goal_path(&last_node);
785        // try_goal_path should add a new node (direct connection to goal)
786        assert!(
787            planner.node_list.len() >= count_before,
788            "try_goal_path should not remove nodes"
789        );
790    }
791}
rust_robotics_planning/rrt_star_reeds_shepp.rs

rust_robotics_planning/
rrt_star_reeds_shepp.rs
