1#![allow(clippy::too_many_arguments)]
2
3use rand::Rng;
9
10use rust_robotics_core::{Path2D, PathPlanner, Point2D, RoboticsError};
11
12#[derive(Debug, Clone)]
14pub struct RRTNode {
15 pub x: f64,
16 pub y: f64,
17 pub path_x: Vec<f64>,
18 pub path_y: Vec<f64>,
19 pub parent: Option<usize>,
20}
21
22impl RRTNode {
23 pub fn new(x: f64, y: f64) -> Self {
24 RRTNode {
25 x,
26 y,
27 path_x: Vec::new(),
28 path_y: Vec::new(),
29 parent: None,
30 }
31 }
32 pub fn to_point(&self) -> Point2D {
33 Point2D::new(self.x, self.y)
34 }
35}
36
37#[derive(Debug, Clone)]
39pub struct AreaBounds {
40 pub xmin: f64,
41 pub xmax: f64,
42 pub ymin: f64,
43 pub ymax: f64,
44}
45
46impl AreaBounds {
47 pub fn new(xmin: f64, xmax: f64, ymin: f64, ymax: f64) -> Self {
48 AreaBounds {
49 xmin,
50 xmax,
51 ymin,
52 ymax,
53 }
54 }
55 pub fn from_array(area: [f64; 4]) -> Self {
56 AreaBounds {
57 xmin: area[0],
58 xmax: area[1],
59 ymin: area[2],
60 ymax: area[3],
61 }
62 }
63}
64
65#[derive(Debug, Clone)]
67pub struct CircleObstacle {
68 pub x: f64,
69 pub y: f64,
70 pub radius: f64,
71}
72
73impl CircleObstacle {
74 pub fn new(x: f64, y: f64, radius: f64) -> Self {
75 Self { x, y, radius }
76 }
77}
78
79#[derive(Debug, Clone)]
81pub struct RRTConfig {
82 pub expand_dis: f64,
83 pub path_resolution: f64,
84 pub goal_sample_rate: i32,
85 pub max_iter: usize,
86 pub robot_radius: f64,
87}
88
89impl Default for RRTConfig {
90 fn default() -> Self {
91 Self {
92 expand_dis: 3.0,
93 path_resolution: 0.5,
94 goal_sample_rate: 5,
95 max_iter: 500,
96 robot_radius: 0.8,
97 }
98 }
99}
100
101pub struct RRTPlanner {
103 config: RRTConfig,
104 obstacles: Vec<CircleObstacle>,
105 play_area: Option<AreaBounds>,
106 rand_area: AreaBounds,
107 node_list: Vec<RRTNode>,
108 _start: RRTNode,
109 goal: RRTNode,
110}
111
112impl RRTPlanner {
113 pub fn new(
114 obstacles: Vec<CircleObstacle>,
115 rand_area: AreaBounds,
116 play_area: Option<AreaBounds>,
117 config: RRTConfig,
118 ) -> Self {
119 RRTPlanner {
120 config,
121 obstacles,
122 play_area,
123 rand_area,
124 node_list: Vec::new(),
125 _start: RRTNode::new(0.0, 0.0),
126 goal: RRTNode::new(0.0, 0.0),
127 }
128 }
129
130 pub fn from_obstacles(
131 obstacle_list: Vec<(f64, f64, f64)>,
132 rand_area: [f64; 2],
133 expand_dis: f64,
134 path_resolution: f64,
135 goal_sample_rate: i32,
136 max_iter: usize,
137 play_area: Option<[f64; 4]>,
138 robot_radius: f64,
139 ) -> Self {
140 let obstacles = obstacle_list
141 .into_iter()
142 .map(|(x, y, r)| CircleObstacle::new(x, y, r))
143 .collect();
144 let config = RRTConfig {
145 expand_dis,
146 path_resolution,
147 goal_sample_rate,
148 max_iter,
149 robot_radius,
150 };
151 let rand_bounds = AreaBounds::new(rand_area[0], rand_area[1], rand_area[0], rand_area[1]);
152 let play_bounds = play_area.map(AreaBounds::from_array);
153 Self::new(obstacles, rand_bounds, play_bounds, config)
154 }
155
156 pub fn planning(&mut self, start: [f64; 2], goal: [f64; 2]) -> Option<Vec<[f64; 2]>> {
157 let start_pt = Point2D::new(start[0], start[1]);
158 let goal_pt = Point2D::new(goal[0], goal[1]);
159 match self.plan_with_sampler(start_pt, goal_pt, |planner| planner.get_random_node()) {
162 Ok(path) => Some(path.points.iter().map(|p| [p.x, p.y]).collect()),
163 Err(_) => None,
164 }
165 }
166
167 pub fn get_tree(&self) -> &[RRTNode] {
168 &self.node_list
169 }
170 pub fn get_obstacles(&self) -> &[CircleObstacle] {
171 &self.obstacles
172 }
173
174 fn reset_search(&mut self, start: Point2D, goal: Point2D) {
175 self.node_list = vec![RRTNode::new(start.x, start.y)];
176 self._start = RRTNode::new(start.x, start.y);
177 self.goal = RRTNode::new(goal.x, goal.y);
178 }
179
180 fn steer(&self, from_node: &RRTNode, to_node: &RRTNode, extend_length: f64) -> RRTNode {
181 let mut new_node = RRTNode::new(from_node.x, from_node.y);
182 let (d, theta) = self.calc_distance_and_angle(from_node, to_node);
183 new_node.path_x = vec![new_node.x];
184 new_node.path_y = vec![new_node.y];
185 let extend_length = extend_length.min(d);
186 let n_expand = (extend_length / self.config.path_resolution).floor() as usize;
187 for _ in 0..n_expand {
188 new_node.x += self.config.path_resolution * theta.cos();
189 new_node.y += self.config.path_resolution * theta.sin();
190 new_node.path_x.push(new_node.x);
191 new_node.path_y.push(new_node.y);
192 }
193 let (d, _) = self.calc_distance_and_angle(&new_node, to_node);
194 if d <= self.config.path_resolution {
195 new_node.path_x.push(to_node.x);
196 new_node.path_y.push(to_node.y);
197 new_node.x = to_node.x;
198 new_node.y = to_node.y;
199 }
200 new_node
201 }
202
203 fn generate_final_course(&self, goal_ind: usize) -> Path2D {
204 let mut points = vec![self.goal.to_point()];
205 let mut node_index = Some(goal_ind);
206 while let Some(index) = node_index {
207 let node = &self.node_list[index];
208 points.push(node.to_point());
209 node_index = node.parent;
210 }
211 points.reverse();
212 Path2D::from_points(points)
213 }
214
215 fn calc_dist_to_goal(&self, x: f64, y: f64) -> f64 {
216 let dx = x - self.goal.x;
217 let dy = y - self.goal.y;
218 (dx * dx + dy * dy).sqrt()
219 }
220
221 fn get_random_node(&self) -> RRTNode {
222 let mut rng = rand::rng();
223 if rng.random_range(0..=100) > self.config.goal_sample_rate {
224 RRTNode::new(
225 rng.random_range(self.rand_area.xmin..=self.rand_area.xmax),
226 rng.random_range(self.rand_area.ymin..=self.rand_area.ymax),
227 )
228 } else {
229 RRTNode::new(self.goal.x, self.goal.y)
230 }
231 }
232
233 fn get_nearest_node_index(&self, rnd_node: &RRTNode) -> usize {
234 let mut min_dist = f64::INFINITY;
235 let mut min_ind = 0;
236 for (i, node) in self.node_list.iter().enumerate() {
237 let dist = (node.x - rnd_node.x).powi(2) + (node.y - rnd_node.y).powi(2);
238 if dist < min_dist {
239 min_dist = dist;
240 min_ind = i;
241 }
242 }
243 min_ind
244 }
245
246 fn check_if_outside_play_area(&self, node: &RRTNode) -> bool {
247 if let Some(ref play_area) = self.play_area {
248 if node.x < play_area.xmin
249 || node.x > play_area.xmax
250 || node.y < play_area.ymin
251 || node.y > play_area.ymax
252 {
253 return false;
254 }
255 }
256 true
257 }
258
259 fn check_collision(&self, node: &RRTNode) -> bool {
260 for obs in &self.obstacles {
261 let reach = obs.radius + self.config.robot_radius;
262 if polyline_within(&node.path_x, &node.path_y, [obs.x, obs.y], reach) {
263 return false;
264 }
265 }
266 true
267 }
268
269 fn calc_distance_and_angle(&self, from_node: &RRTNode, to_node: &RRTNode) -> (f64, f64) {
270 let dx = to_node.x - from_node.x;
271 let dy = to_node.y - from_node.y;
272 ((dx * dx + dy * dy).sqrt(), dy.atan2(dx))
273 }
274
275 pub(crate) fn plan_with_sampler<F>(
276 &mut self,
277 start: Point2D,
278 goal: Point2D,
279 mut sample_node: F,
280 ) -> Result<Path2D, RoboticsError>
281 where
282 F: FnMut(&RRTPlanner) -> RRTNode,
283 {
284 self.reset_search(start, goal);
285 for _ in 0..self.config.max_iter {
286 let rnd_node = sample_node(self);
287 let nearest_ind = self.get_nearest_node_index(&rnd_node);
288 let nearest_node = self.node_list[nearest_ind].clone();
289 let new_node = self.steer(&nearest_node, &rnd_node, self.config.expand_dis);
290 if self.check_if_outside_play_area(&new_node) && self.check_collision(&new_node) {
291 let mut new_node = new_node;
292 new_node.parent = Some(nearest_ind);
293 self.node_list.push(new_node);
294 let last = self.node_list.last().unwrap();
295 if self.calc_dist_to_goal(last.x, last.y) <= self.config.expand_dis {
296 let final_node = self.steer(last, &self.goal.clone(), self.config.expand_dis);
297 if self.check_collision(&final_node) {
298 return Ok(self.generate_final_course(self.node_list.len() - 1));
299 }
300 }
301 }
302 }
303 Err(RoboticsError::PlanningError(
304 "RRT: Cannot find path within max iterations".to_string(),
305 ))
306 }
307}
308
309pub(crate) fn polyline_within(xs: &[f64], ys: &[f64], p: [f64; 2], reach: f64) -> bool {
313 let dist2 = |a: [f64; 2], b: [f64; 2]| {
314 let (dx, dy) = (b[0] - a[0], b[1] - a[1]);
315 let l2 = dx * dx + dy * dy;
316 let t = if l2 > 0.0 {
317 (((p[0] - a[0]) * dx + (p[1] - a[1]) * dy) / l2).clamp(0.0, 1.0)
318 } else {
319 0.0
320 };
321 let (cx, cy) = (a[0] + t * dx - p[0], a[1] + t * dy - p[1]);
322 cx * cx + cy * cy
323 };
324 let reach2 = reach * reach;
325 match xs.len().min(ys.len()) {
326 0 => false,
327 1 => dist2([xs[0], ys[0]], [xs[0], ys[0]]) <= reach2,
328 n => (0..n - 1).any(|i| dist2([xs[i], ys[i]], [xs[i + 1], ys[i + 1]]) <= reach2),
329 }
330}
331
332impl PathPlanner for RRTPlanner {
333 fn plan(&self, start: Point2D, goal: Point2D) -> Result<Path2D, RoboticsError> {
334 let mut planner = RRTPlanner {
335 config: self.config.clone(),
336 obstacles: self.obstacles.clone(),
337 play_area: self.play_area.clone(),
338 rand_area: self.rand_area.clone(),
339 node_list: vec![RRTNode::new(start.x, start.y)],
340 _start: RRTNode::new(start.x, start.y),
341 goal: RRTNode::new(goal.x, goal.y),
342 };
343 planner.plan_with_sampler(start, goal, |planner| planner.get_random_node())
344 }
345}
346
347#[cfg(test)]
348mod tests {
349 use super::*;
350
351 #[test]
352 fn a_thin_obstacle_between_two_path_samples_is_a_collision() {
353 let xs = [0.0, 0.5];
355 let ys = [0.0, 0.0];
356 assert!(polyline_within(&xs, &ys, [0.25, 0.0], 0.1));
357 assert!(!polyline_within(&xs, &ys, [0.25, 0.3], 0.1));
358 assert!(polyline_within(&[1.0], &[1.0], [1.05, 1.0], 0.1));
359 assert!(!polyline_within(&[], &[], [0.0, 0.0], 1.0));
360 }
361
362 fn assert_close(actual: f64, expected: f64) {
363 assert!(
364 (actual - expected).abs() < 1.0e-12,
365 "expected {expected}, got {actual}"
366 );
367 }
368
369 fn assert_point_close(actual: &Point2D, expected: [f64; 2]) {
370 assert_close(actual.x, expected[0]);
371 assert_close(actual.y, expected[1]);
372 }
373
374 fn parse_xy_fixture(csv: &str) -> Vec<[f64; 2]> {
375 csv.lines()
376 .skip(1)
377 .filter(|line| !line.trim().is_empty())
378 .map(|line| {
379 let (x, y) = line
380 .split_once(',')
381 .expect("xy fixture rows must contain a comma");
382 [x.parse().unwrap(), y.parse().unwrap()]
383 })
384 .collect()
385 }
386
387 fn create_test_planner() -> RRTPlanner {
388 let obstacles = vec![
389 CircleObstacle::new(5.0, 5.0, 1.0),
390 CircleObstacle::new(3.0, 6.0, 2.0),
391 CircleObstacle::new(7.0, 5.0, 2.0),
392 ];
393 let rand_area = AreaBounds::new(-2.0, 15.0, -2.0, 15.0);
394 let config = RRTConfig {
395 max_iter: 1000,
396 ..Default::default()
397 };
398 RRTPlanner::new(obstacles, rand_area, None, config)
399 }
400
401 fn create_pythonrobotics_main_planner() -> RRTPlanner {
402 let obstacles = vec![
403 CircleObstacle::new(5.0, 5.0, 1.0),
404 CircleObstacle::new(3.0, 6.0, 2.0),
405 CircleObstacle::new(3.0, 8.0, 2.0),
406 CircleObstacle::new(3.0, 10.0, 2.0),
407 CircleObstacle::new(7.0, 5.0, 2.0),
408 CircleObstacle::new(9.0, 5.0, 2.0),
409 CircleObstacle::new(8.0, 10.0, 1.0),
410 ];
411 let rand_area = AreaBounds::new(-2.0, 15.0, -2.0, 15.0);
412 let config = RRTConfig {
413 robot_radius: 0.8,
414 ..Default::default()
415 };
416 RRTPlanner::new(obstacles, rand_area, None, config)
417 }
418
419 #[test]
420 fn test_rrt_finds_path() {
421 let planner = create_test_planner();
422 let start = Point2D::new(0.0, 0.0);
423 let goal = Point2D::new(10.0, 10.0);
424 let result = planner.plan(start, goal);
425 assert!(result.is_ok() || result.is_err());
426 }
427
428 #[test]
429 fn test_rrt_config_default() {
430 let config = RRTConfig::default();
431 assert_eq!(config.expand_dis, 3.0);
432 assert_eq!(config.max_iter, 500);
433 }
434
435 #[test]
436 fn test_rrt_upstream_test_rrt_short_goal_matches_pythonrobotics_reference() {
437 let mut planner = create_pythonrobotics_main_planner();
438 let start = Point2D::new(0.0, 0.0);
439 let goal = Point2D::new(1.0, 1.0);
440 let sample = [10.455_649_682_677_358, 11.942_970_283_541_907];
441 let path = planner
442 .plan_with_sampler(start, goal, |_| RRTNode::new(sample[0], sample[1]))
443 .unwrap();
444
445 assert_eq!(planner.node_list.len(), 2);
446 assert_eq!(path.points.len(), 3);
447 assert_point_close(&path.points[0], [0.0, 0.0]);
448 assert_point_close(
449 &path.points[1],
450 [1.976_107_921_083_293_5, 2.257_210_110_785_406],
451 );
452 assert_point_close(&path.points[2], [1.0, 1.0]);
453 assert_eq!(planner.node_list[1].parent, Some(0));
454 assert_eq!(planner.node_list[1].path_x.len(), 7);
455 assert_close(planner.node_list[1].path_x[1], 0.329_351_320_180_548_95);
456 assert_close(planner.node_list[1].path_y[1], 0.376_201_685_130_900_95);
457 }
458
459 #[test]
460 fn test_rrt_upstream_main_seeded_path_matches_pythonrobotics_reference() {
461 let mut planner = create_pythonrobotics_main_planner();
462 let start = Point2D::new(0.0, 0.0);
463 let goal = Point2D::new(6.0, 10.0);
464 let samples = parse_xy_fixture(include_str!("testdata/rrt_main_seed12345_samples.csv"));
465 let expected_path = parse_xy_fixture(include_str!("testdata/rrt_main_seed12345_path.csv"));
466 let mut sample_iter = samples.iter();
467
468 let path = planner
469 .plan_with_sampler(start, goal, |_| {
470 let sample = sample_iter
471 .next()
472 .expect("python reference sample sequence exhausted");
473 RRTNode::new(sample[0], sample[1])
474 })
475 .unwrap();
476
477 assert_eq!(planner.node_list.len(), 88);
478 assert_eq!(path.points.len(), expected_path.len());
479 for (actual, expected) in path.points.iter().zip(expected_path.iter()) {
480 assert_point_close(actual, *expected);
481 }
482
483 let expected_nodes = [
484 (
485 1,
486 [1.976_107_921_083_293, 2.257_210_110_785_406],
487 Some(0),
488 7,
489 [0.0, 0.329_351_320_180_549, 0.658_702_640_361_098],
490 [0.0, 0.376_201_685_130_901, 0.752_403_370_261_802],
491 ),
492 (
493 5,
494 [1.229_513_438_270_946, 3.806_527_238_150_387],
495 Some(1),
496 5,
497 [
498 1.976_107_921_083_293,
499 1.759_052_148_041_024,
500 1.541_996_374_998_755,
501 ],
502 [
503 2.257_210_110_785_406,
504 2.707_639_673_968_178,
505 3.158_069_237_150_95,
506 ],
507 ),
508 (
509 10,
510 [-0.964_870_854_478_227, 5.566_933_984_574_426],
511 Some(9),
512 7,
513 [
514 0.668_674_115_555_151,
515 0.358_283_476_598_378,
516 0.047_892_837_641_605,
517 ],
518 [
519 3.503_932_336_109_255,
520 3.895_924_238_127_659,
521 4.287_916_140_146_064,
522 ],
523 ),
524 (
525 20,
526 [13.060_964_451_302_038, 12.199_474_225_398_257],
527 Some(16),
528 8,
529 [
530 11.070_558_229_598_33,
531 11.395_810_000_597_592,
532 11.721_061_771_596_855,
533 ],
534 [
535 9.875_551_544_349_548,
536 10.255_303_154_565_809,
537 10.635_054_764_782_069,
538 ],
539 ),
540 (
541 87,
542 [5.860_033_119_067_657, 10.721_216_347_248_003],
543 Some(72),
544 7,
545 [
546 5.288_485_092_568_921,
547 5.383_743_096_985_377,
548 5.479_001_101_401_833,
549 ],
550 [
551 13.666_268_613_915_847,
552 13.175_426_569_471_206,
553 12.684_584_525_026_565,
554 ],
555 ),
556 ];
557 for (index, xy, parent, path_len, path_x3, path_y3) in expected_nodes {
558 let node = &planner.node_list[index];
559 assert_close(node.x, xy[0]);
560 assert_close(node.y, xy[1]);
561 assert_eq!(node.parent, parent);
562 assert_eq!(node.path_x.len(), path_len);
563 for (actual, expected) in node.path_x.iter().take(3).zip(path_x3.iter()) {
564 assert_close(*actual, *expected);
565 }
566 for (actual, expected) in node.path_y.iter().take(3).zip(path_y3.iter()) {
567 assert_close(*actual, *expected);
568 }
569 }
570 }
571}