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