1#![allow(
2 dead_code,
3 clippy::needless_borrows_for_generic_args,
4 clippy::new_without_default,
5 clippy::type_complexity,
6 clippy::too_many_arguments,
7 clippy::assign_op_pattern
8)]
9
10use rust_robotics_core::normalize_angle;
16use std::f64::consts::PI;
17
18#[derive(Debug, Clone)]
19pub struct Path {
20 pub lengths: Vec<f64>,
21 pub ctypes: Vec<char>,
22 pub l: f64,
23 pub x: Vec<f64>,
24 pub y: Vec<f64>,
25 pub yaw: Vec<f64>,
26 pub directions: Vec<i32>,
27}
28
29impl Path {
30 pub fn new() -> Self {
31 Path {
32 lengths: Vec::new(),
33 ctypes: Vec::new(),
34 l: 0.0,
35 x: Vec::new(),
36 y: Vec::new(),
37 yaw: Vec::new(),
38 directions: Vec::new(),
39 }
40 }
41}
42
43fn mod2pi(x: f64) -> f64 {
44 let v = x % (2.0 * PI);
45 if v < -PI {
46 v + 2.0 * PI
47 } else if v > PI {
48 v - 2.0 * PI
49 } else {
50 v
51 }
52}
53
54fn polar(x: f64, y: f64) -> (f64, f64) {
55 let r = (x * x + y * y).sqrt();
56 let theta = y.atan2(x);
57 (r, theta)
58}
59
60fn left_straight_left(x: f64, y: f64, phi: f64) -> (bool, Vec<f64>, Vec<char>) {
61 let (u, t) = polar(x - phi.sin(), y - 1.0 + phi.cos());
62 if (0.0..=PI).contains(&t) {
63 let v = mod2pi(phi - t);
64 if (0.0..=PI).contains(&v) {
65 return (true, vec![t, u, v], vec!['L', 'S', 'L']);
66 }
67 }
68 (false, Vec::new(), Vec::new())
69}
70
71fn left_straight_right(x: f64, y: f64, phi: f64) -> (bool, Vec<f64>, Vec<char>) {
72 let (u1, t1) = polar(x + phi.sin(), y - 1.0 - phi.cos());
73 let u1_sq = u1 * u1;
74 if u1_sq >= 4.0 {
75 let u = (u1_sq - 4.0).sqrt();
76 let theta = (2.0_f64).atan2(u);
77 let t = mod2pi(t1 + theta);
78 let v = mod2pi(t - phi);
79
80 if t >= 0.0 && v >= 0.0 {
81 return (true, vec![t, u, v], vec!['L', 'S', 'R']);
82 }
83 }
84 (false, Vec::new(), Vec::new())
85}
86
87fn left_x_right_x_left(x: f64, y: f64, phi: f64) -> (bool, Vec<f64>, Vec<char>) {
88 let zeta = x - phi.sin();
89 let eeta = y - 1.0 + phi.cos();
90 let (u1, theta) = polar(zeta, eeta);
91
92 if u1 <= 4.0 {
93 let a = (0.25 * u1).acos();
94 let t = mod2pi(a + theta + PI / 2.0);
95 let u = mod2pi(PI - 2.0 * a);
96 let v = mod2pi(phi - t - u);
97 return (true, vec![t, -u, v], vec!['L', 'R', 'L']);
98 }
99 (false, Vec::new(), Vec::new())
100}
101
102fn left_x_right_left(x: f64, y: f64, phi: f64) -> (bool, Vec<f64>, Vec<char>) {
103 let zeta = x - phi.sin();
104 let eeta = y - 1.0 + phi.cos();
105 let (u1, theta) = polar(zeta, eeta);
106
107 if u1 <= 4.0 {
108 let a = (0.25 * u1).acos();
109 let t = mod2pi(a + theta + PI / 2.0);
110 let u = mod2pi(PI - 2.0 * a);
111 let v = mod2pi(-phi + t + u);
112 return (true, vec![t, -u, -v], vec!['L', 'R', 'L']);
113 }
114 (false, Vec::new(), Vec::new())
115}
116
117fn left_right_x_left(x: f64, y: f64, phi: f64) -> (bool, Vec<f64>, Vec<char>) {
118 let zeta = x - phi.sin();
119 let eeta = y - 1.0 + phi.cos();
120 let (u1, theta) = polar(zeta, eeta);
121
122 if u1 <= 4.0 {
123 let u = (1.0 - u1 * u1 * 0.125).acos();
124 let a = (2.0 * u.sin() / u1).asin();
125 let t = mod2pi(-a + theta + PI / 2.0);
126 let v = mod2pi(t - u - phi);
127 return (true, vec![t, u, -v], vec!['L', 'R', 'L']);
128 }
129 (false, Vec::new(), Vec::new())
130}
131
132fn left_right_x_left_right(x: f64, y: f64, phi: f64) -> (bool, Vec<f64>, Vec<char>) {
133 let zeta = x + phi.sin();
134 let eeta = y - 1.0 - phi.cos();
135 let (u1, theta) = polar(zeta, eeta);
136
137 if u1 <= 2.0 {
138 let a = ((u1 + 2.0) * 0.25).acos();
139 let t = mod2pi(theta + a + PI / 2.0);
140 let u = mod2pi(a);
141 let v = mod2pi(phi - t + 2.0 * u);
142 if t >= 0.0 && u >= 0.0 && v >= 0.0 {
143 return (true, vec![t, u, -u, -v], vec!['L', 'R', 'L', 'R']);
144 }
145 }
146 (false, Vec::new(), Vec::new())
147}
148
149fn left_x_right_left_x_right(x: f64, y: f64, phi: f64) -> (bool, Vec<f64>, Vec<char>) {
150 let zeta = x + phi.sin();
151 let eeta = y - 1.0 - phi.cos();
152 let (u1, theta) = polar(zeta, eeta);
153 let u2 = (20.0 - u1 * u1) / 16.0;
154
155 if (0.0..=1.0).contains(&u2) {
156 let u = u2.acos();
157 let a = (2.0 * u.sin() / u1).asin();
158 let t = mod2pi(theta + a + PI / 2.0);
159 let v = mod2pi(t - phi);
160 if t >= 0.0 && v >= 0.0 {
161 return (true, vec![t, -u, -u, v], vec!['L', 'R', 'L', 'R']);
162 }
163 }
164 (false, Vec::new(), Vec::new())
165}
166
167fn left_x_right90_straight_left(x: f64, y: f64, phi: f64) -> (bool, Vec<f64>, Vec<char>) {
168 let zeta = x - phi.sin();
169 let eeta = y - 1.0 + phi.cos();
170 let (u1, theta) = polar(zeta, eeta);
171
172 if u1 >= 2.0 {
173 let u = (u1 * u1 - 4.0).sqrt() - 2.0;
174 let a = (2.0_f64).atan2((u1 * u1 - 4.0).sqrt());
175 let t = mod2pi(theta + a + PI / 2.0);
176 let v = mod2pi(t - phi + PI / 2.0);
177 if t >= 0.0 && v >= 0.0 {
178 return (true, vec![t, -PI / 2.0, -u, -v], vec!['L', 'R', 'S', 'L']);
179 }
180 }
181 (false, Vec::new(), Vec::new())
182}
183
184fn left_straight_right90_x_left(x: f64, y: f64, phi: f64) -> (bool, Vec<f64>, Vec<char>) {
185 let zeta = x - phi.sin();
186 let eeta = y - 1.0 + phi.cos();
187 let (u1, theta) = polar(zeta, eeta);
188
189 if u1 >= 2.0 {
190 let u = (u1 * u1 - 4.0).sqrt() - 2.0;
191 let a = ((u1 * u1 - 4.0).sqrt()).atan2(2.0);
192 let t = mod2pi(theta - a + PI / 2.0);
193 let v = mod2pi(t - phi - PI / 2.0);
194 if t >= 0.0 && v >= 0.0 {
195 return (true, vec![t, u, PI / 2.0, -v], vec!['L', 'S', 'R', 'L']);
196 }
197 }
198 (false, Vec::new(), Vec::new())
199}
200
201fn left_x_right90_straight_right(x: f64, y: f64, phi: f64) -> (bool, Vec<f64>, Vec<char>) {
202 let zeta = x + phi.sin();
203 let eeta = y - 1.0 - phi.cos();
204 let (u1, theta) = polar(zeta, eeta);
205
206 if u1 >= 2.0 {
207 let t = mod2pi(theta + PI / 2.0);
208 let u = u1 - 2.0;
209 let v = mod2pi(phi - t - PI / 2.0);
210 if t >= 0.0 && v >= 0.0 {
211 return (true, vec![t, -PI / 2.0, -u, -v], vec!['L', 'R', 'S', 'R']);
212 }
213 }
214 (false, Vec::new(), Vec::new())
215}
216
217fn left_straight_left90_x_right(x: f64, y: f64, phi: f64) -> (bool, Vec<f64>, Vec<char>) {
218 let zeta = x + phi.sin();
219 let eeta = y - 1.0 - phi.cos();
220 let (u1, theta) = polar(zeta, eeta);
221
222 if u1 >= 2.0 {
223 let t = mod2pi(theta);
224 let u = u1 - 2.0;
225 let v = mod2pi(phi - t - PI / 2.0);
226 if t >= 0.0 && v >= 0.0 {
227 return (true, vec![t, u, PI / 2.0, -v], vec!['L', 'S', 'L', 'R']);
228 }
229 }
230 (false, Vec::new(), Vec::new())
231}
232
233fn left_x_right90_straight_left90_x_right(x: f64, y: f64, phi: f64) -> (bool, Vec<f64>, Vec<char>) {
234 let zeta = x + phi.sin();
235 let eeta = y - 1.0 - phi.cos();
236 let (u1, theta) = polar(zeta, eeta);
237
238 if u1 >= 4.0 {
239 let u = (u1 * u1 - 4.0).sqrt() - 4.0;
240 let a = (2.0_f64).atan2((u1 * u1 - 4.0).sqrt());
241 let t = mod2pi(theta + a + PI / 2.0);
242 let v = mod2pi(t - phi);
243 if t >= 0.0 && v >= 0.0 {
244 return (
245 true,
246 vec![t, -PI / 2.0, -u, -PI / 2.0, v],
247 vec!['L', 'R', 'S', 'L', 'R'],
248 );
249 }
250 }
251 (false, Vec::new(), Vec::new())
252}
253
254fn timeflip(travel_distances: Vec<f64>) -> Vec<f64> {
255 travel_distances.iter().map(|x| -x).collect()
256}
257
258fn reflect(steering_directions: Vec<char>) -> Vec<char> {
259 steering_directions
260 .iter()
261 .map(|&dirn| match dirn {
262 'L' => 'R',
263 'R' => 'L',
264 _ => 'S',
265 })
266 .collect()
267}
268
269fn set_path(paths: &mut Vec<Path>, lengths: Vec<f64>, ctypes: Vec<char>, step_size: f64) {
270 let mut path = Path::new();
271 path.ctypes = ctypes;
272 path.lengths = lengths;
273 path.l = path.lengths.iter().map(|x| x.abs()).sum();
274
275 for existing_path in paths.iter() {
276 let type_is_same = existing_path.ctypes == path.ctypes;
277 let length_is_close = (existing_path.l - path.l) <= step_size;
278 if type_is_same && length_is_close {
279 return;
280 }
281 }
282
283 if path.l <= step_size {
284 return;
285 }
286
287 paths.push(path);
288}
289
290fn has_too_small_segment(travel_distances: &[f64], step_size: f64) -> bool {
291 let min_dist = 0.1 * travel_distances.iter().map(|d| d.abs()).sum::<f64>();
292 travel_distances
293 .iter()
294 .any(|&distance| min_dist < distance.abs() && distance.abs() < step_size)
295}
296
297fn generate_path(q0: [f64; 3], q1: [f64; 3], max_curvature: f64, step_size: f64) -> Vec<Path> {
298 let dx = q1[0] - q0[0];
299 let dy = q1[1] - q0[1];
300 let dth = q1[2] - q0[2];
301 let c = q0[2].cos();
302 let s = q0[2].sin();
303 let x = (c * dx + s * dy) * max_curvature;
304 let y = (-s * dx + c * dy) * max_curvature;
305 let step_size = step_size * max_curvature;
306
307 let mut paths = Vec::new();
308
309 let path_functions: Vec<fn(f64, f64, f64) -> (bool, Vec<f64>, Vec<char>)> = vec![
310 left_straight_left,
311 left_straight_right,
312 left_x_right_x_left,
313 left_x_right_left,
314 left_right_x_left,
315 left_right_x_left_right,
316 left_x_right_left_x_right,
317 left_x_right90_straight_left,
318 left_x_right90_straight_right,
319 left_straight_right90_x_left,
320 left_straight_left90_x_right,
321 left_x_right90_straight_left90_x_right,
322 ];
323
324 for path_func in path_functions {
327 let (flag, travel_distances, steering_dirns) = path_func(x, y, dth);
329 if flag && !has_too_small_segment(&travel_distances, step_size) {
330 set_path(&mut paths, travel_distances, steering_dirns, step_size);
331 }
332
333 let (flag, travel_distances, steering_dirns) = path_func(-x, y, -dth);
335 if flag && !has_too_small_segment(&travel_distances, step_size) {
336 let travel_distances = timeflip(travel_distances);
337 set_path(&mut paths, travel_distances, steering_dirns, step_size);
338 }
339
340 let (flag, travel_distances, steering_dirns) = path_func(x, -y, -dth);
342 if flag && !has_too_small_segment(&travel_distances, step_size) {
343 let steering_dirns = reflect(steering_dirns);
344 set_path(&mut paths, travel_distances, steering_dirns, step_size);
345 }
346
347 let (flag, travel_distances, steering_dirns) = path_func(-x, -y, dth);
349 if flag && !has_too_small_segment(&travel_distances, step_size) {
350 let travel_distances = timeflip(travel_distances);
351 let steering_dirns = reflect(steering_dirns);
352 set_path(&mut paths, travel_distances, steering_dirns, step_size);
353 }
354 }
355
356 paths
357}
358
359fn calc_interpolate_dists_list(lengths: &[f64], step_size: f64) -> Vec<Vec<f64>> {
360 let mut interpolate_dists_list = Vec::new();
361
362 for &length in lengths {
363 let d_dist = if length >= 0.0 { step_size } else { -step_size };
364 let mut interp_dists = Vec::new();
365 let mut current = 0.0;
366
367 while (length >= 0.0 && current < length) || (length < 0.0 && current > length) {
368 interp_dists.push(current);
369 current += d_dist;
370 }
371 interp_dists.push(length);
372 interpolate_dists_list.push(interp_dists);
373 }
374
375 interpolate_dists_list
376}
377
378fn interpolate(
379 dist: f64,
380 length: f64,
381 mode: char,
382 max_curvature: f64,
383 origin_x: f64,
384 origin_y: f64,
385 origin_yaw: f64,
386) -> (f64, f64, f64, i32) {
387 if mode == 'S' {
388 let x = origin_x + dist / max_curvature * origin_yaw.cos();
389 let y = origin_y + dist / max_curvature * origin_yaw.sin();
390 let yaw = origin_yaw;
391 (x, y, yaw, if length > 0.0 { 1 } else { -1 })
392 } else {
393 let ldx = dist.sin() / max_curvature;
394 let (ldy, yaw) = if mode == 'L' {
395 ((1.0 - dist.cos()) / max_curvature, origin_yaw + dist)
396 } else {
397 ((1.0 - dist.cos()) / -max_curvature, origin_yaw - dist)
399 };
400
401 let gdx = (-origin_yaw).cos() * ldx + (-origin_yaw).sin() * ldy;
402 let gdy = -(-origin_yaw).sin() * ldx + (-origin_yaw).cos() * ldy;
403 let x = origin_x + gdx;
404 let y = origin_y + gdy;
405
406 (x, y, yaw, if length > 0.0 { 1 } else { -1 })
407 }
408}
409
410fn generate_local_course(
411 lengths: &[f64],
412 modes: &[char],
413 max_curvature: f64,
414 step_size: f64,
415) -> (Vec<f64>, Vec<f64>, Vec<f64>, Vec<i32>) {
416 let interpolate_dists_list = calc_interpolate_dists_list(lengths, step_size * max_curvature);
417
418 let mut origin_x = 0.0;
419 let mut origin_y = 0.0;
420 let mut origin_yaw = 0.0;
421
422 let mut xs = Vec::new();
423 let mut ys = Vec::new();
424 let mut yaws = Vec::new();
425 let mut directions = Vec::new();
426
427 for ((interp_dists, &mode), &length) in interpolate_dists_list.iter().zip(modes).zip(lengths) {
428 if length == 0.0 {
432 continue;
433 }
434 for &dist in interp_dists {
435 let (x, y, yaw, direction) = interpolate(
436 dist,
437 length,
438 mode,
439 max_curvature,
440 origin_x,
441 origin_y,
442 origin_yaw,
443 );
444 xs.push(x);
445 ys.push(y);
446 yaws.push(yaw);
447 directions.push(direction);
448 }
449 if !xs.is_empty() {
450 origin_x = xs[xs.len() - 1];
451 origin_y = ys[ys.len() - 1];
452 origin_yaw = yaws[yaws.len() - 1];
453 }
454 }
455
456 (xs, ys, yaws, directions)
457}
458
459fn calc_paths(
460 sx: f64,
461 sy: f64,
462 syaw: f64,
463 gx: f64,
464 gy: f64,
465 gyaw: f64,
466 maxc: f64,
467 step_size: f64,
468) -> Vec<Path> {
469 let q0 = [sx, sy, syaw];
470 let q1 = [gx, gy, gyaw];
471
472 let mut paths = generate_path(q0, q1, maxc, step_size);
473
474 for path in &mut paths {
475 let (xs, ys, yaws, directions) =
476 generate_local_course(&path.lengths, &path.ctypes, maxc, step_size);
477
478 path.x = xs
479 .iter()
480 .zip(ys.iter())
481 .map(|(&ix, &iy)| (-q0[2]).cos() * ix + (-q0[2]).sin() * iy + q0[0])
482 .collect();
483
484 path.y = xs
485 .iter()
486 .zip(ys.iter())
487 .map(|(&ix, &iy)| -(-q0[2]).sin() * ix + (-q0[2]).cos() * iy + q0[1])
488 .collect();
489
490 path.yaw = yaws
491 .iter()
492 .map(|&yaw| normalize_angle(yaw + q0[2]))
493 .collect();
494 path.directions = directions;
495 path.lengths = path.lengths.iter().map(|&length| length / maxc).collect();
496 path.l = path.l / maxc;
497 }
498
499 paths
500}
501
502#[allow(clippy::too_many_arguments)]
508pub fn reeds_shepp_paths(
509 sx: f64,
510 sy: f64,
511 syaw: f64,
512 gx: f64,
513 gy: f64,
514 gyaw: f64,
515 maxc: f64,
516 step_size: f64,
517) -> Vec<Path> {
518 calc_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size)
519}
520
521pub fn reeds_shepp_path_planning(
522 sx: f64,
523 sy: f64,
524 syaw: f64,
525 gx: f64,
526 gy: f64,
527 gyaw: f64,
528 maxc: f64,
529 step_size: f64,
530) -> Option<(Vec<f64>, Vec<f64>, Vec<f64>, Vec<char>, Vec<f64>)> {
531 let paths = calc_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size);
532
533 if paths.is_empty() {
534 return None;
535 }
536
537 let best_path = paths.iter().min_by(|a, b| a.l.partial_cmp(&b.l).unwrap())?;
538
539 Some((
540 best_path.x.clone(),
541 best_path.y.clone(),
542 best_path.yaw.clone(),
543 best_path.ctypes.clone(),
544 best_path.lengths.clone(),
545 ))
546}
547
548pub struct ReedsSheppPlanner {
549 pub path_x: Vec<f64>,
550 pub path_y: Vec<f64>,
551 pub path_yaw: Vec<f64>,
552 pub modes: Vec<char>,
553 pub lengths: Vec<f64>,
554}
555
556impl ReedsSheppPlanner {
557 pub fn new() -> Self {
558 ReedsSheppPlanner {
559 path_x: Vec::new(),
560 path_y: Vec::new(),
561 path_yaw: Vec::new(),
562 modes: Vec::new(),
563 lengths: Vec::new(),
564 }
565 }
566
567 pub fn planning(
568 &mut self,
569 sx: f64,
570 sy: f64,
571 syaw: f64,
572 gx: f64,
573 gy: f64,
574 gyaw: f64,
575 max_curvature: f64,
576 step_size: f64,
577 ) -> bool {
578 if let Some((x, y, yaw, modes, lengths)) =
579 reeds_shepp_path_planning(sx, sy, syaw, gx, gy, gyaw, max_curvature, step_size)
580 {
581 self.path_x = x;
582 self.path_y = y;
583 self.path_yaw = yaw;
584 self.modes = modes;
585 self.lengths = lengths;
586 true
587 } else {
588 false
589 }
590 }
591}
592
593impl Default for ReedsSheppPlanner {
594 fn default() -> Self {
595 Self::new()
596 }
597}
598
599#[cfg(test)]
600#[allow(clippy::excessive_precision)]
601mod tests {
602 use super::*;
603 use std::f64::consts::PI;
604
605 fn approx_eq(a: f64, b: f64, tol: f64) -> bool {
606 (a - b).abs() < tol
607 }
608
609 fn path_fingerprint(path: &Path) -> (f64, f64, f64, f64, f64, f64) {
610 let sum_x: f64 = path.x.iter().sum();
611 let sum_y: f64 = path.y.iter().sum();
612 let sum_yaw: f64 = path.yaw.iter().sum();
613 let weighted_sum_x: f64 = path
614 .x
615 .iter()
616 .enumerate()
617 .map(|(index, value)| (index + 1) as f64 * value)
618 .sum();
619 let weighted_sum_y: f64 = path
620 .y
621 .iter()
622 .enumerate()
623 .map(|(index, value)| (index + 1) as f64 * value)
624 .sum();
625 let weighted_sum_yaw: f64 = path
626 .yaw
627 .iter()
628 .enumerate()
629 .map(|(index, value)| (index + 1) as f64 * value)
630 .sum();
631
632 (
633 sum_x,
634 sum_y,
635 sum_yaw,
636 weighted_sum_x,
637 weighted_sum_y,
638 weighted_sum_yaw,
639 )
640 }
641
642 fn assert_fingerprint_close(
643 actual: (f64, f64, f64, f64, f64, f64),
644 expected: (f64, f64, f64, f64, f64, f64),
645 ) {
646 assert!(approx_eq(actual.0, expected.0, 1e-9));
647 assert!(approx_eq(actual.1, expected.1, 1e-9));
648 assert!(approx_eq(actual.2, expected.2, 1e-9));
649 assert!(approx_eq(actual.3, expected.3, 1e-6));
650 assert!(approx_eq(actual.4, expected.4, 1e-6));
651 assert!(approx_eq(actual.5, expected.5, 1e-6));
652 }
653
654 #[test]
655 fn test_reeds_shepp_planning() {
656 let mut planner = ReedsSheppPlanner::new();
657 let result = planner.planning(
658 -1.0,
659 -4.0,
660 (-20.0_f64).to_radians(),
661 5.0,
662 5.0,
663 (25.0_f64).to_radians(),
664 0.1,
665 0.05,
666 );
667 assert!(result);
668 assert!(!planner.path_x.is_empty());
669 assert!(!planner.modes.is_empty());
670 }
671
672 #[test]
673 fn test_reeds_shepp_path_planning_fn() {
674 let result = reeds_shepp_path_planning(0.0, 0.0, 0.0, 5.0, 5.0, 0.5, 0.2, 0.1);
675 assert!(result.is_some());
676 }
677
678 #[test]
679 fn test_calc_paths_matches_upstream_main_example_candidates() {
680 let paths = calc_paths(
681 -1.0,
682 -4.0,
683 (-20.0_f64).to_radians(),
684 5.0,
685 5.0,
686 25.0_f64.to_radians(),
687 0.1,
688 0.05,
689 );
690
691 assert_eq!(paths.len(), 5);
692
693 let best_path = paths
694 .iter()
695 .min_by(|a, b| a.l.partial_cmp(&b.l).unwrap())
696 .unwrap();
697 assert_eq!(best_path.ctypes, vec!['R', 'L', 'R']);
698 assert!(approx_eq(best_path.l, 19.647_318_205_087_014, 1e-12));
699 assert!(approx_eq(
700 best_path.lengths[0],
701 -5.228_525_224_523_852,
702 1e-12
703 ));
704 assert!(approx_eq(
705 best_path.lengths[1],
706 8.522_124_695_006_896,
707 1e-12
708 ));
709 assert!(approx_eq(
710 best_path.lengths[2],
711 5.896_668_285_556_266,
712 1e-12
713 ));
714 assert_eq!(best_path.x.len(), 397);
715 assert_eq!(best_path.directions.len(), 397);
716 assert_eq!(best_path.directions.iter().sum::<i32>(), 185);
717 assert_eq!(
718 best_path
719 .directions
720 .iter()
721 .enumerate()
722 .map(|(index, direction)| (index + 1) as i32 * direction)
723 .sum::<i32>(),
724 67_661
725 );
726 }
727
728 #[test]
729 fn test_reeds_shepp_path_planning_matches_upstream_main_example() {
730 let (x, y, yaw, modes, lengths) = reeds_shepp_path_planning(
731 -1.0,
732 -4.0,
733 (-20.0_f64).to_radians(),
734 5.0,
735 5.0,
736 25.0_f64.to_radians(),
737 0.1,
738 0.05,
739 )
740 .unwrap();
741
742 assert_eq!(modes, vec!['R', 'L', 'R']);
743 assert!(approx_eq(lengths[0], -5.228_525_224_523_852, 1e-12));
744 assert!(approx_eq(lengths[1], 8.522_124_695_006_896, 1e-12));
745 assert!(approx_eq(lengths[2], 5.896_668_285_556_266, 1e-12));
746 assert_eq!(x.len(), 397);
747 assert_eq!(y.len(), 397);
748 assert_eq!(yaw.len(), 397);
749
750 let expected_samples = [
751 (0usize, -1.0, -4.0, -0.349_065_850_398_865_95),
752 (
753 50,
754 -3.431_162_528_468_660_4,
755 -3.445_956_303_130_415_2,
756 -0.099_065_850_398_865_95,
757 ),
758 (
759 100,
760 -5.923_818_705_704_953,
761 -3.510_615_718_197_37,
762 0.150_934_149_601_134_05,
763 ),
764 (
765 150,
766 -4.041_585_620_182_658,
767 -2.932_812_755_977_549,
768 0.393_786_672_053_519_47,
769 ),
770 (396, 5.000_000_000_000_005, 5.0, 0.436_332_312_998_582_33),
771 ];
772
773 for (index, expected_x, expected_y, expected_yaw) in expected_samples {
774 assert!(approx_eq(x[index], expected_x, 1e-12));
775 assert!(approx_eq(y[index], expected_y, 1e-12));
776 assert!(approx_eq(yaw[index], expected_yaw, 1e-12));
777 }
778
779 let sum_x: f64 = x.iter().sum();
780 let sum_y: f64 = y.iter().sum();
781 let sum_yaw: f64 = yaw.iter().sum();
782 let weighted_sum_x: f64 = x
783 .iter()
784 .enumerate()
785 .map(|(index, value)| (index + 1) as f64 * value)
786 .sum();
787 let weighted_sum_y: f64 = y
788 .iter()
789 .enumerate()
790 .map(|(index, value)| (index + 1) as f64 * value)
791 .sum();
792 let weighted_sum_yaw: f64 = yaw
793 .iter()
794 .enumerate()
795 .map(|(index, value)| (index + 1) as f64 * value)
796 .sum();
797
798 assert!(approx_eq(sum_x, -474.632_647_731_712_4, 1e-9));
799 assert!(approx_eq(sum_y, -278.042_684_871_068_6, 1e-9));
800 assert!(approx_eq(sum_yaw, 181.229_623_459_273_85, 1e-9));
801 assert!(approx_eq(weighted_sum_x, 24_450.581_151_074_42, 1e-6));
802 assert!(approx_eq(weighted_sum_y, 72_297.919_198_954_09, 1e-6));
803 assert!(approx_eq(weighted_sum_yaw, 50_733.291_448_638_01, 1e-6));
804 }
805
806 #[test]
807 fn test_calc_paths_matches_upstream_representative_mode_coverage() {
808 struct Scenario {
809 goal: (f64, f64, f64),
810 path_count: usize,
811 mode: &'static [char],
812 lengths: &'static [f64],
813 total: f64,
814 sample_count: usize,
815 direction_sum: i32,
816 direction_weighted: i32,
817 fingerprint: (f64, f64, f64, f64, f64, f64),
818 }
819
820 let start = (0.0, 0.0, -PI / 2.0);
821 let scenarios = [
822 Scenario {
823 goal: (2.0, -6.0, -PI / 4.0),
824 path_count: 9,
825 mode: &['L', 'S', 'L'],
826 lengths: &[
827 1.069_877_996_599_901_2,
828 2.521_981_303_104_907_6,
829 2.857_112_820_387_34,
830 ],
831 total: 6.448_972_120_092_149,
832 sample_count: 69,
833 direction_sum: 69,
834 direction_weighted: 2_415,
835 fingerprint: (
836 47.658_101_248_550_764,
837 -214.691_563_475_246_82,
838 -86.182_939_224_211_54,
839 2_416.980_927_612_332_4,
840 -9_962.039_734_007_922,
841 -2_763.536_289_485_696,
842 ),
843 },
844 Scenario {
845 goal: (4.0, -6.0, -PI / 4.0),
846 path_count: 8,
847 mode: &['L', 'S', 'R'],
848 lengths: &[
849 3.999_542_478_656_482,
850 3.390_792_633_449_932_8,
851 0.072_551_661_669_240_65,
852 ],
853 total: 7.462_886_773_775_654,
854 sample_count: 78,
855 direction_sum: 78,
856 direction_weighted: 3_081,
857 fingerprint: (
858 125.110_316_958_156_83,
859 -256.540_636_968_477_1,
860 -76.540_100_984_546_51,
861 7_190.184_713_371_897_5,
862 -13_176.153_323_150_214,
863 -2_605.762_179_928_655_3,
864 ),
865 },
866 Scenario {
867 goal: (-4.0, -6.0, -3.0 * PI / 4.0),
868 path_count: 8,
869 mode: &['R', 'S', 'L'],
870 lengths: &[
871 3.999_542_478_656_482_4,
872 3.390_792_633_449_932_8,
873 0.072_551_661_669_241_21,
874 ],
875 total: 7.462_886_773_775_656,
876 sample_count: 78,
877 direction_sum: 78,
878 direction_weighted: 3_081,
879 fingerprint: (
880 -125.110_316_958_156_82,
881 -256.540_636_968_477_14,
882 -168.504_125_995_457_34,
883 -7_190.184_713_371_896,
884 -13_176.153_323_150_216,
885 -7_073.484_785_781_497_5,
886 ),
887 },
888 Scenario {
889 goal: (-6.0, -6.0, PI),
890 path_count: 9,
891 mode: &['R', 'S', 'R'],
892 lengths: &[
893 3.926_990_816_987_239,
894 1.414_213_562_373_095_6,
895 3.926_990_816_987_246,
896 ],
897 total: 9.268_195_196_347_58,
898 sample_count: 98,
899 direction_sum: 98,
900 direction_weighted: 4_851,
901 fingerprint: (
902 -224.230_783_041_927_37,
903 -368.245_146_496_736_9,
904 -225.193_346_359_169_7,
905 -16_127.734_907_593_596,
906 -23_204.350_340_917_714,
907 -11_989.089_837_846_506,
908 ),
909 },
910 Scenario {
911 goal: (-6.0, -6.0, -3.0 * PI / 4.0),
912 path_count: 6,
913 mode: &['L', 'R', 'L'],
914 lengths: &[
915 -0.068_966_848_925_894_33,
916 6.503_746_832_829_835,
917 2.645_722_864_768_488,
918 ],
919 total: 9.218_436_546_524_217,
920 sample_count: 97,
921 direction_sum: 93,
922 direction_weighted: 4_747,
923 fingerprint: (
924 -230.574_821_137_002_54,
925 -331.875_810_770_970_8,
926 -226.763_994_255_941_5,
927 -16_521.117_485_464_518,
928 -20_916.307_531_974_84,
929 -11_997.697_736_223_088,
930 ),
931 },
932 Scenario {
933 goal: (-6.0, -6.0, -PI / 4.0),
934 path_count: 6,
935 mode: &['R', 'L', 'R'],
936 lengths: &[
937 5.000_868_664_582_23,
938 5.561_988_484_973_19,
939 -3.365_870_996_596_280_8,
940 ],
941 total: 13.928_728_146_151_7,
942 sample_count: 144,
943 direction_sum: 74,
944 direction_weighted: 1_550,
945 fingerprint: (
946 -443.806_209_833_689_9,
947 -744.765_650_985_955,
948 -261.971_336_928_568_35,
949 -43_201.590_996_218_62,
950 -67_652.561_148_743_36,
951 -16_979.120_856_723_654,
952 ),
953 },
954 Scenario {
955 goal: (-6.0, -6.0, -PI / 2.0),
956 path_count: 6,
957 mode: &['L', 'R', 'L', 'R'],
958 lengths: &[
959 -0.823_349_563_605_059_2,
960 5.119_726_880_494_763,
961 5.119_726_880_494_763,
962 -0.823_349_563_605_059_2,
963 ],
964 total: 11.886_152_888_199_643,
965 sample_count: 126,
966 direction_sum: 86,
967 direction_weighted: 5_461,
968 fingerprint: (
969 -380.403_475_990_598_96,
970 -380.444_909_002_660_5,
971 -271.291_151_985_038_8,
972 -34_379.127_386_055_37,
973 -34_617.891_225_324_86,
974 -17_198.177_166_350_25,
975 ),
976 },
977 Scenario {
978 goal: (-6.0, -6.0, PI / 4.0),
979 path_count: 4,
980 mode: &['L', 'R', 'S', 'L'],
981 lengths: &[
982 4.566_911_224_275_087,
983 -7.853_981_633_974_483,
984 -0.833_066_927_667_685,
985 -0.639_920_407_287_846_2,
986 ],
987 total: 13.893_880_193_205_101,
988 sample_count: 145,
989 direction_sum: -51,
990 direction_weighted: -8_329,
991 fingerprint: (
992 -203.973_341_223_846_2,
993 -476.554_271_600_198_1,
994 -25.723_479_016_967_794,
995 -28_387.645_373_178_693,
996 -41_452.300_874_567_176,
997 2_865.645_752_604_772,
998 ),
999 },
1000 Scenario {
1001 goal: (-4.0, -6.0, PI),
1002 path_count: 6,
1003 mode: &['L', 'S', 'R', 'L'],
1004 lengths: &[
1005 0.473_590_382_777_882,
1006 0.099_504_938_362_080_52,
1007 7.853_981_633_974_483,
1008 -0.473_590_382_777_882,
1009 ],
1010 total: 8.900_667_337_892_326,
1011 sample_count: 94,
1012 direction_sum: 82,
1013 direction_weighted: 3_367,
1014 fingerprint: (
1015 -145.451_641_721_792_1,
1016 -351.919_906_224_341_96,
1017 -211.935_238_269_393_84,
1018 -10_708.096_596_694_699,
1019 -21_581.461_832_452_35,
1020 -11_398.570_917_831_472,
1021 ),
1022 },
1023 Scenario {
1024 goal: (4.0, -6.0, 0.0),
1025 path_count: 6,
1026 mode: &['R', 'S', 'L', 'R'],
1027 lengths: &[
1028 0.473_590_382_777_883_1,
1029 0.099_504_938_362_076_08,
1030 7.853_981_633_974_483,
1031 -0.473_590_382_777_883_1,
1032 ],
1033 total: 8.900_667_337_892_324,
1034 sample_count: 94,
1035 direction_sum: 82,
1036 direction_weighted: 3_367,
1037 fingerprint: (
1038 145.451_641_721_792_13,
1039 -351.919_906_224_341_6,
1040 -83.374_471_168_046_72,
1041 10_708.096_596_694_699,
1042 -21_581.461_832_452_34,
1043 -2_628.640_280_446_955,
1044 ),
1045 },
1046 Scenario {
1047 goal: (-6.0, -6.0, 3.0 * PI / 4.0),
1048 path_count: 4,
1049 mode: &['R', 'S', 'R', 'L'],
1050 lengths: &[
1051 2.219_874_422_095_635_2,
1052 0.559_236_463_071_481_9,
1053 7.853_981_633_974_483,
1054 -1.707_116_394_891_605_7,
1055 ],
1056 total: 12.340_208_914_033_205,
1057 sample_count: 130,
1058 direction_sum: 92,
1059 direction_weighted: 3_917,
1060 fingerprint: (
1061 -435.220_953_689_669,
1062 -527.340_252_871_888_3,
1063 -89.312_402_029_516_8,
1064 -40_676.492_201_712_46,
1065 -42_014.939_961_467_01,
1066 2_377.125_258_488_301,
1067 ),
1068 },
1069 Scenario {
1070 goal: (-6.0, -2.0, -PI / 4.0),
1071 path_count: 5,
1072 mode: &['R', 'L', 'R', 'L'],
1073 lengths: &[
1074 1.605_151_609_076_620_5,
1075 4.327_807_515_918_15,
1076 -4.327_807_515_918_15,
1077 -3.123_472_605_772_438,
1078 ],
1079 total: 13.384_239_246_685_356,
1080 sample_count: 141,
1081 direction_sum: -15,
1082 direction_weighted: -5_979,
1083 fingerprint: (
1084 -234.424_840_465_174_4,
1085 -469.790_322_454_025_9,
1086 -138.906_286_644_584_9,
1087 -26_289.793_852_112_638,
1088 -35_567.135_089_521_57,
1089 -6_993.944_431_851_961,
1090 ),
1091 },
1092 ];
1093
1094 for scenario in scenarios {
1095 let paths = calc_paths(
1096 start.0,
1097 start.1,
1098 start.2,
1099 scenario.goal.0,
1100 scenario.goal.1,
1101 scenario.goal.2,
1102 0.2,
1103 0.1,
1104 );
1105 assert_eq!(paths.len(), scenario.path_count);
1106
1107 let best_path = paths
1108 .iter()
1109 .min_by(|a, b| a.l.partial_cmp(&b.l).unwrap())
1110 .unwrap();
1111
1112 assert_eq!(best_path.ctypes, scenario.mode);
1113 assert_eq!(best_path.lengths.len(), scenario.lengths.len());
1114 for (actual, expected) in best_path.lengths.iter().zip(scenario.lengths.iter()) {
1115 assert!(approx_eq(*actual, *expected, 1e-12));
1116 }
1117 assert!(approx_eq(best_path.l, scenario.total, 1e-12));
1118 assert_eq!(best_path.x.len(), scenario.sample_count);
1119 assert_eq!(best_path.y.len(), scenario.sample_count);
1120 assert_eq!(best_path.yaw.len(), scenario.sample_count);
1121 assert_eq!(
1122 best_path.directions.iter().sum::<i32>(),
1123 scenario.direction_sum
1124 );
1125 assert_eq!(
1126 best_path
1127 .directions
1128 .iter()
1129 .enumerate()
1130 .map(|(index, direction)| (index + 1) as i32 * direction)
1131 .sum::<i32>(),
1132 scenario.direction_weighted
1133 );
1134 assert!(approx_eq(
1135 *best_path.x.last().unwrap(),
1136 scenario.goal.0,
1137 1e-12
1138 ));
1139 assert!(approx_eq(
1140 *best_path.y.last().unwrap(),
1141 scenario.goal.1,
1142 1e-12
1143 ));
1144 assert!(approx_eq(
1145 normalize_angle(*best_path.yaw.last().unwrap() - scenario.goal.2),
1146 0.0,
1147 1e-12
1148 ));
1149 assert_fingerprint_close(path_fingerprint(best_path), scenario.fingerprint);
1150 }
1151 }
1152}