Skip to main content

rust_robotics_planning/
frenet_optimal_trajectory.rs

1#![allow(dead_code)]
2
3//! Frenet Optimal Trajectory planning
4//!
5//! Generates optimal trajectories in the Frenet frame, considering
6//! lateral and longitudinal motion with jerk minimization.
7
8// Parameters
9use 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
23// Cost weights
24const 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
32/// Quintic polynomial for trajectory generation
33struct 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
112/// Quartic polynomial for velocity keeping
113struct 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/// Frenet path
166#[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
237/// Cubic spline for reference path
238pub 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
786/// Generate Frenet candidate paths
787pub 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
846/// Calculate global positions from Frenet paths
847pub 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
898/// Check constraints and collisions
899pub 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
911/// Frenet optimal planning
912pub 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}