Skip to main content

rust_robotics_planning/
informed_rrt_star.rs

1#![allow(dead_code, clippy::needless_borrows_for_generic_args)]
2
3//! Informed RRT* path planning algorithm
4//!
5//! An extension of RRT* that uses ellipsoidal sampling to focus the search
6//! once an initial solution is found.
7
8use rand::Rng;
9use std::f64::consts::PI;
10
11use rust_robotics_core::{Path2D, Point2D, RoboticsError, RoboticsResult};
12
13#[derive(Clone, Debug)]
14pub struct Node {
15    pub x: f64,
16    pub y: f64,
17    pub cost: f64,
18    pub parent: Option<usize>,
19}
20
21impl Node {
22    pub fn new(x: f64, y: f64) -> Self {
23        Node {
24            x,
25            y,
26            cost: 0.0,
27            parent: None,
28        }
29    }
30}
31
32pub struct InformedRRTStar {
33    pub start: Node,
34    pub goal: Node,
35    pub min_rand: f64,
36    pub max_rand: f64,
37    pub expand_dis: f64,
38    pub goal_sample_rate: i32,
39    pub max_iter: usize,
40    pub obstacle_list: Vec<(f64, f64, f64)>,
41    pub node_list: Vec<Node>,
42}
43
44impl InformedRRTStar {
45    pub fn new(
46        start: (f64, f64),
47        goal: (f64, f64),
48        obstacle_list: Vec<(f64, f64, f64)>,
49        rand_area: (f64, f64),
50        expand_dis: f64,
51        goal_sample_rate: i32,
52        max_iter: usize,
53    ) -> Self {
54        InformedRRTStar {
55            start: Node::new(start.0, start.1),
56            goal: Node::new(goal.0, goal.1),
57            min_rand: rand_area.0,
58            max_rand: rand_area.1,
59            expand_dis,
60            goal_sample_rate,
61            max_iter,
62            obstacle_list,
63            node_list: Vec::new(),
64        }
65    }
66
67    pub fn planning(&mut self) -> Option<Vec<[f64; 2]>> {
68        self.planning_with_sampler(|planner, c_best, c_min, x_center, rotation_matrix| {
69            planner.informed_sample(c_best, c_min, x_center, rotation_matrix)
70        })
71    }
72
73    fn sampling_frame(&self) -> (f64, [f64; 2], [[f64; 2]; 2]) {
74        let c_min =
75            ((self.start.x - self.goal.x).powi(2) + (self.start.y - self.goal.y).powi(2)).sqrt();
76        let x_center = [
77            (self.start.x + self.goal.x) / 2.0,
78            (self.start.y + self.goal.y) / 2.0,
79        ];
80        let a1 = [
81            (self.goal.x - self.start.x) / c_min,
82            (self.goal.y - self.start.y) / c_min,
83        ];
84        let e_theta = a1[1].atan2(a1[0]);
85        let cos_theta = e_theta.cos();
86        let sin_theta = e_theta.sin();
87        let rotation_matrix = [[cos_theta, -sin_theta], [sin_theta, cos_theta]];
88
89        (c_min, x_center, rotation_matrix)
90    }
91
92    fn reset_search(&mut self) {
93        self.node_list = vec![self.start.clone()];
94    }
95
96    fn planning_with_sampler<F>(&mut self, mut sample_node: F) -> Option<Vec<[f64; 2]>>
97    where
98        F: FnMut(&InformedRRTStar, f64, f64, &[f64; 2], &[[f64; 2]; 2]) -> [f64; 2],
99    {
100        self.reset_search();
101        let mut c_best = f64::INFINITY;
102        let mut path = None;
103
104        let (c_min, x_center, rotation_matrix) = self.sampling_frame();
105
106        for _i in 0..self.max_iter {
107            let rnd = sample_node(self, c_best, c_min, &x_center, &rotation_matrix);
108            let n_ind = self.get_nearest_list_index(&rnd);
109            let nearest_node = &self.node_list[n_ind];
110
111            let theta = (rnd[1] - nearest_node.y).atan2(rnd[0] - nearest_node.x);
112            let new_node = self.get_new_node(theta, n_ind, nearest_node);
113            let d = self.line_cost(nearest_node, &new_node);
114
115            let no_collision = self.check_collision(nearest_node, theta, d);
116
117            if no_collision {
118                let near_inds = self.find_near_nodes(&new_node);
119                let new_node = self.choose_parent(new_node, &near_inds);
120
121                let new_node_index = self.node_list.len();
122                self.node_list.push(new_node);
123                self.rewire(new_node_index, &near_inds);
124
125                if self.is_near_goal(&self.node_list[new_node_index])
126                    && self.check_segment_collision(
127                        self.node_list[new_node_index].x,
128                        self.node_list[new_node_index].y,
129                        self.goal.x,
130                        self.goal.y,
131                    )
132                {
133                    let temp_path = self.get_final_course(new_node_index);
134                    let temp_path_len = self.get_path_len(&temp_path);
135                    if temp_path_len < c_best {
136                        path = Some(temp_path);
137                        c_best = temp_path_len;
138                    }
139                }
140            }
141        }
142
143        path
144    }
145
146    fn choose_parent(&self, mut new_node: Node, near_inds: &[usize]) -> Node {
147        if near_inds.is_empty() {
148            return new_node;
149        }
150
151        let mut d_list = Vec::new();
152        for &i in near_inds {
153            let dx = new_node.x - self.node_list[i].x;
154            let dy = new_node.y - self.node_list[i].y;
155            let d = (dx * dx + dy * dy).sqrt();
156            let theta = dy.atan2(dx);
157            if self.check_collision(&self.node_list[i], theta, d) {
158                d_list.push(self.node_list[i].cost + d);
159            } else {
160                d_list.push(f64::INFINITY);
161            }
162        }
163
164        let min_cost = d_list.iter().fold(f64::INFINITY, |a, &b| a.min(b));
165        if let Some(min_index) = d_list.iter().position(|&x| x == min_cost) {
166            if min_cost != f64::INFINITY {
167                new_node.cost = min_cost;
168                new_node.parent = Some(near_inds[min_index]);
169            }
170        }
171
172        new_node
173    }
174
175    fn find_near_nodes(&self, new_node: &Node) -> Vec<usize> {
176        let n_node = self.node_list.len();
177        let r = 50.0 * ((n_node as f64).ln() / n_node as f64).sqrt();
178        let mut near_inds = Vec::new();
179
180        for (i, node) in self.node_list.iter().enumerate() {
181            let d_sq = (node.x - new_node.x).powi(2) + (node.y - new_node.y).powi(2);
182            if d_sq <= r * r {
183                near_inds.push(i);
184            }
185        }
186
187        near_inds
188    }
189
190    fn informed_sample(
191        &self,
192        c_max: f64,
193        c_min: f64,
194        x_center: &[f64; 2],
195        rotation_matrix: &[[f64; 2]; 2],
196    ) -> [f64; 2] {
197        if c_max < f64::INFINITY {
198            let x_ball = self.sample_unit_ball();
199            self.informed_sample_from_unit_ball(c_max, c_min, x_center, rotation_matrix, x_ball)
200        } else {
201            self.sample_free_space()
202        }
203    }
204
205    fn informed_sample_from_unit_ball(
206        &self,
207        c_max: f64,
208        c_min: f64,
209        x_center: &[f64; 2],
210        rotation_matrix: &[[f64; 2]; 2],
211        x_ball: [f64; 2],
212    ) -> [f64; 2] {
213        let r = [c_max / 2.0, (c_max * c_max - c_min * c_min).sqrt() / 2.0];
214        let scaled = [r[0] * x_ball[0], r[1] * x_ball[1]];
215        let rotated = [
216            rotation_matrix[0][0] * scaled[0] + rotation_matrix[0][1] * scaled[1],
217            rotation_matrix[1][0] * scaled[0] + rotation_matrix[1][1] * scaled[1],
218        ];
219
220        [rotated[0] + x_center[0], rotated[1] + x_center[1]]
221    }
222
223    fn sample_unit_ball(&self) -> [f64; 2] {
224        let mut rng = rand::rng();
225        let a: f64 = rng.random();
226        let b: f64 = rng.random();
227
228        Self::sample_unit_ball_from_uniforms(a, b)
229    }
230
231    fn sample_unit_ball_from_uniforms(a: f64, b: f64) -> [f64; 2] {
232        let (a, b) = if b < a { (b, a) } else { (a, b) };
233        let sample = (b * (2.0 * PI * a / b).cos(), b * (2.0 * PI * a / b).sin());
234        [sample.0, sample.1]
235    }
236
237    fn sample_free_space(&self) -> [f64; 2] {
238        let mut rng = rand::rng();
239        if rng.random_range(0..=100) > self.goal_sample_rate {
240            [
241                rng.random_range(self.min_rand..=self.max_rand),
242                rng.random_range(self.min_rand..=self.max_rand),
243            ]
244        } else {
245            [self.goal.x, self.goal.y]
246        }
247    }
248
249    fn get_path_len(&self, path: &[[f64; 2]]) -> f64 {
250        let mut path_len = 0.0;
251        for i in 1..path.len() {
252            let dx = path[i][0] - path[i - 1][0];
253            let dy = path[i][1] - path[i - 1][1];
254            path_len += (dx * dx + dy * dy).sqrt();
255        }
256        path_len
257    }
258
259    fn line_cost(&self, node1: &Node, node2: &Node) -> f64 {
260        ((node1.x - node2.x).powi(2) + (node1.y - node2.y).powi(2)).sqrt()
261    }
262
263    fn get_nearest_list_index(&self, rnd: &[f64; 2]) -> usize {
264        let mut min_dist = f64::INFINITY;
265        let mut min_index = 0;
266
267        for (i, node) in self.node_list.iter().enumerate() {
268            let dist = (node.x - rnd[0]).powi(2) + (node.y - rnd[1]).powi(2);
269            if dist < min_dist {
270                min_dist = dist;
271                min_index = i;
272            }
273        }
274
275        min_index
276    }
277
278    fn get_new_node(&self, theta: f64, n_ind: usize, nearest_node: &Node) -> Node {
279        let mut new_node = nearest_node.clone();
280        new_node.x += self.expand_dis * theta.cos();
281        new_node.y += self.expand_dis * theta.sin();
282        new_node.cost += self.expand_dis;
283        new_node.parent = Some(n_ind);
284        new_node
285    }
286
287    fn is_near_goal(&self, node: &Node) -> bool {
288        let d = self.line_cost(node, &self.goal);
289        d < self.expand_dis
290    }
291
292    fn rewire(&mut self, new_node_index: usize, near_inds: &[usize]) {
293        for &i in near_inds {
294            let near_node = &self.node_list[i];
295            let new_node = &self.node_list[new_node_index];
296
297            let d =
298                ((near_node.x - new_node.x).powi(2) + (near_node.y - new_node.y).powi(2)).sqrt();
299            let s_cost = new_node.cost + d;
300
301            if near_node.cost > s_cost {
302                let theta = (new_node.y - near_node.y).atan2(new_node.x - near_node.x);
303                if self.check_collision(near_node, theta, d) {
304                    self.node_list[i].parent = Some(new_node_index);
305                    self.node_list[i].cost = s_cost;
306                }
307            }
308        }
309    }
310
311    fn distance_squared_point_to_segment(&self, v: [f64; 2], w: [f64; 2], p: [f64; 2]) -> f64 {
312        if v[0] == w[0] && v[1] == w[1] {
313            return (p[0] - v[0]).powi(2) + (p[1] - v[1]).powi(2);
314        }
315
316        let l2 = (w[0] - v[0]).powi(2) + (w[1] - v[1]).powi(2);
317        let t =
318            (((p[0] - v[0]) * (w[0] - v[0]) + (p[1] - v[1]) * (w[1] - v[1])) / l2).clamp(0.0, 1.0);
319        let projection = [v[0] + t * (w[0] - v[0]), v[1] + t * (w[1] - v[1])];
320        (p[0] - projection[0]).powi(2) + (p[1] - projection[1]).powi(2)
321    }
322
323    fn check_segment_collision(&self, x1: f64, y1: f64, x2: f64, y2: f64) -> bool {
324        for &(ox, oy, size) in &self.obstacle_list {
325            let dd = self.distance_squared_point_to_segment([x1, y1], [x2, y2], [ox, oy]);
326            if dd <= size * size {
327                return false;
328            }
329        }
330        true
331    }
332
333    fn check_collision(&self, near_node: &Node, theta: f64, d: f64) -> bool {
334        let end_x = near_node.x + theta.cos() * d;
335        let end_y = near_node.y + theta.sin() * d;
336        self.check_segment_collision(near_node.x, near_node.y, end_x, end_y)
337    }
338
339    fn get_final_course(&self, last_index: usize) -> Vec<[f64; 2]> {
340        let mut path = vec![[self.goal.x, self.goal.y]];
341        let mut current_index = last_index;
342
343        while let Some(parent_index) = self.node_list[current_index].parent {
344            let node = &self.node_list[current_index];
345            path.push([node.x, node.y]);
346            current_index = parent_index;
347        }
348
349        path.push([self.start.x, self.start.y]);
350        path
351    }
352
353    /// Plan a path from the given start to goal, returning a [`Path2D`].
354    ///
355    /// This is a convenience wrapper around [`planning()`](Self::planning) that accepts
356    /// [`Point2D`], sets the start/goal, runs the planner, and returns [`Path2D`].
357    /// Requires `&mut self` because the underlying algorithm mutates internal state.
358    pub fn plan_from(&mut self, start: Point2D, goal: Point2D) -> RoboticsResult<Path2D> {
359        self.start = Node::new(start.x, start.y);
360        self.goal = Node::new(goal.x, goal.y);
361
362        self.planning()
363            .map(|raw_path| {
364                Path2D::from_points(
365                    raw_path
366                        .into_iter()
367                        .rev()
368                        .map(|p| Point2D::new(p[0], p[1]))
369                        .collect(),
370                )
371            })
372            .ok_or_else(|| {
373                RoboticsError::PlanningError(
374                    "InformedRRT*: Cannot find path within max iterations".to_string(),
375                )
376            })
377    }
378
379    /// Get the tree nodes for external inspection
380    pub fn get_tree(&self) -> &[Node] {
381        &self.node_list
382    }
383
384    /// Get the obstacle list
385    pub fn get_obstacles(&self) -> &[(f64, f64, f64)] {
386        &self.obstacle_list
387    }
388}
389
390#[cfg(test)]
391mod tests {
392    use super::*;
393
394    fn assert_close(actual: f64, expected: f64) {
395        assert!(
396            (actual - expected).abs() < 1.0e-12,
397            "expected {expected}, got {actual}"
398        );
399    }
400
401    fn parse_xy_fixture(csv: &str) -> Vec<[f64; 2]> {
402        csv.lines()
403            .skip(1)
404            .filter(|line| !line.trim().is_empty())
405            .map(|line| {
406                let (x, y) = line
407                    .split_once(',')
408                    .expect("xy fixture rows must contain a comma");
409                [x.parse().unwrap(), y.parse().unwrap()]
410            })
411            .collect()
412    }
413
414    fn create_pythonrobotics_main_planner() -> InformedRRTStar {
415        InformedRRTStar::new(
416            (0.0, 0.0),
417            (5.0, 10.0),
418            vec![
419                (5.0, 5.0, 0.5),
420                (9.0, 6.0, 1.0),
421                (7.0, 5.0, 1.0),
422                (1.0, 5.0, 1.0),
423                (3.0, 6.0, 1.0),
424                (7.0, 9.0, 1.0),
425            ],
426            (-2.0, 15.0),
427            0.5,
428            10,
429            200,
430        )
431    }
432
433    #[test]
434    fn test_informed_rrt_star_config() {
435        let rrt = InformedRRTStar::new(
436            (0.0, 0.0),
437            (5.0, 10.0),
438            vec![(5.0, 5.0, 0.5)],
439            (-2.0, 15.0),
440            0.5,
441            10,
442            300,
443        );
444        assert_eq!(rrt.expand_dis, 0.5);
445        assert_eq!(rrt.max_iter, 300);
446    }
447
448    #[test]
449    fn test_informed_rrt_star_sampling_frame_matches_upstream_reference() {
450        let rrt = create_pythonrobotics_main_planner();
451        let (c_min, x_center, rotation_matrix) = rrt.sampling_frame();
452
453        assert_close(c_min, 11.180_339_887_498_949);
454        assert_close(x_center[0], 2.5);
455        assert_close(x_center[1], 5.0);
456        assert_close(rotation_matrix[0][0], 0.447_213_595_499_958);
457        assert_close(rotation_matrix[0][1], -0.894_427_190_999_916);
458        assert_close(rotation_matrix[1][0], 0.894_427_190_999_916);
459        assert_close(rotation_matrix[1][1], 0.447_213_595_499_958);
460    }
461
462    #[test]
463    fn test_informed_rrt_star_sample_unit_ball_matches_upstream_reference() {
464        let expected = [
465            ((0.2, 0.8), [0.0, 0.8]),
466            ((0.1, 0.4), [0.0, 0.4]),
467            ((0.33, 0.9), [-0.602_217_545_722_972, 0.668_830_342_929_655]),
468        ];
469
470        for ((a, b), xy) in expected {
471            let sample = InformedRRTStar::sample_unit_ball_from_uniforms(a, b);
472            assert_close(sample[0], xy[0]);
473            assert_close(sample[1], xy[1]);
474        }
475    }
476
477    #[test]
478    fn test_informed_rrt_star_finite_ellipse_sampling_matches_upstream_reference() {
479        let rrt = create_pythonrobotics_main_planner();
480        let (c_min, x_center, rotation_matrix) = rrt.sampling_frame();
481        let unit_ball_samples = [
482            [0.0, 0.8],
483            [0.0, 0.4],
484            [-0.602_217_545_722_972, 0.668_830_342_929_655],
485        ];
486        let expected = [
487            (
488                12.0,
489                [
490                    [0.940_512_904_830_566, 5.779_743_547_584_717],
491                    [1.720_256_452_415_283, 5.389_871_773_792_358],
492                    [-0.419_709_604_196_265, 2.420_056_693_659_169],
493                ],
494            ),
495            (
496                14.0,
497                [
498                    [-0.514_630_989_026_684, 6.507_315_494_513_342],
499                    [0.992_684_505_486_658, 5.753_657_747_256_671],
500                    [-1.905_584_965_017_868, 2.489_694_689_330_144],
501                ],
502            ),
503            (
504                20.0,
505                [
506                    [-3.432_958_789_676_533, 7.966_479_394_838_267],
507                    [-0.466_479_394_838_267, 6.483_239_697_419_133],
508                    [-5.153_377_316_317_89, 2.093_691_810_760_673],
509                ],
510            ),
511        ];
512
513        for (c_best, xy_samples) in expected {
514            for (x_ball, xy) in unit_ball_samples.iter().zip(xy_samples.iter()) {
515                let sample = rrt.informed_sample_from_unit_ball(
516                    c_best,
517                    c_min,
518                    &x_center,
519                    &rotation_matrix,
520                    *x_ball,
521                );
522                assert_close(sample[0], xy[0]);
523                assert_close(sample[1], xy[1]);
524            }
525        }
526    }
527
528    #[test]
529    fn test_informed_rrt_star_plan_from_returns_start_to_goal_path() {
530        let mut rrt = InformedRRTStar::new(
531            (0.0, 0.0),
532            (5.0, 10.0),
533            vec![],
534            (-2.0, 15.0),
535            5.0_f64.hypot(10.0),
536            100,
537            1,
538        );
539
540        let path = rrt
541            .plan_from(Point2D::new(0.0, 0.0), Point2D::new(5.0, 10.0))
542            .unwrap();
543
544        assert_eq!(path.points.first().unwrap().x, 0.0);
545        assert_eq!(path.points.first().unwrap().y, 0.0);
546        assert_eq!(path.points.last().unwrap().x, 5.0);
547        assert_eq!(path.points.last().unwrap().y, 10.0);
548    }
549
550    #[test]
551    fn test_informed_rrt_star_upstream_seeded_main_prefix_matches_pythonrobotics_reference() {
552        let mut rrt = create_pythonrobotics_main_planner();
553        rrt.max_iter = 20;
554        let samples = [
555            [10.455_649_682_677_358, 11.942_970_283_541_907],
556            [12.537_351_811_401_535, 13.840_339_744_778_298],
557            [7.622_138_868_390_643, 0.748_693_006_799_259],
558            [12.860_963_734_685_752, 2.438_529_994_751_022],
559            [0.963_840_532_303_441, 7.404_758_454_678_607],
560            [10.548_525_179_081_42, 10.458_784_524_738_785],
561            [14.636_879_924_696_97, 5.006_029_679_968_117],
562            [0.831_739_168_751_872, 1.497_141_169_178_794],
563            [5.0, 10.0],
564            [12.230_194_783_259_385, 3.474_659_848_212_157],
565            [3.771_802_130_764_652, 14.447_201_800_957_814],
566            [10.657_010_682_397_589, -1.941_271_617_163_375],
567            [12.803_038_468_843_097, 11.104_183_754_785_783],
568            [2.871_774_581_756_395, 9.093_185_216_538_995],
569            [13.054_119_941_473_152, 7.827_462_465_227_494],
570            [4.375_040_433_962_334, 14.322_895_906_129_173],
571            [10.059_569_437_230_238, 12.022_177_097_262_272],
572            [5.0, 10.0],
573            [0.036_638_892_026_463, 6.486_823_094_475_769],
574            [11.632_287_039_256_342, 7.436_450_081_952_417],
575        ];
576        let mut sample_index = 0_usize;
577
578        let path = rrt.planning_with_sampler(|_, _, _, _, _| {
579            let sample = *samples
580                .get(sample_index)
581                .expect("python reference sample sequence exhausted");
582            sample_index += 1;
583            sample
584        });
585
586        assert!(path.is_none());
587        assert_eq!(sample_index, samples.len());
588        assert_eq!(rrt.node_list.len(), 20);
589
590        let expected_nodes = [
591            (
592                1,
593                [0.329_351_320_180_549, 0.376_201_685_130_901],
594                0.5,
595                Some(0),
596            ),
597            (
598                2,
599                [0.665_203_546_734_372, 0.746_611_298_827_467],
600                0.999_962_094_343_993,
601                Some(0),
602            ),
603            (
604                5,
605                [1.607_494_092_207_533, 1.315_569_836_427_121],
606                2.077_200_339_639_631,
607                Some(0),
608            ),
609            (
610                10,
611                [3.085_807_934_983_019, 2.339_074_098_076_378],
612                3.872_141_300_094_301,
613                Some(0),
614            ),
615            (
616                15,
617                [3.900_997_195_468_872, 3.858_949_080_306_712],
618                5.487_191_187_069_758,
619                Some(0),
620            ),
621        ];
622
623        for (index, xy, cost, parent) in expected_nodes {
624            let node = &rrt.node_list[index];
625            assert_close(node.x, xy[0]);
626            assert_close(node.y, xy[1]);
627            assert_close(node.cost, cost);
628            assert_eq!(node.parent, parent);
629        }
630    }
631
632    #[test]
633    fn test_informed_rrt_star_upstream_seeded_main_matches_pythonrobotics_reference() {
634        let mut rrt = create_pythonrobotics_main_planner();
635        let samples = parse_xy_fixture(include_str!(
636            "testdata/informed_rrt_star_main_seed12345_samples.csv"
637        ));
638        let expected_path = parse_xy_fixture(include_str!(
639            "testdata/informed_rrt_star_main_seed12345_path.csv"
640        ));
641        let mut sample_index = 0_usize;
642
643        let path = rrt
644            .planning_with_sampler(|_, _, _, _, _| {
645                let sample = *samples
646                    .get(sample_index)
647                    .expect("python reference sample sequence exhausted");
648                sample_index += 1;
649                sample
650            })
651            .expect("python reference run should find a path");
652
653        assert_eq!(sample_index, samples.len());
654        assert_eq!(rrt.node_list.len(), 158);
655        assert_eq!(path.len(), expected_path.len());
656        for (actual, expected) in path.iter().zip(expected_path.iter()) {
657            assert_close(actual[0], expected[0]);
658            assert_close(actual[1], expected[1]);
659        }
660
661        let expected_nodes = [
662            (
663                1,
664                [0.329_351_320_180_549, 0.376_201_685_130_901],
665                0.5,
666                Some(0),
667            ),
668            (
669                2,
670                [0.665_203_546_734_372, 0.746_611_298_827_467],
671                0.999_962_094_343_993,
672                Some(0),
673            ),
674            (
675                5,
676                [1.607_494_092_207_533, 1.315_569_836_427_121],
677                2.077_200_339_639_631,
678                Some(0),
679            ),
680            (
681                10,
682                [3.085_807_934_983_019, 2.339_074_098_076_378],
683                3.872_141_300_094_301,
684                Some(0),
685            ),
686            (
687                20,
688                [2.839_225_131_469_749, 4.429_224_288_562_779],
689                5.261_105_125_880_291,
690                Some(0),
691            ),
692            (
693                40,
694                [7.308_140_070_034_553, 7.411_628_497_256_421],
695                10.487_807_493_996_291,
696                Some(18),
697            ),
698            (
699                80,
700                [2.374_311_082_825_791, 4.245_227_203_331_836],
701                4.864_083_379_829_957,
702                Some(0),
703            ),
704            (
705                120,
706                [1.995_074_025_995_299, 5.624_254_528_604_578],
707                6.049_311_878_312_089,
708                Some(77),
709            ),
710            (
711                157,
712                [1.654_832_682_319_505, 3.738_064_093_591_951],
713                4.087_981_699_356_565,
714                Some(0),
715            ),
716        ];
717
718        for (index, xy, cost, parent) in expected_nodes {
719            let node = &rrt.node_list[index];
720            assert_close(node.x, xy[0]);
721            assert_close(node.y, xy[1]);
722            assert_close(node.cost, cost);
723            assert_eq!(node.parent, parent);
724        }
725    }
726}