rust_robotics_planning/
lqr_rrt_star.rs

1#![allow(dead_code, clippy::too_many_arguments)]
2
3//! LQR-RRT* path planning algorithm
4//!
5//! Combines RRT* with LQR-based steering and cost computation.
6//! Instead of straight-line extension, the tree is grown using
7//! LQR trajectories, and the cost metric is the sum of LQR
8//! trajectory segment lengths.
9//!
10//! Reference: PythonRobotics LQRRRTStar by Atsushi Sakai
11
12use rand::Rng;
13
14use crate::lqr_planner::{LqrPlanner, LqrPlannerConfig};
15use rust_robotics_core::{Path2D, Point2D, RoboticsError, RoboticsResult};
16
17/// A node in the LQR-RRT* tree.
18#[derive(Debug, Clone)]
19pub struct Node {
20    pub x: f64,
21    pub y: f64,
22    pub path_x: Vec<f64>,
23    pub path_y: Vec<f64>,
24    pub cost: f64,
25    pub parent: Option<usize>,
26}
27
28impl Node {
29    pub fn new(x: f64, y: f64) -> Self {
30        Node {
31            x,
32            y,
33            path_x: Vec::new(),
34            path_y: Vec::new(),
35            cost: 0.0,
36            parent: None,
37        }
38    }
39}
40
41/// Configuration for the LQR-RRT* planner.
42#[derive(Debug, Clone)]
43pub struct LqrRrtStarConfig {
44    /// Random sampling area minimum bound
45    pub min_rand: f64,
46    /// Random sampling area maximum bound
47    pub max_rand: f64,
48    /// Goal sampling rate (0-100). Higher means more goal-biased sampling.
49    pub goal_sample_rate: i32,
50    /// Maximum number of iterations
51    pub max_iter: i32,
52    /// Connection circle distance for finding near nodes
53    pub connect_circle_dist: f64,
54    /// Step size for path resampling
55    pub step_size: f64,
56    /// Goal proximity threshold
57    pub goal_xy_th: f64,
58    /// Robot radius for collision checking
59    pub robot_radius: f64,
60    /// Whether to continue searching until max iterations
61    pub search_until_max_iter: bool,
62    /// Configuration for the internal LQR planner
63    pub lqr_config: LqrPlannerConfig,
64}
65
66impl Default for LqrRrtStarConfig {
67    fn default() -> Self {
68        Self {
69            min_rand: -2.0,
70            max_rand: 15.0,
71            goal_sample_rate: 10,
72            max_iter: 200,
73            connect_circle_dist: 50.0,
74            step_size: 0.2,
75            goal_xy_th: 0.5,
76            robot_radius: 0.0,
77            search_until_max_iter: true,
78            lqr_config: LqrPlannerConfig::default(),
79        }
80    }
81}
82
83/// LQR-RRT* planner.
84///
85/// Uses LQR trajectories for tree steering and LQR path length
86/// as the cost metric, combined with the RRT* rewiring strategy.
87pub struct LqrRrtStar {
88    pub start: Node,
89    pub end: Node,
90    pub config: LqrRrtStarConfig,
91    pub obstacle_list: Vec<(f64, f64, f64)>,
92    pub node_list: Vec<Node>,
93    lqr_planner: LqrPlanner,
94}
95
96impl LqrRrtStar {
97    /// Create a new LQR-RRT* planner.
98    pub fn new(
99        start: (f64, f64),
100        goal: (f64, f64),
101        obstacle_list: Vec<(f64, f64, f64)>,
102        config: LqrRrtStarConfig,
103    ) -> Self {
104        let lqr_planner = LqrPlanner::new(config.lqr_config.clone());
105        LqrRrtStar {
106            start: Node::new(start.0, start.1),
107            end: Node::new(goal.0, goal.1),
108            config,
109            obstacle_list,
110            node_list: Vec::new(),
111            lqr_planner,
112        }
113    }
114
115    /// Run the LQR-RRT* planning algorithm.
116    ///
117    /// Returns a path as a vector of \[x, y\] pairs (goal to start order),
118    /// or `None` if no path is found.
119    pub fn planning(&mut self) -> Option<Vec<[f64; 2]>> {
120        self.planning_with_sampler(|planner| planner.get_random_node())
121    }
122
123    /// Run planning with a custom sampling function (useful for testing).
124    fn planning_with_sampler<F>(&mut self, mut sample_node: F) -> Option<Vec<[f64; 2]>>
125    where
126        F: FnMut(&LqrRrtStar) -> Node,
127    {
128        self.node_list = vec![self.start.clone()];
129
130        for _i in 0..self.config.max_iter {
131            let rnd = sample_node(self);
132            let nearest_ind = self.get_nearest_node_index(&rnd);
133            let new_node = self.steer(&self.node_list[nearest_ind].clone(), &rnd);
134
135            if let Some(node) = new_node {
136                if self.check_collision_free(&node) {
137                    let near_inds = self.find_near_nodes(&node);
138                    let chosen = self.choose_parent(node, &near_inds);
139
140                    if let Some(new_node) = chosen {
141                        let new_index = self.node_list.len();
142                        self.node_list.push(new_node);
143                        self.rewire(new_index, &near_inds);
144                    }
145                }
146            }
147
148            if !self.config.search_until_max_iter {
149                if let Some(last_index) = self.search_best_goal_node() {
150                    return Some(self.generate_final_course(last_index));
151                }
152            }
153        }
154
155        if let Some(last_index) = self.search_best_goal_node() {
156            return Some(self.generate_final_course(last_index));
157        }
158
159        None
160    }
161
162    /// Sample a random node, with goal bias.
163    fn get_random_node(&self) -> Node {
164        let mut rng = rand::rng();
165        if rng.random_range(0..=100) > self.config.goal_sample_rate {
166            Node::new(
167                rng.random_range(self.config.min_rand..=self.config.max_rand),
168                rng.random_range(self.config.min_rand..=self.config.max_rand),
169            )
170        } else {
171            Node::new(self.end.x, self.end.y)
172        }
173    }
174
175    /// Find the index of the nearest node to `rnd_node` using Euclidean distance.
176    fn get_nearest_node_index(&self, rnd_node: &Node) -> usize {
177        let mut min_dist = f64::INFINITY;
178        let mut nearest_ind = 0;
179        for (i, node) in self.node_list.iter().enumerate() {
180            let dist = self.euclidean_distance(node, rnd_node);
181            if dist < min_dist {
182                min_dist = dist;
183                nearest_ind = i;
184            }
185        }
186        nearest_ind
187    }
188
189    /// Steer from `from_node` towards `to_node` using LQR trajectory.
190    ///
191    /// Returns a new node at the end of the LQR path, or `None` if
192    /// the LQR planner fails.
193    fn steer(&self, from_node: &Node, to_node: &Node) -> Option<Node> {
194        let (wx, wy) = self
195            .lqr_planner
196            .planning(from_node.x, from_node.y, to_node.x, to_node.y)
197            .ok()?;
198
199        let (px, py, course_lens) = self.sample_path(&wx, &wy, self.config.step_size);
200
201        if px.is_empty() {
202            return None;
203        }
204
205        let mut new_node = from_node.clone();
206        new_node.x = *px.last().unwrap();
207        new_node.y = *py.last().unwrap();
208        new_node.path_x = px;
209        new_node.path_y = py;
210        new_node.cost += course_lens.iter().map(|c| c.abs()).sum::<f64>();
211        // parent will be set by choose_parent or the caller
212        new_node.parent = None;
213
214        Some(new_node)
215    }
216
217    /// Resample an LQR trajectory at the given step size.
218    ///
219    /// Returns resampled (px, py) and segment lengths.
220    fn sample_path(&self, wx: &[f64], wy: &[f64], step: f64) -> (Vec<f64>, Vec<f64>, Vec<f64>) {
221        let mut px = Vec::new();
222        let mut py = Vec::new();
223
224        for i in 0..wx.len().saturating_sub(1) {
225            let mut t = 0.0;
226            while t < 1.0 {
227                px.push(t * wx[i + 1] + (1.0 - t) * wx[i]);
228                py.push(t * wy[i + 1] + (1.0 - t) * wy[i]);
229                t += step;
230            }
231        }
232
233        if px.len() < 2 {
234            return (px, py, Vec::new());
235        }
236
237        let clen: Vec<f64> = px
238            .windows(2)
239            .zip(py.windows(2))
240            .map(|(xw, yw)| {
241                let dx = xw[1] - xw[0];
242                let dy = yw[1] - yw[0];
243                (dx * dx + dy * dy).sqrt()
244            })
245            .collect();
246
247        (px, py, clen)
248    }
249
250    /// Compute the cost of an LQR path from `from_node` to `to_node`.
251    fn calc_new_cost(&self, from_node: &Node, to_node: &Node) -> f64 {
252        let result = self
253            .lqr_planner
254            .planning(from_node.x, from_node.y, to_node.x, to_node.y);
255
256        let (wx, wy) = match result {
257            Ok(v) => v,
258            Err(_) => return f64::INFINITY,
259        };
260
261        let (_px, _py, course_lens) = self.sample_path(&wx, &wy, self.config.step_size);
262
263        if course_lens.is_empty() {
264            return f64::INFINITY;
265        }
266
267        from_node.cost + course_lens.iter().sum::<f64>()
268    }
269
270    /// Check if a node's path is collision-free against all obstacles.
271    fn check_collision_free(&self, node: &Node) -> bool {
272        if node.path_x.is_empty() || node.path_y.is_empty() {
273            return true;
274        }
275        for &(ox, oy, size) in &self.obstacle_list {
276            for (&px, &py) in node.path_x.iter().zip(node.path_y.iter()) {
277                let d = (px - ox).powi(2) + (py - oy).powi(2);
278                if d <= (size + self.config.robot_radius).powi(2) {
279                    return false;
280                }
281            }
282        }
283        true
284    }
285
286    /// Find all nodes within the connection radius.
287    fn find_near_nodes(&self, new_node: &Node) -> Vec<usize> {
288        let nnode = self.node_list.len() + 1;
289        let r = self.config.connect_circle_dist * ((nnode as f64).ln() / nnode as f64).sqrt();
290
291        self.node_list
292            .iter()
293            .enumerate()
294            .filter_map(|(i, node)| {
295                let dist_sq = (node.x - new_node.x).powi(2) + (node.y - new_node.y).powi(2);
296                if dist_sq <= r.powi(2) {
297                    Some(i)
298                } else {
299                    None
300                }
301            })
302            .collect()
303    }
304
305    /// Choose the best parent for `new_node` from the near nodes,
306    /// using LQR cost as the metric.
307    fn choose_parent(&self, mut new_node: Node, near_inds: &[usize]) -> Option<Node> {
308        if near_inds.is_empty() {
309            return None;
310        }
311
312        let mut best_cost = f64::INFINITY;
313        let mut best_ind: Option<usize> = None;
314
315        for &i in near_inds {
316            let near_node = &self.node_list[i];
317            let cost = self.calc_new_cost(near_node, &new_node);
318
319            // Build a temporary node with the LQR path to check collision
320            if cost < best_cost {
321                let t_node = self.steer(near_node, &new_node);
322                if let Some(ref tn) = t_node {
323                    if self.check_collision_free(tn) {
324                        best_cost = cost;
325                        best_ind = Some(i);
326                    }
327                }
328            }
329        }
330
331        let parent_ind = best_ind?;
332
333        // Re-steer from the best parent to get the actual path
334        let near_node = &self.node_list[parent_ind].clone();
335        if let Some(mut result_node) = self.steer(near_node, &new_node) {
336            result_node.cost = best_cost;
337            result_node.parent = Some(parent_ind);
338            Some(result_node)
339        } else {
340            // Fallback: use the new_node as-is with the best parent
341            new_node.cost = best_cost;
342            new_node.parent = Some(parent_ind);
343            Some(new_node)
344        }
345    }
346
347    /// Rewire the tree: check if routing through `new_node_ind` is cheaper
348    /// for any of the `near_inds` nodes.
349    fn rewire(&mut self, new_node_ind: usize, near_inds: &[usize]) {
350        for &i in near_inds {
351            let near_node = self.node_list[i].clone();
352            let new_node = self.node_list[new_node_ind].clone();
353
354            let edge_cost = self.calc_new_cost(&new_node, &near_node);
355
356            if edge_cost < near_node.cost {
357                if let Some(mut edge_node) = self.steer(&new_node, &near_node) {
358                    edge_node.cost = edge_cost;
359                    edge_node.parent = Some(new_node_ind);
360
361                    if self.check_collision_free(&edge_node) {
362                        self.node_list[i] = edge_node;
363                        self.propagate_cost_to_leaves(i);
364                    }
365                }
366            }
367        }
368    }
369
370    /// Propagate cost updates from a parent to all descendants.
371    fn propagate_cost_to_leaves(&mut self, parent_ind: usize) {
372        for i in 0..self.node_list.len() {
373            if let Some(node_parent) = self.node_list[i].parent {
374                if node_parent == parent_ind {
375                    let parent = self.node_list[parent_ind].clone();
376                    self.node_list[i].cost =
377                        self.calc_new_cost(&parent, &self.node_list[i].clone());
378                    self.propagate_cost_to_leaves(i);
379                }
380            }
381        }
382    }
383
384    /// Search for the best goal node within `goal_xy_th` distance.
385    fn search_best_goal_node(&self) -> Option<usize> {
386        let dist_to_goal_list: Vec<f64> = self
387            .node_list
388            .iter()
389            .map(|n| self.calc_dist_to_goal(n.x, n.y))
390            .collect();
391
392        let goal_inds: Vec<usize> = dist_to_goal_list
393            .iter()
394            .enumerate()
395            .filter_map(|(i, &dist)| {
396                if dist <= self.config.goal_xy_th {
397                    Some(i)
398                } else {
399                    None
400                }
401            })
402            .collect();
403
404        if goal_inds.is_empty() {
405            return None;
406        }
407
408        let mut min_cost = f64::INFINITY;
409        let mut best_ind = None;
410        for &i in &goal_inds {
411            if self.node_list[i].cost < min_cost {
412                min_cost = self.node_list[i].cost;
413                best_ind = Some(i);
414            }
415        }
416
417        best_ind
418    }
419
420    /// Generate the final path by tracing from goal node back to start.
421    ///
422    /// Returns path segments in reverse (goal to start), including
423    /// the full LQR sub-paths stored at each node.
424    fn generate_final_course(&self, goal_index: usize) -> Vec<[f64; 2]> {
425        let mut path = vec![[self.end.x, self.end.y]];
426        let mut node = &self.node_list[goal_index];
427
428        while let Some(parent_ind) = node.parent {
429            for (&ix, &iy) in node.path_x.iter().rev().zip(node.path_y.iter().rev()) {
430                path.push([ix, iy]);
431            }
432            node = &self.node_list[parent_ind];
433        }
434        path.push([self.start.x, self.start.y]);
435
436        path
437    }
438
439    fn calc_dist_to_goal(&self, x: f64, y: f64) -> f64 {
440        let dx = x - self.end.x;
441        let dy = y - self.end.y;
442        (dx * dx + dy * dy).sqrt()
443    }
444
445    fn euclidean_distance(&self, a: &Node, b: &Node) -> f64 {
446        let dx = a.x - b.x;
447        let dy = a.y - b.y;
448        (dx * dx + dy * dy).sqrt()
449    }
450
451    /// Plan a path from `start` to `goal`, returning a [`Path2D`].
452    pub fn plan_from(&mut self, start: Point2D, goal: Point2D) -> RoboticsResult<Path2D> {
453        self.start = Node::new(start.x, start.y);
454        self.end = Node::new(goal.x, goal.y);
455
456        self.planning()
457            .map(|raw_path| {
458                Path2D::from_points(
459                    raw_path
460                        .into_iter()
461                        .rev()
462                        .map(|p| Point2D::new(p[0], p[1]))
463                        .collect(),
464                )
465            })
466            .ok_or_else(|| {
467                RoboticsError::PlanningError(
468                    "LQR-RRT*: Cannot find path within max iterations".to_string(),
469                )
470            })
471    }
472
473    /// Get the tree nodes for external inspection.
474    pub fn get_tree(&self) -> &[Node] {
475        &self.node_list
476    }
477
478    /// Get the obstacle list.
479    pub fn get_obstacles(&self) -> &[(f64, f64, f64)] {
480        &self.obstacle_list
481    }
482}
483
484#[cfg(test)]
485mod tests {
486    use super::*;
487
488    fn default_obstacle_list() -> Vec<(f64, f64, f64)> {
489        vec![
490            (5.0, 5.0, 1.0),
491            (4.0, 6.0, 1.0),
492            (4.0, 7.5, 1.0),
493            (4.0, 9.0, 1.0),
494            (6.0, 5.0, 1.0),
495            (7.0, 5.0, 1.0),
496        ]
497    }
498
499    fn default_config() -> LqrRrtStarConfig {
500        LqrRrtStarConfig {
501            min_rand: -2.0,
502            max_rand: 15.0,
503            goal_sample_rate: 10,
504            max_iter: 200,
505            connect_circle_dist: 50.0,
506            step_size: 0.2,
507            goal_xy_th: 0.5,
508            robot_radius: 0.0,
509            search_until_max_iter: true,
510            lqr_config: LqrPlannerConfig::default(),
511        }
512    }
513
514    #[test]
515    fn test_node_creation() {
516        let node = Node::new(1.0, 2.0);
517        assert_eq!(node.x, 1.0);
518        assert_eq!(node.y, 2.0);
519        assert!(node.path_x.is_empty());
520        assert!(node.path_y.is_empty());
521        assert_eq!(node.cost, 0.0);
522        assert!(node.parent.is_none());
523    }
524
525    #[test]
526    fn test_config_default() {
527        let config = LqrRrtStarConfig::default();
528        assert_eq!(config.min_rand, -2.0);
529        assert_eq!(config.max_rand, 15.0);
530        assert_eq!(config.goal_sample_rate, 10);
531        assert_eq!(config.max_iter, 200);
532        assert_eq!(config.step_size, 0.2);
533        assert_eq!(config.goal_xy_th, 0.5);
534    }
535
536    #[test]
537    fn test_sample_path_basic() {
538        let config = default_config();
539        let planner = LqrRrtStar::new((0.0, 0.0), (10.0, 10.0), vec![], config);
540
541        let wx = vec![0.0, 1.0, 2.0];
542        let wy = vec![0.0, 1.0, 2.0];
543        let (px, py, clen) = planner.sample_path(&wx, &wy, 0.5);
544
545        assert!(!px.is_empty());
546        assert_eq!(px.len(), py.len());
547        assert!(!clen.is_empty());
548        // All segment lengths should be positive
549        for &c in &clen {
550            assert!(c >= 0.0);
551        }
552    }
553
554    #[test]
555    fn test_sample_path_empty() {
556        let config = default_config();
557        let planner = LqrRrtStar::new((0.0, 0.0), (10.0, 10.0), vec![], config);
558
559        let (px, py, clen) = planner.sample_path(&[], &[], 0.5);
560        assert!(px.is_empty());
561        assert!(py.is_empty());
562        assert!(clen.is_empty());
563    }
564
565    #[test]
566    fn test_sample_path_single_point() {
567        let config = default_config();
568        let planner = LqrRrtStar::new((0.0, 0.0), (10.0, 10.0), vec![], config);
569
570        let (px, py, clen) = planner.sample_path(&[1.0], &[2.0], 0.5);
571        assert!(px.is_empty());
572        assert!(py.is_empty());
573        assert!(clen.is_empty());
574    }
575
576    #[test]
577    fn test_lqr_steer_produces_path() {
578        let config = default_config();
579        let planner = LqrRrtStar::new((0.0, 0.0), (10.0, 10.0), vec![], config);
580
581        let from = Node::new(0.0, 0.0);
582        let to = Node::new(3.0, 3.0);
583        let result = planner.steer(&from, &to);
584
585        assert!(result.is_some());
586        let node = result.unwrap();
587        assert!(!node.path_x.is_empty());
588        assert!(!node.path_y.is_empty());
589        assert!(node.cost > 0.0);
590    }
591
592    #[test]
593    fn test_collision_free_no_obstacles() {
594        let config = default_config();
595        let planner = LqrRrtStar::new((0.0, 0.0), (10.0, 10.0), vec![], config);
596
597        let mut node = Node::new(1.0, 1.0);
598        node.path_x = vec![0.0, 0.5, 1.0];
599        node.path_y = vec![0.0, 0.5, 1.0];
600        assert!(planner.check_collision_free(&node));
601    }
602
603    #[test]
604    fn test_collision_detected() {
605        let config = default_config();
606        let planner = LqrRrtStar::new((0.0, 0.0), (10.0, 10.0), vec![(0.5, 0.5, 1.0)], config);
607
608        let mut node = Node::new(1.0, 1.0);
609        node.path_x = vec![0.0, 0.5, 1.0];
610        node.path_y = vec![0.0, 0.5, 1.0];
611        assert!(!planner.check_collision_free(&node));
612    }
613
614    #[test]
615    fn test_find_near_nodes() {
616        let mut config = default_config();
617        config.connect_circle_dist = 100.0;
618        let mut planner = LqrRrtStar::new((0.0, 0.0), (10.0, 10.0), vec![], config);
619
620        planner.node_list = vec![
621            Node::new(0.0, 0.0),
622            Node::new(1.0, 0.0),
623            Node::new(100.0, 100.0),
624        ];
625
626        let query = Node::new(0.5, 0.0);
627        let near = planner.find_near_nodes(&query);
628        // Nodes 0 and 1 should be near; node 2 is far away
629        assert!(near.contains(&0));
630        assert!(near.contains(&1));
631    }
632
633    #[test]
634    fn test_euclidean_distance() {
635        let config = default_config();
636        let planner = LqrRrtStar::new((0.0, 0.0), (10.0, 10.0), vec![], config);
637
638        let a = Node::new(0.0, 0.0);
639        let b = Node::new(3.0, 4.0);
640        let d = planner.euclidean_distance(&a, &b);
641        assert!((d - 5.0).abs() < 1e-10);
642    }
643
644    #[test]
645    fn test_calc_dist_to_goal() {
646        let config = default_config();
647        let planner = LqrRrtStar::new((0.0, 0.0), (10.0, 10.0), vec![], config);
648
649        let d = planner.calc_dist_to_goal(7.0, 6.0);
650        let expected = ((7.0 - 10.0_f64).powi(2) + (6.0 - 10.0_f64).powi(2)).sqrt();
651        assert!((d - expected).abs() < 1e-10);
652    }
653
654    #[test]
655    fn test_planning_no_obstacles() {
656        let mut config = default_config();
657        config.max_iter = 300;
658        config.search_until_max_iter = true;
659        let mut planner = LqrRrtStar::new((0.0, 0.0), (5.0, 5.0), vec![], config);
660
661        let path = planner.planning();
662        assert!(path.is_some(), "Should find a path with no obstacles");
663        let path = path.unwrap();
664        assert!(path.len() >= 2);
665        // First point should be near goal, last near start
666        let first = path.first().unwrap();
667        assert!(
668            (first[0] - 5.0).abs() < 1.0 && (first[1] - 5.0).abs() < 1.0,
669            "Path should start near goal"
670        );
671    }
672
673    #[test]
674    fn test_planning_with_obstacles() {
675        let mut config = default_config();
676        config.max_iter = 300;
677        config.search_until_max_iter = true;
678        let mut planner = LqrRrtStar::new((0.0, 0.0), (6.0, 7.0), default_obstacle_list(), config);
679
680        let path = planner.planning();
681        // With randomness we cannot guarantee a path every time,
682        // but we verify the structure if found.
683        if let Some(path) = path {
684            assert!(path.len() >= 2);
685            let first = path.first().unwrap();
686            assert!(
687                (first[0] - 6.0).abs() < 1.0 && (first[1] - 7.0).abs() < 1.0,
688                "Path should start near goal"
689            );
690        }
691    }
692
693    #[test]
694    fn test_planning_early_termination() {
695        let mut config = default_config();
696        config.max_iter = 500;
697        config.search_until_max_iter = false;
698        let mut planner = LqrRrtStar::new((0.0, 0.0), (3.0, 3.0), vec![], config);
699
700        let path = planner.planning();
701        assert!(path.is_some(), "Should find a path with early termination");
702    }
703
704    #[test]
705    fn test_plan_from_returns_path2d() {
706        let mut config = default_config();
707        config.max_iter = 300;
708        let mut planner = LqrRrtStar::new((0.0, 0.0), (5.0, 5.0), vec![], config);
709
710        let result = planner.plan_from(Point2D::new(0.0, 0.0), Point2D::new(5.0, 5.0));
711        assert!(result.is_ok(), "plan_from should succeed with no obstacles");
712        let path = result.unwrap();
713        assert!(path.points.len() >= 2);
714    }
715
716    #[test]
717    fn test_get_tree_and_obstacles() {
718        let config = default_config();
719        let obstacles = default_obstacle_list();
720        let planner = LqrRrtStar::new((0.0, 0.0), (10.0, 10.0), obstacles.clone(), config);
721
722        assert!(planner.get_tree().is_empty());
723        assert_eq!(planner.get_obstacles().len(), obstacles.len());
724    }
725
726    #[test]
727    fn test_search_best_goal_node_none_when_far() {
728        let config = default_config();
729        let mut planner = LqrRrtStar::new((0.0, 0.0), (100.0, 100.0), vec![], config);
730        planner.node_list = vec![Node::new(0.0, 0.0)];
731
732        assert!(planner.search_best_goal_node().is_none());
733    }
734
735    #[test]
736    fn test_search_best_goal_node_finds_closest() {
737        let mut config = default_config();
738        config.goal_xy_th = 1.0;
739        let mut planner = LqrRrtStar::new((0.0, 0.0), (5.0, 5.0), vec![], config);
740
741        let mut n1 = Node::new(5.0, 5.0);
742        n1.cost = 10.0;
743        let mut n2 = Node::new(4.5, 4.5);
744        n2.cost = 8.0;
745        let n3 = Node::new(0.0, 0.0); // far from goal
746
747        planner.node_list = vec![n3, n1, n2];
748
749        let best = planner.search_best_goal_node();
750        assert!(best.is_some());
751        // Node at index 2 has cost 8.0 < 10.0
752        assert_eq!(best.unwrap(), 2);
753    }
754
755    #[test]
756    fn test_generate_final_course_simple() {
757        let config = default_config();
758        let mut planner = LqrRrtStar::new((0.0, 0.0), (5.0, 5.0), vec![], config);
759
760        let mut start_node = Node::new(0.0, 0.0);
761        start_node.parent = None;
762
763        let mut mid_node = Node::new(2.5, 2.5);
764        mid_node.parent = Some(0);
765        mid_node.path_x = vec![0.5, 1.0, 1.5, 2.0, 2.5];
766        mid_node.path_y = vec![0.5, 1.0, 1.5, 2.0, 2.5];
767
768        let mut goal_node = Node::new(4.8, 4.8);
769        goal_node.parent = Some(1);
770        goal_node.path_x = vec![3.0, 3.5, 4.0, 4.5, 4.8];
771        goal_node.path_y = vec![3.0, 3.5, 4.0, 4.5, 4.8];
772
773        planner.node_list = vec![start_node, mid_node, goal_node];
774
775        let path = planner.generate_final_course(2);
776        // Should start with goal [5.0, 5.0], then reverse of goal_node path,
777        // then reverse of mid_node path, then start [0.0, 0.0]
778        assert_eq!(path.first().unwrap(), &[5.0, 5.0]);
779        assert_eq!(path.last().unwrap(), &[0.0, 0.0]);
780        assert!(path.len() > 2);
781    }
782
783    #[test]
784    fn test_steer_same_point() {
785        let config = default_config();
786        let planner = LqrRrtStar::new((0.0, 0.0), (10.0, 10.0), vec![], config);
787
788        let from = Node::new(5.0, 5.0);
789        let to = Node::new(5.0, 5.0);
790        // LQR from same point to same point should succeed quickly
791        let result = planner.steer(&from, &to);
792        // The LQR planner should handle start == goal
793        if let Some(node) = result {
794            assert!((node.x - 5.0).abs() < 1.0);
795            assert!((node.y - 5.0).abs() < 1.0);
796        }
797    }
798
799    #[test]
800    fn test_seeded_deterministic_no_obstacles() {
801        // Use a fixed sample sequence to verify deterministic behavior
802        let config = LqrRrtStarConfig {
803            max_iter: 50,
804            search_until_max_iter: false,
805            goal_xy_th: 0.5,
806            ..default_config()
807        };
808        let mut planner = LqrRrtStar::new((0.0, 0.0), (3.0, 3.0), vec![], config);
809
810        // Always sample the goal point
811        let path = planner.planning_with_sampler(|p| Node::new(p.end.x, p.end.y));
812
813        assert!(
814            path.is_some(),
815            "Goal-biased sampling with no obstacles should find a path"
816        );
817        let path = path.unwrap();
818        assert!(path.len() >= 2);
819        assert_eq!(path.first().unwrap(), &[3.0, 3.0]);
820        assert_eq!(path.last().unwrap(), &[0.0, 0.0]);
821    }
822}
rust_robotics_planning/lqr_rrt_star.rs

rust_robotics_planning/
lqr_rrt_star.rs
