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