1#![allow(dead_code)]
2
3use rust_robotics_core::normalize_angle;
10const MAX_SPEED: f64 = 50.0 / 3.6;
11const MAX_ACCEL: f64 = 5.0;
12const MAX_CURVATURE: f64 = 1.0;
13const MAX_ROAD_WIDTH: f64 = 7.0;
14const D_ROAD_W: f64 = 1.0;
15const DT: f64 = 0.2;
16const MAX_T: f64 = 5.0;
17const MIN_T: f64 = 4.0;
18const TARGET_SPEED: f64 = 30.0 / 3.6;
19const D_T_S: f64 = 5.0 / 3.6;
20const N_S_SAMPLE: usize = 1;
21const ROBOT_RADIUS: f64 = 2.0;
22
23const K_J: f64 = 0.1;
25const K_T: f64 = 0.1;
26const K_S_DOT: f64 = 1.0;
27const K_D: f64 = 1.0;
28const K_S: f64 = 1.0;
29const K_LAT: f64 = 1.0;
30const K_LON: f64 = 1.0;
31
32struct QuinticPolynomial {
34 a0: f64,
35 a1: f64,
36 a2: f64,
37 a3: f64,
38 a4: f64,
39 a5: f64,
40}
41
42impl QuinticPolynomial {
43 fn new(xs: f64, vxs: f64, axs: f64, xe: f64, vxe: f64, axe: f64, time: f64) -> Self {
44 let a0 = xs;
45 let a1 = vxs;
46 let a2 = axs / 2.0;
47
48 let t2 = time * time;
49 let t3 = t2 * time;
50 let t4 = t3 * time;
51 let t5 = t4 * time;
52
53 let a = nalgebra::Matrix3::new(
54 t3,
55 t4,
56 t5,
57 3.0 * t2,
58 4.0 * t3,
59 5.0 * t4,
60 6.0 * time,
61 12.0 * t2,
62 20.0 * t3,
63 );
64
65 let b = nalgebra::Vector3::new(
66 xe - a0 - a1 * time - a2 * t2,
67 vxe - a1 - 2.0 * a2 * time,
68 axe - 2.0 * a2,
69 );
70
71 let x = a
72 .try_inverse()
73 .map(|inv| inv * b)
74 .unwrap_or(nalgebra::Vector3::zeros());
75
76 QuinticPolynomial {
77 a0,
78 a1,
79 a2,
80 a3: x[0],
81 a4: x[1],
82 a5: x[2],
83 }
84 }
85
86 fn calc_point(&self, t: f64) -> f64 {
87 self.a0
88 + self.a1 * t
89 + self.a2 * t.powi(2)
90 + self.a3 * t.powi(3)
91 + self.a4 * t.powi(4)
92 + self.a5 * t.powi(5)
93 }
94
95 fn calc_first_derivative(&self, t: f64) -> f64 {
96 self.a1
97 + 2.0 * self.a2 * t
98 + 3.0 * self.a3 * t.powi(2)
99 + 4.0 * self.a4 * t.powi(3)
100 + 5.0 * self.a5 * t.powi(4)
101 }
102
103 fn calc_second_derivative(&self, t: f64) -> f64 {
104 2.0 * self.a2 + 6.0 * self.a3 * t + 12.0 * self.a4 * t.powi(2) + 20.0 * self.a5 * t.powi(3)
105 }
106
107 fn calc_third_derivative(&self, t: f64) -> f64 {
108 6.0 * self.a3 + 24.0 * self.a4 * t + 60.0 * self.a5 * t.powi(2)
109 }
110}
111
112struct QuarticPolynomial {
114 a0: f64,
115 a1: f64,
116 a2: f64,
117 a3: f64,
118 a4: f64,
119}
120
121impl QuarticPolynomial {
122 fn new(xs: f64, vxs: f64, axs: f64, vxe: f64, axe: f64, time: f64) -> Self {
123 let a0 = xs;
124 let a1 = vxs;
125 let a2 = axs / 2.0;
126
127 let t2 = time * time;
128 let t3 = t2 * time;
129
130 let a = nalgebra::Matrix2::new(3.0 * t2, 4.0 * t3, 6.0 * time, 12.0 * t2);
131
132 let b = nalgebra::Vector2::new(vxe - a1 - 2.0 * a2 * time, axe - 2.0 * a2);
133
134 let x = a
135 .try_inverse()
136 .map(|inv| inv * b)
137 .unwrap_or(nalgebra::Vector2::zeros());
138
139 QuarticPolynomial {
140 a0,
141 a1,
142 a2,
143 a3: x[0],
144 a4: x[1],
145 }
146 }
147
148 fn calc_point(&self, t: f64) -> f64 {
149 self.a0 + self.a1 * t + self.a2 * t.powi(2) + self.a3 * t.powi(3) + self.a4 * t.powi(4)
150 }
151
152 fn calc_first_derivative(&self, t: f64) -> f64 {
153 self.a1 + 2.0 * self.a2 * t + 3.0 * self.a3 * t.powi(2) + 4.0 * self.a4 * t.powi(3)
154 }
155
156 fn calc_second_derivative(&self, t: f64) -> f64 {
157 2.0 * self.a2 + 6.0 * self.a3 * t + 12.0 * self.a4 * t.powi(2)
158 }
159
160 fn calc_third_derivative(&self, t: f64) -> f64 {
161 6.0 * self.a3 + 24.0 * self.a4 * t
162 }
163}
164
165#[derive(Clone)]
167pub struct FrenetPath {
168 pub t: Vec<f64>,
169 pub d: Vec<f64>,
170 pub d_d: Vec<f64>,
171 pub d_dd: Vec<f64>,
172 pub d_ddd: Vec<f64>,
173 pub s: Vec<f64>,
174 pub s_d: Vec<f64>,
175 pub s_dd: Vec<f64>,
176 pub s_ddd: Vec<f64>,
177 pub cd: f64,
178 pub cv: f64,
179 pub cf: f64,
180 pub x: Vec<f64>,
181 pub y: Vec<f64>,
182 pub yaw: Vec<f64>,
183 pub v: Vec<f64>,
184 pub a: Vec<f64>,
185 pub ds: Vec<f64>,
186 pub c: Vec<f64>,
187}
188
189impl FrenetPath {
190 pub fn new() -> Self {
191 FrenetPath {
192 t: Vec::new(),
193 d: Vec::new(),
194 d_d: Vec::new(),
195 d_dd: Vec::new(),
196 d_ddd: Vec::new(),
197 s: Vec::new(),
198 s_d: Vec::new(),
199 s_dd: Vec::new(),
200 s_ddd: Vec::new(),
201 cd: 0.0,
202 cv: 0.0,
203 cf: 0.0,
204 x: Vec::new(),
205 y: Vec::new(),
206 yaw: Vec::new(),
207 v: Vec::new(),
208 a: Vec::new(),
209 ds: Vec::new(),
210 c: Vec::new(),
211 }
212 }
213
214 fn pop_front(&mut self) {
215 self.x.remove(0);
216 self.y.remove(0);
217 self.yaw.remove(0);
218 self.v.remove(0);
219 self.a.remove(0);
220 self.s.remove(0);
221 self.s_d.remove(0);
222 self.s_dd.remove(0);
223 self.s_ddd.remove(0);
224 self.d.remove(0);
225 self.d_d.remove(0);
226 self.d_dd.remove(0);
227 self.d_ddd.remove(0);
228 }
229}
230
231impl Default for FrenetPath {
232 fn default() -> Self {
233 Self::new()
234 }
235}
236
237pub struct CubicSpline2D {
239 pub s: Vec<f64>,
240 sx: CubicSpline1D,
241 sy: CubicSpline1D,
242}
243
244struct CubicSpline1D {
245 x: Vec<f64>,
246 a: Vec<f64>,
247 b: Vec<f64>,
248 c: Vec<f64>,
249 d: Vec<f64>,
250}
251
252impl CubicSpline1D {
253 fn new(x: &[f64], y: &[f64]) -> Self {
254 let n = x.len();
255 let a = y.to_vec();
256 let mut b = vec![0.0; n];
257 let mut c = vec![0.0; n];
258 let mut d = vec![0.0; n];
259
260 let h: Vec<f64> = (0..n - 1).map(|i| x[i + 1] - x[i]).collect();
261
262 let mut alpha = vec![0.0; n];
263 for i in 1..n - 1 {
264 alpha[i] = 3.0 / h[i] * (a[i + 1] - a[i]) - 3.0 / h[i - 1] * (a[i] - a[i - 1]);
265 }
266
267 let mut l = vec![1.0; n];
268 let mut mu = vec![0.0; n];
269 let mut z = vec![0.0; n];
270
271 for i in 1..n - 1 {
272 l[i] = 2.0 * (x[i + 1] - x[i - 1]) - h[i - 1] * mu[i - 1];
273 mu[i] = h[i] / l[i];
274 z[i] = (alpha[i] - h[i - 1] * z[i - 1]) / l[i];
275 }
276
277 for j in (0..n - 1).rev() {
278 c[j] = z[j] - mu[j] * c[j + 1];
279 b[j] = (a[j + 1] - a[j]) / h[j] - h[j] * (c[j + 1] + 2.0 * c[j]) / 3.0;
280 d[j] = (c[j + 1] - c[j]) / (3.0 * h[j]);
281 }
282
283 CubicSpline1D {
284 x: x.to_vec(),
285 a,
286 b,
287 c,
288 d,
289 }
290 }
291
292 fn calc(&self, t: f64) -> f64 {
293 let i = self.search_index(t);
294 let dx = t - self.x[i];
295 self.a[i] + self.b[i] * dx + self.c[i] * dx.powi(2) + self.d[i] * dx.powi(3)
296 }
297
298 fn calc_d(&self, t: f64) -> f64 {
299 let i = self.search_index(t);
300 let dx = t - self.x[i];
301 self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx.powi(2)
302 }
303
304 fn calc_dd(&self, t: f64) -> f64 {
305 let i = self.search_index(t);
306 let dx = t - self.x[i];
307 2.0 * self.c[i] + 6.0 * self.d[i] * dx
308 }
309
310 fn calc_ddd(&self, t: f64) -> f64 {
311 let i = self.search_index(t);
312 6.0 * self.d[i]
313 }
314
315 fn search_index(&self, t: f64) -> usize {
316 for i in 0..self.x.len() - 1 {
317 if self.x[i] <= t && t < self.x[i + 1] {
318 return i;
319 }
320 }
321 self.x.len() - 2
322 }
323}
324
325impl CubicSpline2D {
326 pub fn new(x: &[f64], y: &[f64]) -> Self {
327 let mut s = vec![0.0];
328 for i in 1..x.len() {
329 let ds = ((x[i] - x[i - 1]).powi(2) + (y[i] - y[i - 1]).powi(2)).sqrt();
330 s.push(s[i - 1] + ds);
331 }
332
333 let sx = CubicSpline1D::new(&s, x);
334 let sy = CubicSpline1D::new(&s, y);
335
336 CubicSpline2D { s, sx, sy }
337 }
338
339 pub fn calc_position(&self, s: f64) -> (f64, f64) {
340 (self.sx.calc(s), self.sy.calc(s))
341 }
342
343 pub fn calc_curvature(&self, s: f64) -> f64 {
344 let dx = self.sx.calc_d(s);
345 let ddx = self.sx.calc_dd(s);
346 let dy = self.sy.calc_d(s);
347 let ddy = self.sy.calc_dd(s);
348 (ddy * dx - ddx * dy) / (dx.powi(2) + dy.powi(2)).powf(1.5)
349 }
350
351 pub fn calc_yaw(&self, s: f64) -> f64 {
352 let dx = self.sx.calc_d(s);
353 let dy = self.sy.calc_d(s);
354 dy.atan2(dx)
355 }
356
357 pub fn calc_curvature_rate(&self, s: f64) -> f64 {
358 let dx = self.sx.calc_d(s);
359 let dy = self.sy.calc_d(s);
360 let ddx = self.sx.calc_dd(s);
361 let ddy = self.sy.calc_dd(s);
362 let dddx = self.sx.calc_ddd(s);
363 let dddy = self.sy.calc_ddd(s);
364 let a = dx * ddy - dy * ddx;
365 let b = dx * dddy - dy * dddx;
366 let c = dx * ddx + dy * ddy;
367 let d = dx * dx + dy * dy;
368 (b * d - 3.0 * a * c) / (d * d * d)
369 }
370}
371
372#[derive(Clone, Copy, Debug, PartialEq, Eq)]
373enum PathCheckStatus {
374 MaxSpeedError,
375 MaxAccelError,
376 MaxCurvatureError,
377 CollisionError,
378 Ok,
379}
380
381#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)]
382struct PathCheckCounts {
383 max_speed_error: usize,
384 max_accel_error: usize,
385 max_curvature_error: usize,
386 collision_error: usize,
387 ok: usize,
388}
389
390#[derive(Clone, Copy, Debug)]
391struct ReferencePathState {
392 s: f64,
393 x: f64,
394 y: f64,
395 theta: f64,
396 kappa: f64,
397 dkappa: f64,
398}
399
400#[derive(Clone, Copy, Debug)]
401pub struct FrenetInitialState {
402 pub s0: f64,
403 pub speed: f64,
404 pub accel: f64,
405 pub d: f64,
406 pub d_d: f64,
407 pub d_dd: f64,
408}
409
410#[derive(Clone, Copy, Debug)]
411struct FrenetPlannerConfig {
412 max_road_width: f64,
413 d_road_w: f64,
414 dt: f64,
415 max_t: f64,
416 min_t: f64,
417 target_speed: f64,
418 d_t_s: f64,
419 n_s_sample: usize,
420 robot_radius: f64,
421 stop_s: f64,
422 d_s: f64,
423 n_stop_s_sample: usize,
424}
425
426impl FrenetPlannerConfig {
427 fn high_speed() -> Self {
428 Self {
429 max_road_width: MAX_ROAD_WIDTH,
430 d_road_w: D_ROAD_W,
431 dt: DT,
432 max_t: MAX_T,
433 min_t: MIN_T,
434 target_speed: TARGET_SPEED,
435 d_t_s: D_T_S,
436 n_s_sample: N_S_SAMPLE,
437 robot_radius: ROBOT_RADIUS,
438 stop_s: 25.0,
439 d_s: 2.0,
440 n_stop_s_sample: 4,
441 }
442 }
443
444 fn low_speed() -> Self {
445 Self {
446 max_road_width: 1.0,
447 d_road_w: 0.2,
448 dt: DT,
449 max_t: MAX_T,
450 min_t: MIN_T,
451 target_speed: 3.0 / 3.6,
452 d_t_s: 0.5 / 3.6,
453 n_s_sample: N_S_SAMPLE,
454 robot_radius: 0.5,
455 stop_s: 4.0,
456 d_s: 0.3,
457 n_stop_s_sample: 3,
458 }
459 }
460}
461
462#[derive(Clone, Copy, Debug)]
463enum LateralStrategyKind {
464 HighSpeed,
465 LowSpeed,
466}
467
468#[derive(Clone, Copy, Debug)]
469enum LongitudinalStrategyKind {
470 VelocityKeeping,
471 MergingAndStopping,
472}
473
474fn arange(start: f64, stop: f64, step: f64) -> Vec<f64> {
475 debug_assert!(step > 0.0);
476 if stop <= start {
477 return Vec::new();
478 }
479
480 let count = ((stop - start) / step).ceil() as usize;
481 (0..count).map(|i| start + step * i as f64).collect()
482}
483
484fn accumulating_arange(start: f64, stop: f64, step: f64) -> Vec<f64> {
485 debug_assert!(step > 0.0);
486 let mut values = Vec::new();
487 let mut current = start;
488
489 while current < stop {
490 values.push(current);
491 current += step;
492 }
493
494 values
495}
496
497fn target_speed_samples(config: FrenetPlannerConfig) -> Vec<f64> {
498 arange(
499 config.target_speed - config.d_t_s * config.n_s_sample as f64,
500 config.target_speed + config.d_t_s * config.n_s_sample as f64,
501 config.d_t_s,
502 )
503}
504
505fn stop_position_samples(config: FrenetPlannerConfig) -> Vec<f64> {
506 accumulating_arange(
507 config.stop_s - config.d_s * config.n_stop_s_sample as f64,
508 config.stop_s + config.d_s * config.n_stop_s_sample as f64,
509 config.d_s,
510 )
511}
512
513fn road_width_samples(
514 config: FrenetPlannerConfig,
515 s0: f64,
516 longitudinal: LongitudinalStrategyKind,
517) -> Vec<f64> {
518 match longitudinal {
519 LongitudinalStrategyKind::VelocityKeeping => accumulating_arange(
520 -config.max_road_width,
521 config.max_road_width,
522 config.d_road_w,
523 ),
524 LongitudinalStrategyKind::MergingAndStopping => {
525 if s0 < config.stop_s / 3.0 {
526 accumulating_arange(
527 -config.max_road_width,
528 config.max_road_width,
529 config.d_road_w,
530 )
531 } else {
532 vec![0.0]
533 }
534 }
535 }
536}
537
538fn frenet_to_cartesian(
539 reference: ReferencePathState,
540 s_condition: [f64; 3],
541 d_condition: [f64; 3],
542) -> (f64, f64, f64, f64, f64, f64) {
543 debug_assert!((reference.s - s_condition[0]).abs() < 1e-6);
544
545 let cos_theta_r = reference.theta.cos();
546 let sin_theta_r = reference.theta.sin();
547
548 let x = reference.x - sin_theta_r * d_condition[0];
549 let y = reference.y + cos_theta_r * d_condition[0];
550
551 let one_minus_kappa_r_d = 1.0 - reference.kappa * d_condition[0];
552 let tan_delta_theta = d_condition[1] / one_minus_kappa_r_d;
553 let delta_theta = d_condition[1].atan2(one_minus_kappa_r_d);
554 let cos_delta_theta = delta_theta.cos();
555
556 let theta = normalize_angle(delta_theta + reference.theta);
557 let kappa_r_d_prime = reference.dkappa * d_condition[0] + reference.kappa * d_condition[1];
558 let kappa = (((d_condition[2] + kappa_r_d_prime * tan_delta_theta) * cos_delta_theta.powi(2))
559 / one_minus_kappa_r_d
560 + reference.kappa)
561 * cos_delta_theta
562 / one_minus_kappa_r_d;
563
564 let d_dot = d_condition[1] * s_condition[1];
565 let v = ((one_minus_kappa_r_d * s_condition[1]).powi(2) + d_dot.powi(2)).sqrt();
566
567 let delta_theta_prime = one_minus_kappa_r_d / cos_delta_theta * kappa - reference.kappa;
568 let a = s_condition[2] * one_minus_kappa_r_d / cos_delta_theta
569 + s_condition[1].powi(2) / cos_delta_theta
570 * (d_condition[1] * delta_theta_prime - kappa_r_d_prime);
571
572 (x, y, theta, kappa, v, a)
573}
574
575fn build_velocity_keeping_path(
576 state: FrenetInitialState,
577 ti: f64,
578 target_speed: f64,
579 config: FrenetPlannerConfig,
580) -> FrenetPath {
581 let lon_qp = QuarticPolynomial::new(state.s0, state.speed, state.accel, target_speed, 0.0, ti);
582 let mut fp = FrenetPath::new();
583 fp.t = arange(0.0, ti, config.dt);
584
585 for &t in &fp.t {
586 fp.s.push(lon_qp.calc_point(t));
587 fp.s_d.push(lon_qp.calc_first_derivative(t));
588 fp.s_dd.push(lon_qp.calc_second_derivative(t));
589 fp.s_ddd.push(lon_qp.calc_third_derivative(t));
590 }
591
592 fp
593}
594
595fn build_merging_stop_path(
596 state: FrenetInitialState,
597 ti: f64,
598 stop_position: f64,
599 config: FrenetPlannerConfig,
600) -> FrenetPath {
601 let lon_qp = QuinticPolynomial::new(
602 state.s0,
603 state.speed,
604 state.accel,
605 stop_position,
606 0.0,
607 0.0,
608 ti,
609 );
610 let mut fp = FrenetPath::new();
611 fp.t = arange(0.0, ti, config.dt);
612
613 for &t in &fp.t {
614 fp.s.push(lon_qp.calc_point(t));
615 fp.s_d.push(lon_qp.calc_first_derivative(t));
616 fp.s_dd.push(lon_qp.calc_second_derivative(t));
617 fp.s_ddd.push(lon_qp.calc_third_derivative(t));
618 }
619
620 fp
621}
622
623fn calc_longitudinal_candidates(
624 state: FrenetInitialState,
625 ti: f64,
626 config: FrenetPlannerConfig,
627 longitudinal: LongitudinalStrategyKind,
628) -> Vec<FrenetPath> {
629 match longitudinal {
630 LongitudinalStrategyKind::VelocityKeeping => target_speed_samples(config)
631 .into_iter()
632 .map(|target_speed| build_velocity_keeping_path(state, ti, target_speed, config))
633 .collect(),
634 LongitudinalStrategyKind::MergingAndStopping => {
635 if state.s0 >= config.stop_s {
636 return Vec::new();
637 }
638
639 stop_position_samples(config)
640 .into_iter()
641 .map(|stop_position| build_merging_stop_path(state, ti, stop_position, config))
642 .collect()
643 }
644 }
645}
646
647fn apply_lateral_strategy(
648 fp: &FrenetPath,
649 di: f64,
650 state: FrenetInitialState,
651 ti: f64,
652 lateral: LateralStrategyKind,
653) -> FrenetPath {
654 let mut tp = fp.clone();
655 tp.d.clear();
656 tp.d_d.clear();
657 tp.d_dd.clear();
658 tp.d_ddd.clear();
659
660 match lateral {
661 LateralStrategyKind::HighSpeed => {
662 let s0_d = fp.s_d[0];
663 let s0_dd = fp.s_dd[0];
664 let lat_qp = QuinticPolynomial::new(
665 state.d,
666 state.d_d * s0_d,
667 state.d_dd * s0_d.powi(2) + state.d_d * s0_dd,
668 di,
669 0.0,
670 0.0,
671 ti,
672 );
673
674 for i in 0..tp.t.len() {
675 let t = tp.t[i];
676 let s_d = tp.s_d[i];
677 let s_dd = tp.s_dd[i];
678 let s_d_inv = 1.0 / (s_d + 1e-6) + 1e-6;
679 let s_d_inv_sq = s_d_inv * s_d_inv;
680
681 let d = lat_qp.calc_point(t);
682 let d_d_t = lat_qp.calc_first_derivative(t);
683 let d_dd_t = lat_qp.calc_second_derivative(t);
684 let d_ddd_t = lat_qp.calc_third_derivative(t);
685 let d_d = d_d_t * s_d_inv;
686
687 tp.d.push(d);
688 tp.d_d.push(d_d);
689 tp.d_dd.push((d_dd_t - d_d * s_dd) * s_d_inv_sq);
690 tp.d_ddd.push(d_ddd_t);
691 }
692 }
693 LateralStrategyKind::LowSpeed => {
694 let s0 = fp.s[0];
695 let s1 = *fp.s.last().unwrap_or(&s0);
696 let lat_qp =
697 QuinticPolynomial::new(state.d, state.d_d, state.d_dd, di, 0.0, 0.0, s1 - s0);
698
699 for &s in &fp.s {
700 let shifted_s = s - s0;
701 tp.d.push(lat_qp.calc_point(shifted_s));
702 tp.d_d.push(lat_qp.calc_first_derivative(shifted_s));
703 tp.d_dd.push(lat_qp.calc_second_derivative(shifted_s));
704 tp.d_ddd.push(lat_qp.calc_third_derivative(shifted_s));
705 }
706 }
707 }
708
709 tp
710}
711
712fn destination_cost(
713 fp: &FrenetPath,
714 config: FrenetPlannerConfig,
715 longitudinal: LongitudinalStrategyKind,
716) -> f64 {
717 match longitudinal {
718 LongitudinalStrategyKind::VelocityKeeping => {
719 let speed_error = (config.target_speed - fp.s_d.last().unwrap_or(&0.0)).powi(2);
720 K_S_DOT * speed_error
721 }
722 LongitudinalStrategyKind::MergingAndStopping => {
723 let stop_error = (config.stop_s - fp.s.last().unwrap_or(&0.0)).powi(2);
724 K_S * stop_error
725 }
726 }
727}
728
729fn classify_path_with_config(
730 fp: &FrenetPath,
731 ob: &[(f64, f64)],
732 config: FrenetPlannerConfig,
733) -> PathCheckStatus {
734 if fp.v.iter().any(|&v| v > MAX_SPEED) {
735 return PathCheckStatus::MaxSpeedError;
736 }
737
738 if fp.a.iter().any(|&a| a.abs() > MAX_ACCEL) {
739 return PathCheckStatus::MaxAccelError;
740 }
741
742 if fp.c.iter().any(|&c| c.abs() > MAX_CURVATURE) {
743 return PathCheckStatus::MaxCurvatureError;
744 }
745
746 for (ox, oy) in ob {
747 for (&x, &y) in fp.x.iter().zip(fp.y.iter()) {
748 let distance_sq = (x - ox).powi(2) + (y - oy).powi(2);
749 if distance_sq <= config.robot_radius.powi(2) {
750 return PathCheckStatus::CollisionError;
751 }
752 }
753 }
754
755 PathCheckStatus::Ok
756}
757
758fn classify_path(fp: &FrenetPath, ob: &[(f64, f64)]) -> PathCheckStatus {
759 classify_path_with_config(fp, ob, FrenetPlannerConfig::high_speed())
760}
761
762fn summarize_paths_with_config(
763 fplist: &[FrenetPath],
764 ob: &[(f64, f64)],
765 config: FrenetPlannerConfig,
766) -> PathCheckCounts {
767 let mut counts = PathCheckCounts::default();
768
769 for fp in fplist {
770 match classify_path_with_config(fp, ob, config) {
771 PathCheckStatus::MaxSpeedError => counts.max_speed_error += 1,
772 PathCheckStatus::MaxAccelError => counts.max_accel_error += 1,
773 PathCheckStatus::MaxCurvatureError => counts.max_curvature_error += 1,
774 PathCheckStatus::CollisionError => counts.collision_error += 1,
775 PathCheckStatus::Ok => counts.ok += 1,
776 }
777 }
778
779 counts
780}
781
782fn summarize_paths(fplist: &[FrenetPath], ob: &[(f64, f64)]) -> PathCheckCounts {
783 summarize_paths_with_config(fplist, ob, FrenetPlannerConfig::high_speed())
784}
785
786pub fn calc_frenet_paths(
788 c_speed: f64,
789 c_d: f64,
790 c_d_d: f64,
791 c_d_dd: f64,
792 s0: f64,
793) -> Vec<FrenetPath> {
794 calc_frenet_paths_with_accel(c_speed, 0.0, c_d, c_d_d, c_d_dd, s0)
795}
796
797pub fn calc_frenet_paths_with_accel(
798 c_speed: f64,
799 c_accel: f64,
800 c_d: f64,
801 c_d_d: f64,
802 c_d_dd: f64,
803 s0: f64,
804) -> Vec<FrenetPath> {
805 calc_frenet_paths_for_strategy(
806 FrenetInitialState {
807 s0,
808 speed: c_speed,
809 accel: c_accel,
810 d: c_d,
811 d_d: c_d_d,
812 d_dd: c_d_dd,
813 },
814 FrenetPlannerConfig::high_speed(),
815 LateralStrategyKind::HighSpeed,
816 LongitudinalStrategyKind::VelocityKeeping,
817 )
818}
819
820fn calc_frenet_paths_for_strategy(
821 state: FrenetInitialState,
822 config: FrenetPlannerConfig,
823 lateral: LateralStrategyKind,
824 longitudinal: LongitudinalStrategyKind,
825) -> Vec<FrenetPath> {
826 let mut fplist = Vec::new();
827
828 for ti in accumulating_arange(config.min_t, config.max_t, config.dt) {
829 for fp in calc_longitudinal_candidates(state, ti, config, longitudinal) {
830 for di in road_width_samples(config, state.s0, longitudinal) {
831 let mut tp = apply_lateral_strategy(&fp, di, state, ti, lateral);
832 let jp: f64 = tp.d_ddd.iter().map(|x| x.powi(2)).sum();
833 let js: f64 = tp.s_ddd.iter().map(|x| x.powi(2)).sum();
834
835 tp.cd = K_J * jp + K_T * ti + K_D * tp.d.last().unwrap_or(&0.0).powi(2);
836 tp.cv = K_J * js + K_T * ti + destination_cost(&tp, config, longitudinal);
837 tp.cf = K_LAT * tp.cd + K_LON * tp.cv;
838 fplist.push(tp);
839 }
840 }
841 }
842
843 fplist
844}
845
846pub fn calc_global_paths(fplist: &mut [FrenetPath], csp: &CubicSpline2D) {
848 for fp in fplist.iter_mut() {
849 fp.x.clear();
850 fp.y.clear();
851 fp.yaw.clear();
852 fp.v.clear();
853 fp.a.clear();
854 fp.ds.clear();
855 fp.c.clear();
856
857 for i in 0..fp.s.len() {
858 let s = fp.s[i];
859 if s > *csp.s.last().unwrap_or(&0.0) {
860 break;
861 }
862
863 let (ix, iy) = csp.calc_position(s);
864 let i_yaw = csp.calc_yaw(s);
865 let i_kappa = csp.calc_curvature(s);
866 let i_dkappa = csp.calc_curvature_rate(s);
867 let reference = ReferencePathState {
868 s: fp.s[i],
869 x: ix,
870 y: iy,
871 theta: i_yaw,
872 kappa: i_kappa,
873 dkappa: i_dkappa,
874 };
875 let s_condition = [fp.s[i], fp.s_d[i], fp.s_dd[i]];
876 let d_condition = [fp.d[i], fp.d_d[i], fp.d_dd[i]];
877 let (x, y, yaw, kappa, v, a) = frenet_to_cartesian(reference, s_condition, d_condition);
878 fp.x.push(x);
879 fp.y.push(y);
880 fp.yaw.push(yaw);
881 fp.v.push(v);
882 fp.a.push(a);
883 fp.c.push(kappa);
884 }
885
886 for i in 0..fp.x.len().saturating_sub(1) {
887 let dx = fp.x[i + 1] - fp.x[i];
888 let dy = fp.y[i + 1] - fp.y[i];
889 fp.ds.push((dx.powi(2) + dy.powi(2)).sqrt());
890 }
891
892 if let Some(last_ds) = fp.ds.last().copied() {
893 fp.ds.push(last_ds);
894 }
895 }
896}
897
898pub fn check_paths(fplist: &mut Vec<FrenetPath>, ob: &[(f64, f64)]) {
900 fplist.retain(|fp| classify_path(fp, ob) == PathCheckStatus::Ok);
901}
902
903fn check_paths_with_config(
904 fplist: &mut Vec<FrenetPath>,
905 ob: &[(f64, f64)],
906 config: FrenetPlannerConfig,
907) {
908 fplist.retain(|fp| classify_path_with_config(fp, ob, config) == PathCheckStatus::Ok);
909}
910
911pub fn frenet_optimal_planning(
913 csp: &CubicSpline2D,
914 s0: f64,
915 c_speed: f64,
916 c_d: f64,
917 c_d_d: f64,
918 c_d_dd: f64,
919 ob: &[(f64, f64)],
920) -> Option<FrenetPath> {
921 frenet_optimal_planning_with_accel(
922 csp,
923 FrenetInitialState {
924 s0,
925 speed: c_speed,
926 accel: 0.0,
927 d: c_d,
928 d_d: c_d_d,
929 d_dd: c_d_dd,
930 },
931 ob,
932 )
933}
934
935pub fn frenet_optimal_planning_with_accel(
936 csp: &CubicSpline2D,
937 state: FrenetInitialState,
938 ob: &[(f64, f64)],
939) -> Option<FrenetPath> {
940 frenet_optimal_planning_for_strategy(
941 csp,
942 state,
943 ob,
944 FrenetPlannerConfig::high_speed(),
945 LateralStrategyKind::HighSpeed,
946 LongitudinalStrategyKind::VelocityKeeping,
947 )
948}
949
950fn frenet_optimal_planning_for_strategy(
951 csp: &CubicSpline2D,
952 state: FrenetInitialState,
953 ob: &[(f64, f64)],
954 config: FrenetPlannerConfig,
955 lateral: LateralStrategyKind,
956 longitudinal: LongitudinalStrategyKind,
957) -> Option<FrenetPath> {
958 let mut fplist = calc_frenet_paths_for_strategy(state, config, lateral, longitudinal);
959 calc_global_paths(&mut fplist, csp);
960 check_paths_with_config(&mut fplist, ob, config);
961
962 fplist
963 .into_iter()
964 .min_by(|a, b| a.cf.partial_cmp(&b.cf).unwrap())
965}
966
967#[cfg(test)]
968mod tests {
969 use super::*;
970
971 const UPSTREAM_WX: [f64; 5] = [0.0, 10.0, 20.5, 35.0, 70.5];
972 const UPSTREAM_WY: [f64; 5] = [0.0, -6.0, 5.0, 6.5, 0.0];
973 const UPSTREAM_OBSTACLES: [(f64, f64); 5] = [
974 (20.0, 10.0),
975 (30.0, 6.0),
976 (30.0, 8.0),
977 (35.0, 8.0),
978 (50.0, 3.0),
979 ];
980 const LOW_SPEED_WX: [f64; 6] = [0.0, 2.0, 4.0, 6.0, 8.0, 10.0];
981 const LOW_SPEED_WY: [f64; 6] = [0.0, 0.0, 1.0, 0.0, -1.0, -2.0];
982 const LOW_SPEED_OBSTACLES: [(f64, f64); 4] = [(3.0, 1.0), (5.0, 0.0), (6.0, 0.5), (8.0, -1.5)];
983 const LOW_SPEED_STATE: FrenetInitialState = FrenetInitialState {
984 s0: 0.0,
985 speed: 1.0 / 3.6,
986 accel: 0.0,
987 d: 0.5,
988 d_d: 0.0,
989 d_dd: 0.0,
990 };
991
992 fn assert_close(actual: f64, expected: f64) {
993 assert!(
994 (actual - expected).abs() < 1e-6,
995 "actual={actual}, expected={expected}"
996 );
997 }
998
999 #[derive(Clone, Copy, Debug)]
1000 struct SimulationSnapshot {
1001 fallback: bool,
1002 cf: f64,
1003 len: usize,
1004 x1: f64,
1005 y1: f64,
1006 yaw1: f64,
1007 v1: f64,
1008 a1: f64,
1009 c1: f64,
1010 s1: f64,
1011 d1: f64,
1012 s_d1: f64,
1013 s_dd1: f64,
1014 d_d1: f64,
1015 d_dd1: f64,
1016 counts: PathCheckCounts,
1017 }
1018
1019 #[derive(Clone, Copy, Debug, PartialEq, Eq)]
1020 enum SimulationOutcomeKind {
1021 Goal,
1022 Finish,
1023 }
1024
1025 fn sampled_course_goal(csp: &CubicSpline2D) -> (f64, f64) {
1026 let mut goal = csp.calc_position(0.0);
1027 for s in arange(0.0, *csp.s.last().unwrap_or(&0.0), 0.1) {
1028 goal = csp.calc_position(s);
1029 }
1030 goal
1031 }
1032
1033 fn run_simulation_for_strategy(
1034 csp: &CubicSpline2D,
1035 initial_state: FrenetInitialState,
1036 obstacles: &[(f64, f64)],
1037 config: FrenetPlannerConfig,
1038 lateral: LateralStrategyKind,
1039 longitudinal: LongitudinalStrategyKind,
1040 max_steps: usize,
1041 ) -> (Vec<SimulationSnapshot>, SimulationOutcomeKind, usize) {
1042 let goal = sampled_course_goal(csp);
1043 let mut state = initial_state;
1044 let mut last_path: Option<FrenetPath> = None;
1045 let mut trace = Vec::new();
1046
1047 for step in 0..max_steps {
1048 let mut candidates =
1049 calc_frenet_paths_for_strategy(state, config, lateral, longitudinal);
1050 calc_global_paths(&mut candidates, csp);
1051 let counts = summarize_paths_with_config(&candidates, obstacles, config);
1052 check_paths_with_config(&mut candidates, obstacles, config);
1053
1054 let mut fallback = false;
1055 let path = if let Some(path) = candidates
1056 .into_iter()
1057 .min_by(|a, b| a.cf.partial_cmp(&b.cf).unwrap())
1058 {
1059 path
1060 } else {
1061 let mut path = last_path.clone().expect("expected fallback path");
1062 path.pop_front();
1063 fallback = true;
1064 path
1065 };
1066
1067 if path.x.len() <= 1 {
1068 return (trace, SimulationOutcomeKind::Finish, step);
1069 }
1070
1071 last_path = Some(path.clone());
1072 trace.push(SimulationSnapshot {
1073 fallback,
1074 cf: path.cf,
1075 len: path.x.len(),
1076 x1: path.x[1],
1077 y1: path.y[1],
1078 yaw1: path.yaw[1],
1079 v1: path.v[1],
1080 a1: path.a[1],
1081 c1: path.c[1],
1082 s1: path.s[1],
1083 d1: path.d[1],
1084 s_d1: path.s_d[1],
1085 s_dd1: path.s_dd[1],
1086 d_d1: path.d_d[1],
1087 d_dd1: path.d_dd[1],
1088 counts,
1089 });
1090
1091 state = FrenetInitialState {
1092 s0: path.s[1],
1093 speed: path.s_d[1],
1094 accel: path.s_dd[1],
1095 d: path.d[1],
1096 d_d: path.d_d[1],
1097 d_dd: path.d_dd[1],
1098 };
1099
1100 if ((path.x[1] - goal.0).powi(2) + (path.y[1] - goal.1).powi(2)).sqrt() <= 1.0 {
1101 return (trace, SimulationOutcomeKind::Goal, step);
1102 }
1103 }
1104
1105 panic!("simulation did not terminate within {max_steps} steps");
1106 }
1107
1108 fn assert_snapshot_matches(actual: SimulationSnapshot, expected: SimulationSnapshot) {
1109 assert_eq!(actual.fallback, expected.fallback);
1110 assert_eq!(actual.len, expected.len);
1111 assert_eq!(actual.counts, expected.counts);
1112 assert_close(actual.cf, expected.cf);
1113 assert_close(actual.x1, expected.x1);
1114 assert_close(actual.y1, expected.y1);
1115 assert_close(actual.yaw1, expected.yaw1);
1116 assert_close(actual.v1, expected.v1);
1117 assert_close(actual.a1, expected.a1);
1118 assert_close(actual.c1, expected.c1);
1119 assert_close(actual.s1, expected.s1);
1120 assert_close(actual.d1, expected.d1);
1121 assert_close(actual.s_d1, expected.s_d1);
1122 assert_close(actual.s_dd1, expected.s_dd1);
1123 assert_close(actual.d_d1, expected.d_d1);
1124 assert_close(actual.d_dd1, expected.d_dd1);
1125 }
1126
1127 #[test]
1128 fn test_cubic_spline_2d() {
1129 let wx = UPSTREAM_WX.to_vec();
1130 let wy = UPSTREAM_WY.to_vec();
1131 let csp = CubicSpline2D::new(&wx, &wy);
1132 assert!(!csp.s.is_empty());
1133
1134 let (x, y) = csp.calc_position(0.0);
1135 assert!((x - 0.0).abs() < 1e-6);
1136 assert!((y - 0.0).abs() < 1e-6);
1137 }
1138
1139 #[test]
1140 fn test_frenet_path_creation() {
1141 let fp = FrenetPath::new();
1142 assert!(fp.t.is_empty());
1143 assert_eq!(fp.cf, 0.0);
1144 }
1145
1146 #[test]
1147 fn test_calc_frenet_paths_matches_upstream_initial_candidate_grid() {
1148 let paths = calc_frenet_paths_with_accel(10.0 / 3.6, 0.0, 2.0, 0.0, 0.0, 0.0);
1149 assert_eq!(paths.len(), 210);
1150
1151 let reference = [
1152 (
1153 0usize,
1154 20usize,
1155 3.8000000000000003,
1156 -6.989576874999997,
1157 -0.022026714671375677,
1158 0.030317350631561317,
1159 18.05758680555556,
1160 6.9142361111111095,
1161 0.2968749999999991,
1162 82.49117269655986,
1163 ),
1164 (
1165 105usize,
1166 22usize,
1167 4.2,
1168 0.0017525504343343101,
1169 -0.0030929646585680258,
1170 0.003563824972365069,
1171 22.780021287252694,
1172 8.299941564404374,
1173 0.32870022539443955,
1174 3.955233569168209,
1175 ),
1176 (
1177 209usize,
1178 25usize,
1179 4.800000000000001,
1180 6.0000000000000036,
1181 1.8271274675916536e-15,
1182 7.517396749979645e-17,
1183 30.000000000000014,
1184 9.722222222222223,
1185 0.0,
1186 44.61225288072495,
1187 ),
1188 ];
1189
1190 for (index, t_len, t_last, d_last, d_d_last, d_dd_last, s_last, s_d_last, s_dd_last, cf) in
1191 reference
1192 {
1193 let path = &paths[index];
1194 assert_eq!(path.t.len(), t_len);
1195 assert_close(*path.t.last().unwrap(), t_last);
1196 assert_close(*path.d.last().unwrap(), d_last);
1197 assert_close(*path.d_d.last().unwrap(), d_d_last);
1198 assert_close(*path.d_dd.last().unwrap(), d_dd_last);
1199 assert_close(*path.s.last().unwrap(), s_last);
1200 assert_close(*path.s_d.last().unwrap(), s_d_last);
1201 assert_close(*path.s_dd.last().unwrap(), s_dd_last);
1202 assert_close(path.cf, cf);
1203 }
1204 }
1205
1206 #[test]
1207 fn test_frenet_optimal_planning_matches_upstream_initial_window() {
1208 let csp = CubicSpline2D::new(&UPSTREAM_WX, &UPSTREAM_WY);
1209 let mut all_paths = calc_frenet_paths_with_accel(10.0 / 3.6, 0.0, 2.0, 0.0, 0.0, 0.0);
1210 calc_global_paths(&mut all_paths, &csp);
1211 let counts = summarize_paths(&all_paths, &UPSTREAM_OBSTACLES);
1212 assert_eq!(
1213 counts,
1214 PathCheckCounts {
1215 max_speed_error: 30,
1216 max_accel_error: 150,
1217 max_curvature_error: 0,
1218 collision_error: 0,
1219 ok: 30,
1220 }
1221 );
1222
1223 let best = frenet_optimal_planning_with_accel(
1224 &csp,
1225 FrenetInitialState {
1226 s0: 0.0,
1227 speed: 10.0 / 3.6,
1228 accel: 0.0,
1229 d: 2.0,
1230 d_d: 0.0,
1231 d_dd: 0.0,
1232 },
1233 &UPSTREAM_OBSTACLES,
1234 )
1235 .expect("expected a valid path");
1236
1237 assert_eq!(best.x.len(), 25);
1238 assert_close(best.cf, 3.542294469271419);
1239
1240 let reference = [
1241 (
1242 0usize,
1243 1.3527664541170894,
1244 1.4730997660089002,
1245 -0.7428410682357911,
1246 2.777777777777779,
1247 -0.07124228337505333,
1248 -1.70188803340216e-16,
1249 0.0,
1250 2.0,
1251 2.7777777777777777,
1252 0.0,
1253 0.0,
1254 0.0,
1255 ),
1256 (
1257 1usize,
1258 1.8605645611714472,
1259 1.005284701078841,
1260 -0.7488798286101788,
1261 2.787960815155609,
1262 0.2033476908615227,
1263 -0.021106708455623728,
1264 0.557444380144033,
1265 1.9986421561535495,
1266 2.80590920781893,
1267 0.27729552469135804,
1268 -0.007103103907306435,
1269 -0.02396404262355845,
1270 ),
1271 (
1272 2usize,
1273 2.3695265289095406,
1274 0.5284173023840572,
1275 -0.7638860397722307,
1276 2.8502342514931054,
1277 0.4520558979283277,
1278 -0.03273816922509124,
1279 1.125900205761317,
1280 1.9898244598765433,
1281 2.8870884773662553,
1282 0.5304783950617284,
1283 -0.025264540804632068,
1284 -0.03816888277632631,
1285 ),
1286 (
1287 12usize,
1288 8.344424474847328,
1289 -4.899558751414678,
1290 -0.45189185643671337,
1291 4.679227014638239,
1292 0.1761834255770116,
1293 0.20268006969898653,
1294 9.16666666666667,
1295 1.0000000000000047,
1296 5.555555555555556,
1297 1.7361111111111112,
1298 -0.14062575593750407,
1299 0.007910283815139495,
1300 ),
1301 (
1302 24usize,
1303 20.33573711360301,
1304 4.87317525607841,
1305 0.6678035273152476,
1306 8.333333333333332,
1307 -1.1131604943389124e-14,
1308 -0.09773284704041481,
1309 26.66666666666667,
1310 -1.7763568394002505e-15,
1311 8.333333333333334,
1312 -8.881784197001252e-16,
1313 -1.0658228575266703e-15,
1314 -5.1159917351284273e-17,
1315 ),
1316 ];
1317
1318 for (index, x, y, yaw, v, a, c, s, d, s_d, s_dd, d_d, d_dd) in reference {
1319 assert_close(best.x[index], x);
1320 assert_close(best.y[index], y);
1321 assert_close(best.yaw[index], yaw);
1322 assert_close(best.v[index], v);
1323 assert_close(best.a[index], a);
1324 assert_close(best.c[index], c);
1325 assert_close(best.s[index], s);
1326 assert_close(best.d[index], d);
1327 assert_close(best.s_d[index], s_d);
1328 assert_close(best.s_dd[index], s_dd);
1329 assert_close(best.d_d[index], d_d);
1330 assert_close(best.d_dd[index], d_dd);
1331 }
1332 }
1333
1334 #[test]
1335 fn test_calc_low_speed_velocity_candidates_match_upstream_reference() {
1336 let paths = calc_frenet_paths_for_strategy(
1337 LOW_SPEED_STATE,
1338 FrenetPlannerConfig::low_speed(),
1339 LateralStrategyKind::LowSpeed,
1340 LongitudinalStrategyKind::VelocityKeeping,
1341 );
1342 assert_eq!(paths.len(), 100);
1343
1344 let reference = [
1345 (
1346 0usize,
1347 20usize,
1348 3.8000000000000003,
1349 -0.9999999999999947,
1350 1.4210854715202004e-14,
1351 7.105427357601002e-15,
1352 1.8057586805555559,
1353 0.6914236111111112,
1354 0.029687499999999978,
1355 122.17132690502265,
1356 ),
1357 (
1358 50usize,
1359 22usize,
1360 4.2,
1361 -0.9999999999999964,
1362 0.0,
1363 -7.105427357601002e-15,
1364 2.2780021287252694,
1365 0.8299941564404372,
1366 0.032870022539444155,
1367 35.03551917278308,
1368 ),
1369 (
1370 99usize,
1371 25usize,
1372 4.800000000000001,
1373 0.7999999999999992,
1374 4.440892098500626e-16,
1375 0.0,
1376 2.666666666666667,
1377 0.8333333333333333,
1378 0.0,
1379 2.1965106963760093,
1380 ),
1381 ];
1382
1383 for (index, t_len, t_last, d_last, d_d_last, d_dd_last, s_last, s_d_last, s_dd_last, cf) in
1384 reference
1385 {
1386 let path = &paths[index];
1387 assert_eq!(path.t.len(), t_len);
1388 assert_close(*path.t.last().unwrap(), t_last);
1389 assert_close(*path.d.last().unwrap(), d_last);
1390 assert_close(*path.d_d.last().unwrap(), d_d_last);
1391 assert_close(*path.d_dd.last().unwrap(), d_dd_last);
1392 assert_close(*path.s.last().unwrap(), s_last);
1393 assert_close(*path.s_d.last().unwrap(), s_d_last);
1394 assert_close(*path.s_dd.last().unwrap(), s_dd_last);
1395 assert_close(path.cf, cf);
1396 }
1397 }
1398
1399 #[test]
1400 fn test_low_speed_velocity_planning_matches_upstream_initial_window() {
1401 let csp = CubicSpline2D::new(&LOW_SPEED_WX, &LOW_SPEED_WY);
1402 let mut all_paths = calc_frenet_paths_for_strategy(
1403 LOW_SPEED_STATE,
1404 FrenetPlannerConfig::low_speed(),
1405 LateralStrategyKind::LowSpeed,
1406 LongitudinalStrategyKind::VelocityKeeping,
1407 );
1408 calc_global_paths(&mut all_paths, &csp);
1409 let counts = summarize_paths_with_config(
1410 &all_paths,
1411 &LOW_SPEED_OBSTACLES,
1412 FrenetPlannerConfig::low_speed(),
1413 );
1414 assert_eq!(
1415 counts,
1416 PathCheckCounts {
1417 max_speed_error: 0,
1418 max_accel_error: 0,
1419 max_curvature_error: 57,
1420 collision_error: 0,
1421 ok: 43,
1422 }
1423 );
1424
1425 let best = frenet_optimal_planning_for_strategy(
1426 &csp,
1427 LOW_SPEED_STATE,
1428 &LOW_SPEED_OBSTACLES,
1429 FrenetPlannerConfig::low_speed(),
1430 LateralStrategyKind::LowSpeed,
1431 LongitudinalStrategyKind::VelocityKeeping,
1432 )
1433 .expect("expected a valid low-speed velocity path");
1434
1435 assert_eq!(best.x.len(), 25);
1436 assert_close(best.cf, 1.2030755468845455);
1437
1438 let reference = [
1439 (
1440 0usize,
1441 0.0837531641911651,
1442 0.4929355003324144,
1443 -0.1682997130258217,
1444 0.2777777777777778,
1445 -0.00857959813235128,
1446 0.0,
1447 0.0,
1448 0.5,
1449 0.2777777777777778,
1450 0.0,
1451 0.0,
1452 0.0,
1453 ),
1454 (
1455 1usize,
1456 0.14080514241144224,
1457 0.4832403177978342,
1458 -0.16839917882997923,
1459 0.2787788814960548,
1460 0.01878657598085877,
1461 -0.003816200753394419,
1462 0.0557444380144033,
1463 0.499991149250527,
1464 0.280590920781893,
1465 0.0277295524691358,
1466 -0.0004712686472257448,
1467 -0.016547187583003557,
1468 ),
1469 (
1470 12usize,
1471 0.9665238390623796,
1472 0.3501445932331652,
1473 -0.12806954066719456,
1474 0.495180889632367,
1475 0.11811618453721201,
1476 0.1750548062537276,
1477 0.9166666666666669,
1478 0.47744540572166433,
1479 0.5555555555555555,
1480 0.1736111111111111,
1481 -0.05725014209747345,
1482 -0.05948066711425816,
1483 ),
1484 (
1485 24usize,
1486 2.402278614362034,
1487 0.6586442169705189,
1488 0.5615637714479615,
1489 0.7923271276537857,
1490 0.12135132580938711,
1491 0.12938533923789106,
1492 2.666666666666667,
1493 0.39999999999999947,
1494 0.8333333333333333,
1495 0.0,
1496 -2.220446049250313e-16,
1497 -2.220446049250313e-16,
1498 ),
1499 ];
1500
1501 for (index, x, y, yaw, v, a, c, s, d, s_d, s_dd, d_d, d_dd) in reference {
1502 assert_close(best.x[index], x);
1503 assert_close(best.y[index], y);
1504 assert_close(best.yaw[index], yaw);
1505 assert_close(best.v[index], v);
1506 assert_close(best.a[index], a);
1507 assert_close(best.c[index], c);
1508 assert_close(best.s[index], s);
1509 assert_close(best.d[index], d);
1510 assert_close(best.s_d[index], s_d);
1511 assert_close(best.s_dd[index], s_dd);
1512 assert_close(best.d_d[index], d_d);
1513 assert_close(best.d_dd[index], d_dd);
1514 }
1515 }
1516
1517 #[test]
1518 fn test_calc_low_speed_stop_candidates_match_upstream_reference() {
1519 let paths = calc_frenet_paths_for_strategy(
1520 LOW_SPEED_STATE,
1521 FrenetPlannerConfig::low_speed(),
1522 LateralStrategyKind::LowSpeed,
1523 LongitudinalStrategyKind::MergingAndStopping,
1524 );
1525 assert_eq!(paths.len(), 350);
1526
1527 let reference = [
1528 (
1529 0usize,
1530 20usize,
1531 3.8000000000000003,
1532 -1.0,
1533 3.552713678800501e-15,
1534 7.105427357601002e-15,
1535 3.0969177986111163,
1536 0.04507065972221902,
1537 -0.4276979166666628,
1538 11.64042021329846,
1539 ),
1540 (
1541 175usize,
1542 22usize,
1543 4.2,
1544 -4.440892098500626e-16,
1545 0.0,
1546 -1.7763568394002505e-15,
1547 3.9969182251791864,
1548 0.04516803769544708,
1549 -0.43070595649825094,
1550 3.583659512530786,
1551 ),
1552 (
1553 349usize,
1554 25usize,
1555 4.800000000000001,
1556 0.7999999999999996,
1557 0.0,
1558 2.220446049250313e-16,
1559 4.899999999999999,
1560 7.105427357601002e-15,
1561 0.0,
1562 5.541471593686472,
1563 ),
1564 ];
1565
1566 for (index, t_len, t_last, d_last, d_d_last, d_dd_last, s_last, s_d_last, s_dd_last, cf) in
1567 reference
1568 {
1569 let path = &paths[index];
1570 assert_eq!(path.t.len(), t_len);
1571 assert_close(*path.t.last().unwrap(), t_last);
1572 assert_close(*path.d.last().unwrap(), d_last);
1573 assert_close(*path.d_d.last().unwrap(), d_d_last);
1574 assert_close(*path.d_dd.last().unwrap(), d_dd_last);
1575 assert_close(*path.s.last().unwrap(), s_last);
1576 assert_close(*path.s_d.last().unwrap(), s_d_last);
1577 assert_close(*path.s_dd.last().unwrap(), s_dd_last);
1578 assert_close(path.cf, cf);
1579 }
1580 }
1581
1582 #[test]
1583 fn test_low_speed_stop_planning_matches_upstream_initial_window() {
1584 let csp = CubicSpline2D::new(&LOW_SPEED_WX, &LOW_SPEED_WY);
1585 let mut all_paths = calc_frenet_paths_for_strategy(
1586 LOW_SPEED_STATE,
1587 FrenetPlannerConfig::low_speed(),
1588 LateralStrategyKind::LowSpeed,
1589 LongitudinalStrategyKind::MergingAndStopping,
1590 );
1591 calc_global_paths(&mut all_paths, &csp);
1592 let counts = summarize_paths_with_config(
1593 &all_paths,
1594 &LOW_SPEED_OBSTACLES,
1595 FrenetPlannerConfig::low_speed(),
1596 );
1597 assert_eq!(
1598 counts,
1599 PathCheckCounts {
1600 max_speed_error: 0,
1601 max_accel_error: 0,
1602 max_curvature_error: 90,
1603 collision_error: 179,
1604 ok: 81,
1605 }
1606 );
1607
1608 let best = frenet_optimal_planning_for_strategy(
1609 &csp,
1610 LOW_SPEED_STATE,
1611 &LOW_SPEED_OBSTACLES,
1612 FrenetPlannerConfig::low_speed(),
1613 LateralStrategyKind::LowSpeed,
1614 LongitudinalStrategyKind::MergingAndStopping,
1615 )
1616 .expect("expected a valid low-speed stop path");
1617
1618 assert_eq!(best.x.len(), 23);
1619 assert_close(best.cf, 3.278202056506379);
1620
1621 let reference = [
1622 (
1623 0usize,
1624 0.0837531641911651,
1625 0.4929355003324144,
1626 -0.1682997130258217,
1627 0.2777777777777778,
1628 -0.00857959813235128,
1629 0.0,
1630 0.0,
1631 0.5,
1632 0.2777777777777778,
1633 0.0,
1634 0.0,
1635 0.0,
1636 ),
1637 (
1638 1usize,
1639 0.14290658233093098,
1640 0.4828665878589613,
1641 -0.16925077183662696,
1642 0.30869555293037393,
1643 0.3016699961830643,
1644 -0.033086079297872974,
1645 0.057807503767624294,
1646 0.4999738741535292,
1647 0.31077654569441754,
1648 0.31452125324375435,
1649 -0.001345121009882328,
1650 -0.0457988066141507,
1651 ),
1652 (
1653 11usize,
1654 1.8763619373663174,
1655 0.09290809871307339,
1656 -0.05377592062450898,
1657 1.3921141884482757,
1658 0.2728327787831233,
1659 0.5788942882515342,
1660 1.9106947376794559,
1661 0.12803574707574247,
1662 1.3909637817624072,
1663 0.0037598990606935168,
1664 -0.3541694804931467,
1665 0.025694926643622717,
1666 ),
1667 (
1668 22usize,
1669 3.6101138784518745,
1670 0.6878357985060032,
1671 0.42438076874742103,
1672 0.03273264868551453,
1673 -0.3131098033086571,
1674 -0.5079631926311426,
1675 3.697542577965301,
1676 -0.20000000000000018,
1677 0.03605804483142805,
1678 -0.3446574138968863,
1679 2.6645352591003757e-15,
1680 0.0,
1681 ),
1682 ];
1683
1684 for (index, x, y, yaw, v, a, c, s, d, s_d, s_dd, d_d, d_dd) in reference {
1685 assert_close(best.x[index], x);
1686 assert_close(best.y[index], y);
1687 assert_close(best.yaw[index], yaw);
1688 assert_close(best.v[index], v);
1689 assert_close(best.a[index], a);
1690 assert_close(best.c[index], c);
1691 assert_close(best.s[index], s);
1692 assert_close(best.d[index], d);
1693 assert_close(best.s_d[index], s_d);
1694 assert_close(best.s_dd[index], s_dd);
1695 assert_close(best.d_d[index], d_d);
1696 assert_close(best.d_dd[index], d_dd);
1697 }
1698 }
1699
1700 #[test]
1701 fn test_high_speed_simulation_matches_upstream_main_loop() {
1702 let csp = CubicSpline2D::new(&UPSTREAM_WX, &UPSTREAM_WY);
1703 let (trace, outcome, step) = run_simulation_for_strategy(
1704 &csp,
1705 FrenetInitialState {
1706 s0: 0.0,
1707 speed: 10.0 / 3.6,
1708 accel: 0.0,
1709 d: 2.0,
1710 d_d: 0.0,
1711 d_dd: 0.0,
1712 },
1713 &UPSTREAM_OBSTACLES,
1714 FrenetPlannerConfig::high_speed(),
1715 LateralStrategyKind::HighSpeed,
1716 LongitudinalStrategyKind::VelocityKeeping,
1717 500,
1718 );
1719 assert_eq!(outcome, SimulationOutcomeKind::Goal);
1720 assert_eq!(step, 56);
1721 assert_eq!(trace.len(), 57);
1722
1723 let reference = [
1724 (
1725 0usize,
1726 SimulationSnapshot {
1727 fallback: false,
1728 cf: 3.542294469271419,
1729 len: 25,
1730 x1: 1.8605645611714472,
1731 y1: 1.005284701078841,
1732 yaw1: -0.7488798286101788,
1733 v1: 2.787960815155609,
1734 a1: 0.2033476908615227,
1735 c1: -0.021106708455623728,
1736 s1: 0.557444380144033,
1737 d1: 1.9986421561535495,
1738 s_d1: 2.80590920781893,
1739 s_dd1: 0.27729552469135804,
1740 d_d1: -0.007103103907306435,
1741 d_dd1: -0.02396404262355845,
1742 counts: PathCheckCounts {
1743 max_speed_error: 30,
1744 max_accel_error: 150,
1745 max_curvature_error: 0,
1746 collision_error: 0,
1747 ok: 30,
1748 },
1749 },
1750 ),
1751 (
1752 1usize,
1753 SimulationSnapshot {
1754 fallback: false,
1755 cf: 2.8489891521827695,
1756 len: 23,
1757 x1: 2.3695265115199833,
1758 y1: 0.5284172832738651,
1759 yaw1: -0.7638861092260947,
1760 v1: 2.8502342570516377,
1761 a1: 0.4520559398699433,
1762 c1: -0.03273823640059959,
1763 s1: 1.125900205761317,
1764 d1: 1.9898244340386615,
1765 s_d1: 2.887088477366255,
1766 s_dd1: 0.5304783950617284,
1767 d_d1: -0.02526460939871889,
1768 d_dd1: -0.038168947823938225,
1769 counts: PathCheckCounts {
1770 max_speed_error: 21,
1771 max_accel_error: 155,
1772 max_curvature_error: 0,
1773 collision_error: 0,
1774 ok: 34,
1775 },
1776 },
1777 ),
1778 (
1779 2usize,
1780 SimulationSnapshot {
1781 fallback: false,
1782 cf: 2.323344485225353,
1783 len: 23,
1784 x1: 2.8841546208047726,
1785 y1: 0.033418184614008695,
1786 yaw1: -0.7819891459756945,
1787 v1: 2.956948411602434,
1788 a1: 0.6512431510400924,
1789 c1: -0.032212309902136096,
1790 s1: 1.715336759017426,
1791 d1: 1.9680584361960702,
1792 s_d1: 3.0141498321879223,
1793 s_dd1: 0.7366617048543571,
1794 d_d1: -0.048825425109529065,
1795 d_dd1: -0.04062488602072928,
1796 counts: PathCheckCounts {
1797 max_speed_error: 15,
1798 max_accel_error: 157,
1799 max_curvature_error: 0,
1800 collision_error: 0,
1801 ok: 38,
1802 },
1803 },
1804 ),
1805 (
1806 10usize,
1807 SimulationSnapshot {
1808 fallback: false,
1809 cf: 1.166097731781801,
1810 len: 21,
1811 x1: 7.548253592986458,
1812 y1: -4.294826068828912,
1813 yaw1: -0.5856688254921298,
1814 v1: 4.189278559604101,
1815 a1: 0.21881208796186158,
1816 c1: 0.1254113257858499,
1817 s1: 7.876732837857354,
1818 d1: 1.2776621126305698,
1819 s_d1: 4.8229460558777975,
1820 s_dd1: 1.286834078533147,
1821 d_d1: -0.1255759074190509,
1822 d_dd1: 0.004692247248534772,
1823 counts: PathCheckCounts {
1824 max_speed_error: 2,
1825 max_accel_error: 88,
1826 max_curvature_error: 0,
1827 collision_error: 69,
1828 ok: 51,
1829 },
1830 },
1831 ),
1832 (
1833 20usize,
1834 SimulationSnapshot {
1835 fallback: false,
1836 cf: 17.5303184314179,
1837 len: 23,
1838 x1: 14.6771670455895,
1839 y1: -0.5409502249017437,
1840 yaw1: 1.0432729339961222,
1841 v1: 6.751977179466637,
1842 a1: 1.1716781306769781,
1843 c1: 0.022496673620090123,
1844 s1: 19.67714300233158,
1845 d1: 0.855537905656363,
1846 s_d1: 6.762010642962067,
1847 s_dd1: 0.7188928361205661,
1848 d_d1: 0.0927110353342166,
1849 d_dd1: 0.016517891487136914,
1850 counts: PathCheckCounts {
1851 max_speed_error: 0,
1852 max_accel_error: 3,
1853 max_curvature_error: 0,
1854 collision_error: 150,
1855 ok: 57,
1856 },
1857 },
1858 ),
1859 (
1860 56usize,
1861 SimulationSnapshot {
1862 fallback: false,
1863 cf: 0.8381388542917644,
1864 len: 2,
1865 x1: 70.17562512818523,
1866 y1: 0.3562246796885216,
1867 yaw1: -0.25232078404059566,
1868 v1: 8.365218464633804,
1869 a1: -0.02230330452044404,
1870 c1: 0.005745317758332535,
1871 s1: 77.13369022391397,
1872 d1: 0.28528511463172584,
1873 s_d1: 8.352995857586786,
1874 s_dd1: 0.0014166156483617837,
1875 d_d1: -0.05393428352839567,
1876 d_dd1: 0.005806714817959832,
1877 counts: PathCheckCounts {
1878 max_speed_error: 0,
1879 max_accel_error: 0,
1880 max_curvature_error: 0,
1881 collision_error: 0,
1882 ok: 210,
1883 },
1884 },
1885 ),
1886 ];
1887
1888 for (index, expected) in reference {
1889 assert_snapshot_matches(trace[index], expected);
1890 }
1891 }
1892
1893 #[test]
1894 fn test_low_speed_velocity_simulation_matches_upstream_reference() {
1895 let csp = CubicSpline2D::new(&LOW_SPEED_WX, &LOW_SPEED_WY);
1896 let (trace, outcome, step) = run_simulation_for_strategy(
1897 &csp,
1898 LOW_SPEED_STATE,
1899 &LOW_SPEED_OBSTACLES,
1900 FrenetPlannerConfig::low_speed(),
1901 LateralStrategyKind::LowSpeed,
1902 LongitudinalStrategyKind::VelocityKeeping,
1903 500,
1904 );
1905 assert_eq!(outcome, SimulationOutcomeKind::Goal);
1906 assert_eq!(step, 72);
1907 assert_eq!(trace.len(), 73);
1908
1909 let reference = [
1910 (
1911 0usize,
1912 SimulationSnapshot {
1913 fallback: false,
1914 cf: 1.2030755468845455,
1915 len: 25,
1916 x1: 0.14080514241144224,
1917 y1: 0.4832403177978342,
1918 yaw1: -0.16839917882997923,
1919 v1: 0.2787788814960548,
1920 a1: 0.01878657598085877,
1921 c1: -0.003816200753394419,
1922 s1: 0.0557444380144033,
1923 d1: 0.499991149250527,
1924 s_d1: 0.280590920781893,
1925 s_dd1: 0.0277295524691358,
1926 d_d1: -0.0004712686472257448,
1927 d_dd1: -0.016547187583003557,
1928 counts: PathCheckCounts {
1929 max_speed_error: 0,
1930 max_accel_error: 0,
1931 max_curvature_error: 57,
1932 collision_error: 0,
1933 ok: 43,
1934 },
1935 },
1936 ),
1937 (
1938 1usize,
1939 SimulationSnapshot {
1940 fallback: false,
1941 cf: 1.1593824675794724,
1942 len: 21,
1943 x1: 0.19862397869158743,
1944 y1: 0.4734005087839725,
1945 yaw1: -0.1689488569133979,
1946 v1: 0.2854845708264037,
1947 a1: 0.04834696890578991,
1948 c1: -0.016702081709852195,
1949 s1: 0.1126273382131937,
1950 d1: 0.4999233647275333,
1951 s_d1: 0.2892620076314542,
1952 s_dd1: 0.058447345232429636,
1953 d_d1: -0.002151504794264144,
1954 d_dd1: -0.0418106087316688,
1955 counts: PathCheckCounts {
1956 max_speed_error: 0,
1957 max_accel_error: 0,
1958 max_curvature_error: 52,
1959 collision_error: 0,
1960 ok: 48,
1961 },
1962 },
1963 ),
1964 (
1965 2usize,
1966 SimulationSnapshot {
1967 fallback: false,
1968 cf: 1.09349999893079,
1969 len: 20,
1970 x1: 0.2583590326193169,
1971 y1: 0.46318616576505733,
1972 yaw1: -0.17013710434144036,
1973 v1: 0.2976978528813822,
1974 a1: 0.0739238866402707,
1975 c1: -0.02614265471946679,
1976 s1: 0.1718374528054004,
1977 d1: 0.4997092754709174,
1978 s_d1: 0.30375627055724636,
1979 s_dd1: 0.08596131323144582,
1980 d_d1: -0.005295480363446369,
1981 d_dd1: -0.06364996071308254,
1982 counts: PathCheckCounts {
1983 max_speed_error: 0,
1984 max_accel_error: 0,
1985 max_curvature_error: 44,
1986 collision_error: 3,
1987 ok: 53,
1988 },
1989 },
1990 ),
1991 (
1992 10usize,
1993 SimulationSnapshot {
1994 fallback: false,
1995 cf: 1.5495892077371207,
1996 len: 23,
1997 x1: 0.8720531487807729,
1998 y1: 0.35911979640746683,
1999 yaw1: -0.16214830237709377,
2000 v1: 0.45631948013996126,
2001 a1: 0.08683536859186874,
2002 c1: -0.04279669202248163,
2003 s1: 0.8123519120139755,
2004 d1: 0.47926480753857237,
2005 s_d1: 0.5037646387918939,
2006 s_dd1: 0.12428623137720253,
2007 d_d1: -0.06826002731440607,
2008 d_dd1: -0.20972236566824146,
2009 counts: PathCheckCounts {
2010 max_speed_error: 0,
2011 max_accel_error: 0,
2012 max_curvature_error: 18,
2013 collision_error: 40,
2014 ok: 42,
2015 },
2016 },
2017 ),
2018 (
2019 72usize,
2020 SimulationSnapshot {
2021 fallback: false,
2022 cf: 0.8058978467091669,
2023 len: 7,
2024 x1: 9.125666057110516,
2025 y1: -1.5235516112544554,
2026 yaw1: -0.5413983263388418,
2027 v1: 0.8329541837630935,
2028 a1: -0.0025774853620610617,
2029 c1: 0.08692867844324033,
2030 s1: 9.956315978981115,
2031 d1: 0.021489724603800633,
2032 s_d1: 0.8305162740025163,
2033 s_dd1: 0.003961111852914451,
2034 d_d1: -0.07081502791017966,
2035 d_dd1: 0.10801481070105634,
2036 counts: PathCheckCounts {
2037 max_speed_error: 0,
2038 max_accel_error: 0,
2039 max_curvature_error: 0,
2040 collision_error: 0,
2041 ok: 100,
2042 },
2043 },
2044 ),
2045 ];
2046
2047 for (index, expected) in reference {
2048 assert_snapshot_matches(trace[index], expected);
2049 }
2050 }
2051
2052 #[test]
2053 fn test_low_speed_stop_simulation_matches_upstream_reference() {
2054 let csp = CubicSpline2D::new(&LOW_SPEED_WX, &LOW_SPEED_WY);
2055 let (trace, outcome, step) = run_simulation_for_strategy(
2056 &csp,
2057 LOW_SPEED_STATE,
2058 &LOW_SPEED_OBSTACLES,
2059 FrenetPlannerConfig::low_speed(),
2060 LateralStrategyKind::LowSpeed,
2061 LongitudinalStrategyKind::MergingAndStopping,
2062 500,
2063 );
2064 assert_eq!(outcome, SimulationOutcomeKind::Finish);
2065 assert_eq!(step, 28);
2066 assert_eq!(trace.len(), 28);
2067
2068 let reference = [
2069 (
2070 0usize,
2071 SimulationSnapshot {
2072 fallback: false,
2073 cf: 3.278202056506379,
2074 len: 23,
2075 x1: 0.14290658233093098,
2076 y1: 0.4828665878589613,
2077 yaw1: -0.16925077183662696,
2078 v1: 0.30869555293037393,
2079 a1: 0.3016699961830643,
2080 c1: -0.033086079297872974,
2081 s1: 0.057807503767624294,
2082 d1: 0.4999738741535292,
2083 s_d1: 0.31077654569441754,
2084 s_dd1: 0.31452125324375435,
2085 d_d1: -0.001345121009882328,
2086 d_dd1: -0.0457988066141507,
2087 counts: PathCheckCounts {
2088 max_speed_error: 0,
2089 max_accel_error: 0,
2090 max_curvature_error: 90,
2091 collision_error: 179,
2092 ok: 81,
2093 },
2094 },
2095 ),
2096 (
2097 1usize,
2098 SimulationSnapshot {
2099 fallback: false,
2100 cf: 2.5409670415058536,
2101 len: 23,
2102 x1: 0.21398675876644052,
2103 y1: 0.47062703598230726,
2104 yaw1: -0.17233523193211964,
2105 v1: 0.3908914702453267,
2106 a1: 0.507139996062664,
2107 c1: -0.05720254792836169,
2108 s1: 0.1278355663792582,
2109 d1: 0.49973517177727805,
2110 s_d1: 0.3967668021582189,
2111 s_dd1: 0.5325274854225438,
2112 d_d1: -0.00591896882202722,
2113 d_dd1: -0.08407951439775528,
2114 counts: PathCheckCounts {
2115 max_speed_error: 0,
2116 max_accel_error: 0,
2117 max_curvature_error: 92,
2118 collision_error: 178,
2119 ok: 80,
2120 },
2121 },
2122 ),
2123 (
2124 2usize,
2125 SimulationSnapshot {
2126 fallback: false,
2127 cf: 2.081139888747688,
2128 len: 23,
2129 x1: 0.3051413175662653,
2130 y1: 0.4545276729333922,
2131 yaw1: -0.1784908329442092,
2132 v1: 0.5018045282261229,
2133 a1: 0.5936073747094394,
2134 c1: -0.08344253603936327,
2135 s1: 0.21863965454953663,
2136 d1: 0.4987893954606454,
2137 s_d1: 0.5148189105129078,
2138 s_dd1: 0.6389665942262637,
2139 d_d1: -0.015563825269794466,
2140 d_dd1: -0.127144484147799,
2141 counts: PathCheckCounts {
2142 max_speed_error: 0,
2143 max_accel_error: 0,
2144 max_curvature_error: 91,
2145 collision_error: 179,
2146 ok: 80,
2147 },
2148 },
2149 ),
2150 (
2151 10usize,
2152 SimulationSnapshot {
2153 fallback: true,
2154 cf: 1.4304565347697635,
2155 len: 19,
2156 x1: 1.6926439871748995,
2157 y1: 0.15261800304196171,
2158 yaw1: -0.11885428163574252,
2159 v1: 1.1361148263712337,
2160 a1: 0.20978218334193272,
2161 c1: 0.27909204598500814,
2162 s1: 1.7325635495402325,
2163 d1: 0.23379710386591446,
2164 s_d1: 1.2016568272191095,
2165 s_dd1: 0.043622677081437716,
2166 d_d1: -0.3162049915823444,
2167 d_dd1: -0.08257892976684256,
2168 counts: PathCheckCounts {
2169 max_speed_error: 0,
2170 max_accel_error: 0,
2171 max_curvature_error: 14,
2172 collision_error: 21,
2173 ok: 0,
2174 },
2175 },
2176 ),
2177 (
2178 27usize,
2179 SimulationSnapshot {
2180 fallback: true,
2181 cf: 1.4304565347697635,
2182 len: 2,
2183 x1: 3.8421875321885457,
2184 y1: 0.7718572517229931,
2185 yaw1: 0.2541519104288392,
2186 v1: 0.01058009224298756,
2187 a1: -0.103322941903366,
2188 c1: 0.27909204598500814,
2189 s1: 3.999157563333649,
2190 d1: -0.19999999999999984,
2191 s_d1: 0.01248764848273487,
2192 s_dd1: -0.12190755851008128,
2193 d_d1: 6.661338147750939e-16,
2194 d_dd1: 2.220446049250313e-16,
2195 counts: PathCheckCounts {
2196 max_speed_error: 5,
2197 max_accel_error: 0,
2198 max_curvature_error: 30,
2199 collision_error: 0,
2200 ok: 0,
2201 },
2202 },
2203 ),
2204 ];
2205
2206 for (index, expected) in reference {
2207 assert_snapshot_matches(trace[index], expected);
2208 }
2209 }
2210}