1#![allow(dead_code, clippy::too_many_arguments)]
2
3use rand::{rngs::StdRng, Rng, SeedableRng};
8
9use rust_robotics_core::{Path2D, PathPlanner, Point2D, RoboticsError, RoboticsResult};
10
11#[derive(Debug, Clone)]
13pub struct Node {
14 pub x: f64,
15 pub y: f64,
16 pub path_x: Vec<f64>,
17 pub path_y: Vec<f64>,
18 pub cost: f64,
19 pub parent: Option<usize>,
20}
21
22impl Node {
23 pub fn new(x: f64, y: f64) -> Self {
24 Node {
25 x,
26 y,
27 path_x: Vec::new(),
28 path_y: Vec::new(),
29 cost: 0.0,
30 parent: None,
31 }
32 }
33}
34
35#[derive(Debug, Clone, PartialEq)]
37pub struct RrtStarTraceNode {
38 pub point: [f64; 2],
39 pub parent: Option<usize>,
40 pub cost: f64,
41}
42
43impl From<&Node> for RrtStarTraceNode {
44 fn from(node: &Node) -> Self {
45 Self {
46 point: [node.x, node.y],
47 parent: node.parent,
48 cost: node.cost,
49 }
50 }
51}
52
53#[derive(Debug, Clone, PartialEq)]
55pub struct RrtStarNodeUpdate {
56 pub index: usize,
57 pub before: Option<RrtStarTraceNode>,
58 pub after: RrtStarTraceNode,
59}
60
61#[derive(Debug, Clone, PartialEq)]
63pub struct RrtStarSearchStep {
64 pub sample: [f64; 2],
65 pub nearest: usize,
66 pub candidate: Option<[f64; 2]>,
67 pub accepted: bool,
68 pub updates: Vec<RrtStarNodeUpdate>,
69 pub path: Vec<[f64; 2]>,
71}
72
73#[derive(Debug, Clone, PartialEq)]
75pub struct RrtStarSearchTrace {
76 pub root: RrtStarTraceNode,
77 pub steps: Vec<RrtStarSearchStep>,
78 pub path: Option<Vec<[f64; 2]>>,
79}
80
81impl RrtStarSearchTrace {
82 pub fn tree_at(&self, completed_iterations: usize) -> Vec<RrtStarTraceNode> {
84 let mut nodes = vec![self.root.clone()];
85 for step in self.steps.iter().take(completed_iterations) {
86 for update in &step.updates {
87 if update.index == nodes.len() {
88 nodes.push(update.after.clone());
89 } else {
90 nodes[update.index] = update.after.clone();
91 }
92 }
93 }
94 nodes
95 }
96}
97
98#[derive(Debug, Clone)]
99pub struct RRTStar {
100 pub start: Node,
101 pub end: Node,
102 pub min_rand: f64,
103 pub max_rand: f64,
104 pub expand_dis: f64,
105 pub path_resolution: f64,
106 pub goal_sample_rate: i32,
107 pub max_iter: i32,
108 pub connect_circle_dist: f64,
109 pub search_until_max_iter: bool,
110 pub robot_radius: f64,
111 pub obstacle_list: Vec<(f64, f64, f64)>, pub node_list: Vec<Node>,
113}
114
115impl RRTStar {
116 pub fn new(
117 start: (f64, f64),
118 goal: (f64, f64),
119 obstacle_list: Vec<(f64, f64, f64)>,
120 rand_area: (f64, f64),
121 expand_dis: f64,
122 path_resolution: f64,
123 goal_sample_rate: i32,
124 max_iter: i32,
125 connect_circle_dist: f64,
126 search_until_max_iter: bool,
127 robot_radius: f64,
128 ) -> Self {
129 RRTStar {
130 start: Node::new(start.0, start.1),
131 end: Node::new(goal.0, goal.1),
132 min_rand: rand_area.0,
133 max_rand: rand_area.1,
134 expand_dis,
135 path_resolution,
136 goal_sample_rate,
137 max_iter,
138 connect_circle_dist,
139 search_until_max_iter,
140 robot_radius,
141 obstacle_list,
142 node_list: Vec::new(),
143 }
144 }
145
146 pub fn planning(&mut self) -> Option<Vec<[f64; 2]>> {
147 self.planning_with_sampler(|planner| planner.get_random_node())
148 }
149
150 pub fn planning_with_trace(&mut self, seed: u64) -> RrtStarSearchTrace {
154 let root = RrtStarTraceNode::from(&self.start);
155 let mut previous = vec![root.clone()];
156 let mut steps = Vec::new();
157 let mut rng = StdRng::seed_from_u64(seed);
158 let path = self.planning_with_observer(
159 |planner| planner.get_random_node_with_rng(&mut rng),
160 |planner, sample, nearest, candidate| {
161 let nodes: Vec<_> = planner
162 .node_list
163 .iter()
164 .map(RrtStarTraceNode::from)
165 .collect();
166 let accepted = nodes.len() > previous.len();
167 let updates = nodes
168 .iter()
169 .enumerate()
170 .filter_map(|(index, node)| {
171 let before = previous.get(index);
172 (before != Some(node)).then(|| RrtStarNodeUpdate {
173 index,
174 before: before.cloned(),
175 after: node.clone(),
176 })
177 })
178 .collect();
179 let path = planner
180 .search_best_goal_node()
181 .map(|i| planner.generate_final_course(i))
182 .unwrap_or_default();
183 steps.push(RrtStarSearchStep {
184 sample,
185 nearest,
186 candidate,
187 accepted,
188 updates,
189 path,
190 });
191 previous = nodes;
192 },
193 );
194 RrtStarSearchTrace { root, steps, path }
195 }
196
197 fn reset_search(&mut self) {
198 self.node_list = vec![self.start.clone()];
199 }
200
201 fn planning_with_sampler<F>(&mut self, mut sample_node: F) -> Option<Vec<[f64; 2]>>
202 where
203 F: FnMut(&RRTStar) -> Node,
204 {
205 self.planning_with_observer(&mut sample_node, |_, _, _, _| {})
206 }
207
208 fn planning_with_observer<F, O>(
209 &mut self,
210 mut sample_node: F,
211 mut observe: O,
212 ) -> Option<Vec<[f64; 2]>>
213 where
214 F: FnMut(&RRTStar) -> Node,
215 O: FnMut(&RRTStar, [f64; 2], usize, Option<[f64; 2]>),
216 {
217 self.reset_search();
218
219 for _i in 0..self.max_iter {
220 let rnd_node = sample_node(self);
221 let nearest_ind = self.get_nearest_node_index(&rnd_node);
222 let mut new_node = self.steer(nearest_ind, &rnd_node);
223 let candidate = new_node.as_ref().map(|n| [n.x, n.y]);
224
225 if let Some(ref mut node) = new_node {
226 let near_node = &self.node_list[nearest_ind];
227 node.cost = near_node.cost + self.calc_distance(near_node, node);
228
229 if self.check_collision_free(node) {
230 let near_inds = self.find_near_nodes(node);
231 let node_with_updated_parent = self.choose_parent(node.clone(), &near_inds);
232
233 if let Some(updated_node) = node_with_updated_parent {
234 let new_index = self.node_list.len();
235 self.node_list.push(updated_node);
236 self.rewire(new_index, &near_inds);
237 } else {
238 self.node_list.push(node.clone());
239 }
240 }
241 }
242
243 observe(self, [rnd_node.x, rnd_node.y], nearest_ind, candidate);
244 if !self.search_until_max_iter && new_node.is_some() {
245 if let Some(last_index) = self.search_best_goal_node() {
246 return Some(self.generate_final_course(last_index));
247 }
248 }
249 }
250
251 if let Some(last_index) = self.search_best_goal_node() {
252 return Some(self.generate_final_course(last_index));
253 }
254
255 None
256 }
257
258 fn get_random_node(&self) -> Node {
259 let mut rng = rand::rng();
260 self.get_random_node_with_rng(&mut rng)
261 }
262
263 fn get_random_node_with_rng(&self, rng: &mut impl Rng) -> Node {
264 if rng.random_range(0..=100) > self.goal_sample_rate {
265 Node::new(
266 rng.random_range(self.min_rand..=self.max_rand),
267 rng.random_range(self.min_rand..=self.max_rand),
268 )
269 } else {
270 Node::new(self.end.x, self.end.y)
271 }
272 }
273
274 fn get_nearest_node_index(&self, rnd_node: &Node) -> usize {
275 let mut min_dist = f64::INFINITY;
276 let mut nearest_ind = 0;
277
278 for (i, node) in self.node_list.iter().enumerate() {
279 let dist = self.calc_distance(node, rnd_node);
280 if dist < min_dist {
281 min_dist = dist;
282 nearest_ind = i;
283 }
284 }
285
286 nearest_ind
287 }
288
289 fn steer(&self, from_ind: usize, to_node: &Node) -> Option<Node> {
290 let from_node = &self.node_list[from_ind];
291 Some(self.steer_from_node(from_node, to_node, self.expand_dis, Some(from_ind)))
292 }
293
294 fn steer_from_node(
295 &self,
296 from_node: &Node,
297 to_node: &Node,
298 extend_length: f64,
299 parent: Option<usize>,
300 ) -> Node {
301 let mut new_node = Node::new(from_node.x, from_node.y);
302 let (dist, theta) = self.calc_distance_and_angle(&new_node, to_node);
303 let extend_length = extend_length.min(dist);
304
305 new_node.path_x = vec![new_node.x];
306 new_node.path_y = vec![new_node.y];
307
308 let n_expand = (extend_length / self.path_resolution).floor() as i32;
309 for _ in 0..n_expand {
310 new_node.x += self.path_resolution * theta.cos();
311 new_node.y += self.path_resolution * theta.sin();
312 new_node.path_x.push(new_node.x);
313 new_node.path_y.push(new_node.y);
314 }
315
316 let (remaining_dist, _) = self.calc_distance_and_angle(&new_node, to_node);
317 if remaining_dist <= self.path_resolution {
318 new_node.path_x.push(to_node.x);
319 new_node.path_y.push(to_node.y);
320 new_node.x = to_node.x;
321 new_node.y = to_node.y;
322 }
323
324 new_node.parent = parent;
325 new_node
326 }
327
328 fn check_collision_free(&self, node: &Node) -> bool {
329 if node.path_x.is_empty() || node.path_y.is_empty() {
330 return true;
331 }
332
333 !self.obstacle_list.iter().any(|&(ox, oy, size)| {
335 crate::rrt::polyline_within(
336 &node.path_x,
337 &node.path_y,
338 [ox, oy],
339 size + self.robot_radius,
340 )
341 })
342 }
343
344 fn find_near_nodes(&self, new_node: &Node) -> Vec<usize> {
345 let nnode = self.node_list.len() + 1;
346 let r = self.connect_circle_dist * ((nnode as f64).ln() / nnode as f64).sqrt();
347 let r = r.min(self.expand_dis);
348
349 self.node_list
350 .iter()
351 .enumerate()
352 .filter_map(|(i, node)| {
353 let dist_sq = (node.x - new_node.x).powi(2) + (node.y - new_node.y).powi(2);
354 if dist_sq <= r.powi(2) {
355 Some(i)
356 } else {
357 None
358 }
359 })
360 .collect()
361 }
362
363 fn choose_parent(&self, new_node: Node, near_inds: &[usize]) -> Option<Node> {
364 if near_inds.is_empty() {
365 return None;
366 }
367
368 let mut costs = Vec::new();
369 for &i in near_inds {
370 let near_node = &self.node_list[i];
371 let t_node = self.steer_from_node(near_node, &new_node, f64::INFINITY, Some(i));
372
373 if self.check_collision_free(&t_node) {
374 costs.push(self.calc_new_cost(near_node, &new_node));
375 } else {
376 costs.push(f64::INFINITY);
377 }
378 }
379
380 let min_cost = costs.iter().fold(f64::INFINITY, |a, &b| a.min(b));
381
382 if min_cost == f64::INFINITY {
383 return None;
384 }
385
386 let min_ind = costs.iter().position(|&x| x == min_cost)?;
387 let parent_ind = near_inds[min_ind];
388
389 let mut result_node = self.steer_from_node(
390 &self.node_list[parent_ind],
391 &new_node,
392 f64::INFINITY,
393 Some(parent_ind),
394 );
395 result_node.cost = min_cost;
396
397 Some(result_node)
398 }
399
400 fn search_best_goal_node(&self) -> Option<usize> {
401 let dist_to_goal_list: Vec<f64> = self
402 .node_list
403 .iter()
404 .map(|n| self.calc_dist_to_goal(n.x, n.y))
405 .collect();
406
407 let goal_inds: Vec<usize> = dist_to_goal_list
408 .iter()
409 .enumerate()
410 .filter_map(|(i, &dist)| {
411 if dist <= self.expand_dis {
412 Some(i)
413 } else {
414 None
415 }
416 })
417 .collect();
418
419 let safe_goal_inds: Vec<usize> = goal_inds
420 .into_iter()
421 .filter(|&goal_ind| {
422 let t_node = self.steer_from_node(
423 &self.node_list[goal_ind],
424 &self.end,
425 f64::INFINITY,
426 Some(goal_ind),
427 );
428 self.check_collision_free(&t_node)
429 })
430 .collect();
431
432 if safe_goal_inds.is_empty() {
433 return None;
434 }
435
436 let safe_goal_costs: Vec<f64> = safe_goal_inds
437 .iter()
438 .map(|&i| {
439 self.node_list[i].cost
440 + self.calc_dist_to_goal(self.node_list[i].x, self.node_list[i].y)
441 })
442 .collect();
443
444 let min_cost = safe_goal_costs.iter().fold(f64::INFINITY, |a, &b| a.min(b));
445
446 safe_goal_inds
447 .into_iter()
448 .zip(safe_goal_costs)
449 .find(|(_, cost)| *cost == min_cost)
450 .map(|(i, _)| i)
451 }
452
453 fn rewire(&mut self, new_node_ind: usize, near_inds: &[usize]) {
454 for &i in near_inds {
455 let near_node = self.node_list[i].clone();
456 let new_node = &self.node_list[new_node_ind];
457
458 let mut edge_node =
459 self.steer_from_node(new_node, &near_node, f64::INFINITY, Some(new_node_ind));
460 edge_node.cost = self.calc_new_cost(new_node, &near_node);
461
462 let no_collision = self.check_collision_free(&edge_node);
463 let improved_cost = near_node.cost > edge_node.cost;
464
465 if no_collision && improved_cost {
466 self.node_list[i] = edge_node;
467 self.propagate_cost_to_leaves(i);
468 }
469 }
470 }
471
472 fn calc_new_cost(&self, from_node: &Node, to_node: &Node) -> f64 {
473 from_node.cost + self.calc_distance(from_node, to_node)
474 }
475
476 fn propagate_cost_to_leaves(&mut self, parent_ind: usize) {
477 let parent_node = self.node_list[parent_ind].clone();
478
479 for i in 0..self.node_list.len() {
480 if let Some(node_parent) = self.node_list[i].parent {
481 if node_parent == parent_ind {
482 self.node_list[i].cost =
483 self.calc_new_cost(&parent_node, &self.node_list[i].clone());
484 self.propagate_cost_to_leaves(i);
485 }
486 }
487 }
488 }
489
490 fn generate_final_course(&self, goal_ind: usize) -> Vec<[f64; 2]> {
491 let mut path = vec![[self.end.x, self.end.y]];
492 let mut node = &self.node_list[goal_ind];
493
494 while let Some(parent_ind) = node.parent {
495 path.push([node.x, node.y]);
496 node = &self.node_list[parent_ind];
497 }
498 path.push([node.x, node.y]);
499
500 path
501 }
502
503 fn calc_dist_to_goal(&self, x: f64, y: f64) -> f64 {
504 let dx = x - self.end.x;
505 let dy = y - self.end.y;
506 (dx * dx + dy * dy).sqrt()
507 }
508
509 fn calc_distance(&self, from_node: &Node, to_node: &Node) -> f64 {
510 let dx = to_node.x - from_node.x;
511 let dy = to_node.y - from_node.y;
512 (dx * dx + dy * dy).sqrt()
513 }
514
515 fn calc_distance_and_angle(&self, from_node: &Node, to_node: &Node) -> (f64, f64) {
516 let dx = to_node.x - from_node.x;
517 let dy = to_node.y - from_node.y;
518 let d = (dx * dx + dy * dy).sqrt();
519 let theta = dy.atan2(dx);
520 (d, theta)
521 }
522
523 pub fn plan_from(&mut self, start: Point2D, goal: Point2D) -> RoboticsResult<Path2D> {
529 self.start = Node::new(start.x, start.y);
530 self.end = Node::new(goal.x, goal.y);
531
532 self.planning()
533 .map(|raw_path| {
534 Path2D::from_points(
535 raw_path
536 .into_iter()
537 .rev()
538 .map(|p| Point2D::new(p[0], p[1]))
539 .collect(),
540 )
541 })
542 .ok_or_else(|| {
543 RoboticsError::PlanningError(
544 "RRT*: Cannot find path within max iterations".to_string(),
545 )
546 })
547 }
548
549 pub fn get_tree(&self) -> &[Node] {
551 &self.node_list
552 }
553
554 pub fn get_obstacles(&self) -> &[(f64, f64, f64)] {
556 &self.obstacle_list
557 }
558}
559
560impl PathPlanner for RRTStar {
561 fn plan(&self, start: Point2D, goal: Point2D) -> Result<Path2D, RoboticsError> {
564 self.clone().plan_from(start, goal)
565 }
566}
567
568#[cfg(test)]
569mod tests {
570 use super::*;
571
572 fn assert_close(actual: f64, expected: f64) {
573 assert!(
574 (actual - expected).abs() < 1.0e-12,
575 "expected {expected}, got {actual}"
576 );
577 }
578
579 fn parse_xy_fixture(csv: &str) -> Vec<[f64; 2]> {
580 csv.lines()
581 .skip(1)
582 .filter(|line| !line.trim().is_empty())
583 .map(|line| {
584 let (x, y) = line
585 .split_once(',')
586 .expect("xy fixture rows must contain a comma");
587 [x.parse().unwrap(), y.parse().unwrap()]
588 })
589 .collect()
590 }
591
592 fn create_pythonrobotics_main_planner() -> RRTStar {
593 RRTStar::new(
594 (0.0, 0.0),
595 (6.0, 10.0),
596 vec![
597 (5.0, 5.0, 1.0),
598 (3.0, 6.0, 2.0),
599 (3.0, 8.0, 2.0),
600 (3.0, 10.0, 2.0),
601 (7.0, 5.0, 2.0),
602 (9.0, 5.0, 2.0),
603 (8.0, 10.0, 1.0),
604 (6.0, 12.0, 1.0),
605 ],
606 (-2.0, 15.0),
607 1.0,
608 1.0,
609 20,
610 300,
611 50.0,
612 false,
613 0.8,
614 )
615 }
616
617 #[test]
618 fn test_rrt_star_config() {
619 let rrt = RRTStar::new(
620 (0.0, 0.0),
621 (6.0, 10.0),
622 vec![(5.0, 5.0, 1.0)],
623 (-2.0, 15.0),
624 2.0,
625 0.5,
626 20,
627 500,
628 50.0,
629 false,
630 0.3,
631 );
632 assert_eq!(rrt.expand_dis, 2.0);
633 assert_eq!(rrt.max_iter, 500);
634 }
635
636 #[test]
637 fn test_rrt_star_upstream_no_obstacle_seeded_reference() {
638 for robot_radius in [0.0, 0.8] {
639 let mut rrt = RRTStar::new(
640 (0.0, 0.0),
641 (6.0, 10.0),
642 vec![],
643 (-2.0, 15.0),
644 30.0,
645 1.0,
646 20,
647 300,
648 50.0,
649 false,
650 robot_radius,
651 );
652 let sample = [10.455_649_682_677_358, 11.942_970_283_541_907];
653 let path = rrt
654 .planning_with_sampler(|_| Node::new(sample[0], sample[1]))
655 .unwrap();
656 assert_eq!(rrt.node_list.len(), 2);
657 assert_eq!(path, vec![[6.0, 10.0], [0.0, 0.0]]);
658 assert_close(rrt.node_list[1].x, sample[0]);
659 assert_close(rrt.node_list[1].y, sample[1]);
660 assert_close(rrt.node_list[1].cost, 15.873_095_144_943_73);
661 assert_eq!(rrt.node_list[1].parent, Some(0));
662 }
663 }
664
665 #[test]
666 fn test_rrt_star_upstream_seeded_main_prefix_matches_pythonrobotics_reference() {
667 let mut rrt = create_pythonrobotics_main_planner();
668 rrt.max_iter = 20;
669 let samples = parse_xy_fixture(include_str!("testdata/rrt_star_main_seed10_samples.csv"));
670 let mut sample_index = 0_usize;
671 let prefix_len = 20_usize;
672
673 let path = rrt.planning_with_sampler(|_| {
674 let sample = samples
675 .get(sample_index)
676 .filter(|_| sample_index < prefix_len)
677 .expect("python reference sample sequence exhausted");
678 sample_index += 1;
679 Node::new(sample[0], sample[1])
680 });
681
682 assert!(path.is_none());
683 assert_eq!(sample_index, prefix_len);
684 assert_eq!(rrt.node_list.len(), 14);
685
686 let expected_nodes = [
687 (
688 1,
689 [-0.227_015_105_864_128, 0.973_891_237_104_79],
690 1.0,
691 Some(0),
692 ),
693 (
694 2,
695 [0.340_848_395_898_016, 1.797_013_976_048_647],
696 2.0,
697 Some(1),
698 ),
699 (
700 5,
701 [2.912_922_340_312_151, 1.655_751_229_702_901],
702 5.0,
703 Some(4),
704 ),
705 (
706 10,
707 [5.856_411_905_724_674, 1.320_164_679_038_256],
708 8.0,
709 Some(9),
710 ),
711 (
712 13,
713 [8.543_112_538_843_18, 0.823_740_039_534_573],
714 11.0,
715 Some(12),
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
727 #[test]
728 fn test_rrt_star_upstream_seeded_main_matches_pythonrobotics_reference() {
729 let mut rrt = create_pythonrobotics_main_planner();
730 let samples = parse_xy_fixture(include_str!("testdata/rrt_star_main_seed10_samples.csv"));
731 let expected_path =
732 parse_xy_fixture(include_str!("testdata/rrt_star_main_seed10_path.csv"));
733 let mut sample_index = 0_usize;
734
735 let path = rrt
736 .planning_with_sampler(|_| {
737 let sample = samples
738 .get(sample_index)
739 .expect("python reference sample sequence exhausted");
740 sample_index += 1;
741 Node::new(sample[0], sample[1])
742 })
743 .expect("python reference run should find a path");
744
745 assert_eq!(sample_index, samples.len());
746 assert_eq!(rrt.node_list.len(), 100);
747 assert_eq!(path.len(), expected_path.len());
748 for (actual, expected) in path.iter().zip(expected_path.iter()) {
749 assert_close(actual[0], expected[0]);
750 assert_close(actual[1], expected[1]);
751 }
752
753 let expected_nodes = [
754 (
755 1,
756 [-0.227_015_105_864_128, 0.973_891_237_104_79],
757 1.0,
758 Some(0),
759 ),
760 (
761 2,
762 [0.340_848_395_898_016, 1.797_013_976_048_647],
763 2.0,
764 Some(1),
765 ),
766 (
767 5,
768 [2.912_922_340_312_151, 1.655_751_229_702_901],
769 5.0,
770 Some(4),
771 ),
772 (
773 10,
774 [5.856_411_905_724_674, 1.320_164_679_038_256],
775 8.0,
776 Some(9),
777 ),
778 (
779 20,
780 [12.105_226_205_468_63, 1.607_428_066_363_632],
781 14.812_039_643_502_144,
782 Some(19),
783 ),
784 (
785 40,
786 [13.266_098_354_827_152, 11.032_918_978_213_733],
787 25.673_392_954_630_07,
788 Some(39),
789 ),
790 (
791 60,
792 [8.777_150_456_317_27, 12.593_447_860_337_104],
793 31.673_392_954_630_07,
794 Some(53),
795 ),
796 (
797 80,
798 [10.550_895_454_349_991, 1.108_429_868_595_935],
799 13.203_336_815_700_968,
800 Some(17),
801 ),
802 (99, [6.0, 10.0], 28.741_122_081_549_424, Some(97)),
803 ];
804
805 for (index, xy, cost, parent) in expected_nodes {
806 let node = &rrt.node_list[index];
807 assert_close(node.x, xy[0]);
808 assert_close(node.y, xy[1]);
809 assert_close(node.cost, cost);
810 assert_eq!(node.parent, parent);
811 }
812 }
813
814 fn trace_fixture() -> RRTStar {
815 RRTStar::new(
816 (1.5, 1.5),
817 (18.5, 18.5),
818 vec![(10.0, 10.0, 2.5)],
819 (0.0, 20.0),
820 1.5,
821 0.25,
822 10,
823 500,
824 15.0,
825 true,
826 0.3,
827 )
828 }
829
830 #[test]
831 fn recorded_search_matches_the_same_unobserved_seeded_run() {
832 let mut recorded = trace_fixture();
833 let trace = recorded.planning_with_trace(7);
834 let mut plain = trace_fixture();
835 let mut rng = StdRng::seed_from_u64(7);
836 let path = plain.planning_with_sampler(|p| p.get_random_node_with_rng(&mut rng));
837 assert_eq!(trace.path, path);
838 assert!(path.is_some());
839 assert_eq!(
840 trace.tree_at(trace.steps.len()),
841 plain
842 .node_list
843 .iter()
844 .map(RrtStarTraceNode::from)
845 .collect::<Vec<_>>()
846 );
847 assert_eq!(
848 recorded.planning_with_trace(7),
849 trace,
850 "a repeated run must reset all recorded state"
851 );
852 }
853
854 #[test]
855 fn every_recorded_prefix_has_real_parents_costs_and_feasible_improving_paths() {
856 let mut planner = trace_fixture();
857 let trace = planner.planning_with_trace(7);
858 let mut tree = vec![trace.root.clone()];
859 let mut best = f64::INFINITY;
860 let mut rewires = 0;
861 let mut improvements = 0;
862 for step in &trace.steps {
863 assert!(step.nearest < tree.len());
864 let before_count = tree.len();
865 for update in &step.updates {
866 assert_eq!(update.before.as_ref(), tree.get(update.index));
867 if let Some(before) = &update.before {
868 if before.parent != update.after.parent {
869 rewires += 1;
870 }
871 }
872 if update.index == tree.len() {
873 tree.push(update.after.clone());
874 } else {
875 tree[update.index] = update.after.clone();
876 }
877 }
878 assert_eq!(tree.len() - before_count, usize::from(step.accepted));
879 for (i, node) in tree.iter().enumerate().skip(1) {
880 let parent = node.parent.expect("non-root node has a parent");
881 assert!(parent < tree.len() && parent != i);
882 let p = &tree[parent];
883 assert!(
884 (node.cost
885 - p.cost
886 - (node.point[0] - p.point[0]).hypot(node.point[1] - p.point[1]))
887 .abs()
888 < 1e-7
889 );
890 assert!(!crate::rrt::polyline_within(
891 &[p.point[0], node.point[0]],
892 &[p.point[1], node.point[1]],
893 [10.0, 10.0],
894 2.8
895 ));
896 let mut cursor = i;
897 for depth in 0..=tree.len() {
898 match tree[cursor].parent {
899 Some(p) => cursor = p,
900 None => break,
901 }
902 assert!(depth < tree.len(), "recorded tree has a parent cycle");
903 }
904 }
905 if !step.path.is_empty() {
906 assert_eq!(step.path.first(), Some(&[18.5, 18.5]));
907 assert_eq!(step.path.last(), Some(&[1.5, 1.5]));
908 let length: f64 = step
909 .path
910 .windows(2)
911 .map(|p| (p[1][0] - p[0][0]).hypot(p[1][1] - p[0][1]))
912 .sum();
913 assert!(length <= best + 1e-7);
914 if length < best - 1e-7 {
915 improvements += 1;
916 }
917 best = length;
918 } else {
919 assert!(!best.is_finite(), "a discovered path disappeared");
920 }
921 }
922 assert!(rewires > 0 && improvements > 1);
923 assert_eq!(trace.tree_at(0), vec![trace.root.clone()]);
924 assert_eq!(trace.steps.last().unwrap().path, trace.path.unwrap());
925 }
926
927 #[test]
928 fn trace_preserves_rejections_and_the_early_stop_condition() {
929 let mut blocked = trace_fixture();
930 blocked.obstacle_list = vec![(10.0, 10.0, 100.0)];
931 let trace = blocked.planning_with_trace(7);
932 assert_eq!(trace.steps.len(), 500);
933 assert!(trace.path.is_none());
934 assert!(trace
935 .steps
936 .iter()
937 .all(|s| !s.accepted && s.updates.is_empty() && s.path.is_empty()));
938 let mut early = trace_fixture();
939 early.search_until_max_iter = false;
940 let trace = early.planning_with_trace(7);
941 assert!(trace.path.is_some() && trace.steps.len() < 500);
942 assert!(trace.steps[..trace.steps.len() - 1]
943 .iter()
944 .all(|s| s.path.is_empty()));
945 }
946}