Skip to main content

rust_robotics_planning/state_lattice/
motion_model.rs

1//! Motion model for State Lattice Planner
2//!
3//! Implements a bicycle kinematic model for vehicle motion.
4//! Based on PythonRobotics implementation.
5
6/// Vehicle state
7#[derive(Debug, Clone, Copy)]
8pub struct VehicleState {
9    pub x: f64,
10    pub y: f64,
11    pub yaw: f64,
12    pub v: f64,
13}
14
15impl VehicleState {
16    pub fn new(x: f64, y: f64, yaw: f64, v: f64) -> Self {
17        Self { x, y, yaw, v }
18    }
19
20    pub fn origin() -> Self {
21        Self {
22            x: 0.0,
23            y: 0.0,
24            yaw: 0.0,
25            v: 0.0,
26        }
27    }
28}
29
30/// Motion model configuration
31#[derive(Debug, Clone)]
32pub struct MotionModelConfig {
33    /// Wheelbase length \[m\]
34    pub wheelbase: f64,
35    /// Distance step for trajectory generation \[m\]
36    pub ds: f64,
37    /// Default velocity \[m/s\]
38    pub default_velocity: f64,
39}
40
41impl Default for MotionModelConfig {
42    fn default() -> Self {
43        Self {
44            wheelbase: 1.0,
45            ds: 0.1,
46            default_velocity: 10.0 / 3.6, // ~2.78 m/s
47        }
48    }
49}
50
51/// Bicycle kinematic motion model
52pub struct MotionModel {
53    config: MotionModelConfig,
54}
55
56impl MotionModel {
57    pub fn new(config: MotionModelConfig) -> Self {
58        Self { config }
59    }
60
61    pub fn with_defaults() -> Self {
62        Self::new(MotionModelConfig::default())
63    }
64
65    /// Get wheelbase
66    pub fn wheelbase(&self) -> f64 {
67        self.config.wheelbase
68    }
69
70    /// Get distance step
71    pub fn ds(&self) -> f64 {
72        self.config.ds
73    }
74
75    /// Update state using bicycle kinematic model
76    pub fn update(&self, state: &VehicleState, v: f64, delta: f64, dt: f64) -> VehicleState {
77        let l = self.config.wheelbase;
78
79        VehicleState {
80            x: state.x + v * state.yaw.cos() * dt,
81            y: state.y + v * state.yaw.sin() * dt,
82            yaw: normalize_angle(state.yaw + v / l * delta.tan() * dt),
83            v,
84        }
85    }
86
87    /// Generate trajectory given curvature parameters
88    pub fn generate_trajectory(
89        &self,
90        s: f64,
91        k0: f64,
92        km: f64,
93        kf: f64,
94    ) -> (Vec<f64>, Vec<f64>, Vec<f64>) {
95        if s <= 0.0 {
96            return (vec![0.0], vec![0.0], vec![0.0]);
97        }
98
99        let v = self.config.default_velocity;
100        let dt = self.config.ds / v;
101        let time = s / v;
102
103        let mut x_list = vec![0.0];
104        let mut y_list = vec![0.0];
105        let mut yaw_list = vec![0.0];
106
107        let mut state = VehicleState::origin();
108        let mut step = 0usize;
109
110        loop {
111            let t = step as f64 * dt;
112            if t >= time {
113                break;
114            }
115            let tau = if time > 0.0 { t / time } else { 0.0 };
116            let delta = self.interpolate_curvature(tau, k0, km, kf);
117
118            state = self.update(&state, v, delta, dt);
119
120            x_list.push(state.x);
121            y_list.push(state.y);
122            yaw_list.push(state.yaw);
123            step += 1;
124        }
125
126        (x_list, y_list, yaw_list)
127    }
128
129    /// Interpolate curvature using quadratic polynomial
130    fn interpolate_curvature(&self, t: f64, k0: f64, km: f64, kf: f64) -> f64 {
131        let a = 2.0 * (kf + k0 - 2.0 * km);
132        let b = -kf - 3.0 * k0 + 4.0 * km;
133        let c = k0;
134
135        a * t * t + b * t + c
136    }
137
138    /// Generate trajectory and return final state
139    pub fn generate_trajectory_final_state(
140        &self,
141        s: f64,
142        k0: f64,
143        km: f64,
144        kf: f64,
145    ) -> (f64, f64, f64) {
146        let (x, y, yaw) = self.generate_trajectory(s, k0, km, kf);
147        let n = x.len();
148        if n == 0 {
149            (0.0, 0.0, 0.0)
150        } else {
151            (x[n - 1], y[n - 1], yaw[n - 1])
152        }
153    }
154}
155
156pub use rust_robotics_core::normalize_angle;
157
158#[cfg(test)]
159mod tests {
160    use super::*;
161    use std::f64::consts::PI;
162
163    type TrajectorySample = (f64, f64, f64);
164    type TrajectoryParams = (f64, f64, f64);
165
166    #[derive(Debug, Clone, Copy)]
167    struct TrajectoryFingerprint {
168        len: usize,
169        sum_x: f64,
170        sum_y: f64,
171        sum_yaw: f64,
172        weighted_sum_x: f64,
173        weighted_sum_y: f64,
174        weighted_sum_yaw: f64,
175        sum_x_sq: f64,
176        sum_y_sq: f64,
177        sum_yaw_sq: f64,
178    }
179
180    #[derive(Debug, Clone, Copy)]
181    struct TrajectoryFingerprintTolerance {
182        sum_x: f64,
183        sum_y: f64,
184        sum_yaw: f64,
185        weighted_sum_x: f64,
186        weighted_sum_y: f64,
187        weighted_sum_yaw: f64,
188        sum_x_sq: f64,
189        sum_y_sq: f64,
190        sum_yaw_sq: f64,
191    }
192
193    fn parse_reference_trajectory_samples(csv: &str) -> Vec<TrajectorySample> {
194        csv.lines()
195            .skip(1)
196            .filter(|line| !line.trim().is_empty())
197            .map(|line| {
198                let values: Vec<f64> = line
199                    .split(',')
200                    .skip(1)
201                    .map(|value| value.parse::<f64>().unwrap())
202                    .collect();
203                assert_eq!(values.len(), 3);
204                (values[0], values[1], values[2])
205            })
206            .collect()
207    }
208
209    fn parse_grouped_reference_trajectory_samples(csv: &str) -> Vec<Vec<TrajectorySample>> {
210        let mut trajectories = Vec::new();
211        let mut current_row = None;
212        let mut current_samples = Vec::new();
213
214        for line in csv.lines().skip(1).filter(|line| !line.trim().is_empty()) {
215            let values: Vec<&str> = line.split(',').collect();
216            assert_eq!(values.len(), 5);
217
218            let row = values[0].parse::<usize>().unwrap();
219            let index = values[1].parse::<usize>().unwrap();
220            let sample = (
221                values[2].parse::<f64>().unwrap(),
222                values[3].parse::<f64>().unwrap(),
223                values[4].parse::<f64>().unwrap(),
224            );
225
226            match current_row {
227                Some(current_row_index) if current_row_index != row => {
228                    trajectories.push(std::mem::take(&mut current_samples));
229                    current_row = Some(row);
230                }
231                None => current_row = Some(row),
232                _ => {}
233            }
234
235            assert_eq!(index, current_samples.len());
236            current_samples.push(sample);
237        }
238
239        if !current_samples.is_empty() {
240            trajectories.push(current_samples);
241        }
242
243        trajectories
244    }
245
246    fn parse_terminal_trajectory_params(csv: &str) -> Vec<TrajectoryParams> {
247        csv.lines()
248            .skip(1)
249            .filter(|line| !line.trim().is_empty())
250            .map(|line| {
251                let values: Vec<f64> = line
252                    .split(',')
253                    .map(|value| value.parse::<f64>().unwrap())
254                    .collect();
255                assert_eq!(values.len(), 6);
256                (values[3], values[4], values[5])
257            })
258            .collect()
259    }
260
261    fn parse_reference_trajectory_fingerprints(csv: &str) -> Vec<TrajectoryFingerprint> {
262        csv.lines()
263            .skip(1)
264            .filter(|line| !line.trim().is_empty())
265            .enumerate()
266            .map(|(index, line)| {
267                let values: Vec<&str> = line.split(',').collect();
268                assert_eq!(values.len(), 11);
269                assert_eq!(values[0].parse::<usize>().unwrap(), index);
270
271                TrajectoryFingerprint {
272                    len: values[1].parse::<usize>().unwrap(),
273                    sum_x: values[2].parse::<f64>().unwrap(),
274                    sum_y: values[3].parse::<f64>().unwrap(),
275                    sum_yaw: values[4].parse::<f64>().unwrap(),
276                    weighted_sum_x: values[5].parse::<f64>().unwrap(),
277                    weighted_sum_y: values[6].parse::<f64>().unwrap(),
278                    weighted_sum_yaw: values[7].parse::<f64>().unwrap(),
279                    sum_x_sq: values[8].parse::<f64>().unwrap(),
280                    sum_y_sq: values[9].parse::<f64>().unwrap(),
281                    sum_yaw_sq: values[10].parse::<f64>().unwrap(),
282                }
283            })
284            .collect()
285    }
286
287    fn trajectory_fingerprint(x: &[f64], y: &[f64], yaw: &[f64]) -> TrajectoryFingerprint {
288        assert_eq!(x.len(), y.len());
289        assert_eq!(x.len(), yaw.len());
290
291        let mut sum_x = 0.0;
292        let mut sum_y = 0.0;
293        let mut sum_yaw = 0.0;
294        let mut weighted_sum_x = 0.0;
295        let mut weighted_sum_y = 0.0;
296        let mut weighted_sum_yaw = 0.0;
297        let mut sum_x_sq = 0.0;
298        let mut sum_y_sq = 0.0;
299        let mut sum_yaw_sq = 0.0;
300
301        for (index, ((&x, &y), &yaw)) in x.iter().zip(y.iter()).zip(yaw.iter()).enumerate() {
302            let weight = (index + 1) as f64;
303            sum_x += x;
304            sum_y += y;
305            sum_yaw += yaw;
306            weighted_sum_x += weight * x;
307            weighted_sum_y += weight * y;
308            weighted_sum_yaw += weight * yaw;
309            sum_x_sq += x * x;
310            sum_y_sq += y * y;
311            sum_yaw_sq += yaw * yaw;
312        }
313
314        TrajectoryFingerprint {
315            len: x.len(),
316            sum_x,
317            sum_y,
318            sum_yaw,
319            weighted_sum_x,
320            weighted_sum_y,
321            weighted_sum_yaw,
322            sum_x_sq,
323            sum_y_sq,
324            sum_yaw_sq,
325        }
326    }
327
328    fn assert_trajectory_samples_match(
329        x: &[f64],
330        y: &[f64],
331        yaw: &[f64],
332        expected: &[TrajectorySample],
333        tolerance: TrajectorySample,
334    ) {
335        assert_eq!(x.len(), expected.len());
336        assert_eq!(y.len(), expected.len());
337        assert_eq!(yaw.len(), expected.len());
338
339        for (((&x, &y), &yaw), expected) in x
340            .iter()
341            .zip(y.iter())
342            .zip(yaw.iter())
343            .zip(expected.iter().copied())
344        {
345            let observed = (x, y, yaw);
346            assert!(
347                (x - expected.0).abs() < tolerance.0,
348                "x mismatch: observed={observed:?}, expected={expected:?}"
349            );
350            assert!(
351                (y - expected.1).abs() < tolerance.1,
352                "y mismatch: observed={observed:?}, expected={expected:?}"
353            );
354            assert!(
355                (yaw - expected.2).abs() < tolerance.2,
356                "yaw mismatch: observed={observed:?}, expected={expected:?}"
357            );
358        }
359    }
360
361    fn assert_trajectory_fingerprint_matches(
362        actual: &TrajectoryFingerprint,
363        expected: &TrajectoryFingerprint,
364        tolerance: TrajectoryFingerprintTolerance,
365    ) {
366        assert_eq!(actual.len, expected.len);
367        assert!(
368            (actual.sum_x - expected.sum_x).abs() < tolerance.sum_x,
369            "sum_x mismatch: actual={actual:?}, expected={expected:?}"
370        );
371        assert!(
372            (actual.sum_y - expected.sum_y).abs() < tolerance.sum_y,
373            "sum_y mismatch: actual={actual:?}, expected={expected:?}"
374        );
375        assert!(
376            (actual.sum_yaw - expected.sum_yaw).abs() < tolerance.sum_yaw,
377            "sum_yaw mismatch: actual={actual:?}, expected={expected:?}"
378        );
379        assert!(
380            (actual.weighted_sum_x - expected.weighted_sum_x).abs() < tolerance.weighted_sum_x,
381            "weighted_sum_x mismatch: actual={actual:?}, expected={expected:?}"
382        );
383        assert!(
384            (actual.weighted_sum_y - expected.weighted_sum_y).abs() < tolerance.weighted_sum_y,
385            "weighted_sum_y mismatch: actual={actual:?}, expected={expected:?}"
386        );
387        assert!(
388            (actual.weighted_sum_yaw - expected.weighted_sum_yaw).abs()
389                < tolerance.weighted_sum_yaw,
390            "weighted_sum_yaw mismatch: actual={actual:?}, expected={expected:?}"
391        );
392        assert!(
393            (actual.sum_x_sq - expected.sum_x_sq).abs() < tolerance.sum_x_sq,
394            "sum_x_sq mismatch: actual={actual:?}, expected={expected:?}"
395        );
396        assert!(
397            (actual.sum_y_sq - expected.sum_y_sq).abs() < tolerance.sum_y_sq,
398            "sum_y_sq mismatch: actual={actual:?}, expected={expected:?}"
399        );
400        assert!(
401            (actual.sum_yaw_sq - expected.sum_yaw_sq).abs() < tolerance.sum_yaw_sq,
402            "sum_yaw_sq mismatch: actual={actual:?}, expected={expected:?}"
403        );
404    }
405
406    fn assert_terminal_table_fingerprint_matches(
407        terminal_csv: &str,
408        fingerprint_csv: &str,
409        k0: f64,
410        tolerance: TrajectoryFingerprintTolerance,
411    ) {
412        let model = MotionModel::with_defaults();
413        let params = parse_terminal_trajectory_params(terminal_csv);
414        let expected = parse_reference_trajectory_fingerprints(fingerprint_csv);
415
416        assert_eq!(params.len(), expected.len());
417
418        for ((s, km, kf), expected) in params.iter().copied().zip(expected.iter()) {
419            let (x, y, yaw) = model.generate_trajectory(s, k0, km, kf);
420            let actual = trajectory_fingerprint(&x, &y, &yaw);
421            assert_trajectory_fingerprint_matches(&actual, expected, tolerance);
422        }
423    }
424
425    fn assert_terminal_table_exact_samples_match(
426        terminal_csv: &str,
427        sample_csv: &str,
428        k0: f64,
429        tolerance: TrajectorySample,
430    ) {
431        let model = MotionModel::with_defaults();
432        let params = parse_terminal_trajectory_params(terminal_csv);
433        let expected = parse_grouped_reference_trajectory_samples(sample_csv);
434
435        assert_eq!(params.len(), expected.len());
436
437        for ((s, km, kf), expected_samples) in params.iter().copied().zip(expected.iter()) {
438            let (x, y, yaw) = model.generate_trajectory(s, k0, km, kf);
439            assert_trajectory_samples_match(&x, &y, &yaw, expected_samples, tolerance);
440        }
441    }
442
443    #[test]
444    fn test_vehicle_state() {
445        let state = VehicleState::new(1.0, 2.0, 0.5, 1.0);
446        assert_eq!(state.x, 1.0);
447        assert_eq!(state.y, 2.0);
448        assert_eq!(state.yaw, 0.5);
449    }
450
451    #[test]
452    fn test_motion_model_update() {
453        let model = MotionModel::with_defaults();
454        let state = VehicleState::origin();
455        let next = model.update(&state, 1.0, 0.0, 0.1);
456
457        assert!((next.x - 0.1).abs() < 1e-10);
458        assert!(next.y.abs() < 1e-10);
459        assert!(next.yaw.abs() < 1e-10);
460    }
461
462    #[test]
463    fn test_curvature_interpolation() {
464        let model = MotionModel::with_defaults();
465
466        let k0 = 0.0;
467        let km = 0.1;
468        let kf = 0.2;
469
470        let k_start = model.interpolate_curvature(0.0, k0, km, kf);
471        let k_mid = model.interpolate_curvature(0.5, k0, km, kf);
472        let k_end = model.interpolate_curvature(1.0, k0, km, kf);
473
474        assert!((k_start - k0).abs() < 1e-10);
475        assert!((k_mid - km).abs() < 1e-10);
476        assert!((k_end - kf).abs() < 1e-10);
477    }
478
479    #[test]
480    fn test_generate_trajectory_straight() {
481        let model = MotionModel::with_defaults();
482        let (x, y, _yaw) = model.generate_trajectory(1.0, 0.0, 0.0, 0.0);
483
484        assert!(x.len() > 1);
485        for yi in &y {
486            assert!(yi.abs() < 1e-6);
487        }
488    }
489
490    #[test]
491    fn test_generate_trajectory_turn() {
492        let model = MotionModel::with_defaults();
493        let (x, y, _yaw) = model.generate_trajectory(2.0, 0.1, 0.1, 0.1);
494
495        assert!(x.len() > 1);
496        let final_y = y.last().unwrap();
497        assert!(*final_y > 0.0);
498    }
499
500    #[test]
501    fn test_normalize_angle() {
502        assert!((normalize_angle(0.0) - 0.0).abs() < 1e-10);
503        assert!((normalize_angle(PI) - PI).abs() < 1e-10);
504        assert!((normalize_angle(-PI) - (-PI)).abs() < 1e-10);
505        assert!((normalize_angle(3.0 * PI) - PI).abs() < 1e-10);
506        assert!((normalize_angle(-3.0 * PI) - (-PI)).abs() < 1e-10);
507    }
508
509    #[test]
510    fn test_generate_trajectory_matches_upstream_uniform1_row0_samples() {
511        let model = MotionModel::with_defaults();
512        let expected = parse_reference_trajectory_samples(include_str!(
513            "testdata/uniform1_row0_trajectory.csv"
514        ));
515        let (x, y, yaw) = model.generate_trajectory(
516            23.380_658_812_582,
517            0.0,
518            -0.101556678511287,
519            0.004757708055363,
520        );
521
522        assert_trajectory_samples_match(&x, &y, &yaw, &expected, (1e-9, 1e-9, 1e-9));
523    }
524
525    #[test]
526    fn test_generate_trajectory_matches_upstream_uniform2_row9_samples() {
527        let model = MotionModel::with_defaults();
528        let expected = parse_reference_trajectory_samples(include_str!(
529            "testdata/uniform2_row9_trajectory.csv"
530        ));
531        let (x, y, yaw) = model.generate_trajectory(
532            20.421202670847993,
533            0.1,
534            0.028733328960710,
535            -0.157833468427762,
536        );
537
538        assert_trajectory_samples_match(&x, &y, &yaw, &expected, (1e-9, 1e-9, 1e-9));
539    }
540
541    #[test]
542    fn test_generate_trajectory_matches_upstream_lane1_row2_samples() {
543        let model = MotionModel::with_defaults();
544        let expected =
545            parse_reference_trajectory_samples(include_str!("testdata/lane1_row2_trajectory.csv"));
546        let (x, y, yaw) = model.generate_trajectory(
547            15.865930382436854,
548            0.0,
549            0.147107848059632,
550            -0.569190510845161,
551        );
552
553        assert_trajectory_samples_match(&x, &y, &yaw, &expected, (1e-9, 1e-9, 1e-9));
554    }
555
556    #[test]
557    fn test_generate_trajectory_exact_samples_match_upstream_lane_state_sampling_test1_all_rows() {
558        assert_terminal_table_exact_samples_match(
559            include_str!("testdata/lane_state_sampling_test1.csv"),
560            include_str!("testdata/lane_state_sampling_test1_all_samples.csv"),
561            0.0,
562            (1e-9, 1e-9, 1e-9),
563        );
564    }
565
566    #[test]
567    fn test_generate_trajectory_matches_upstream_biased1_row29_samples() {
568        let model = MotionModel::with_defaults();
569        let expected = parse_reference_trajectory_samples(include_str!(
570            "testdata/biased1_row29_trajectory.csv"
571        ));
572        let (x, y, yaw) = model.generate_trajectory(
573            19.979_736_701_974_8,
574            0.0,
575            -0.006547264051110,
576            0.119218956522636,
577        );
578
579        assert_trajectory_samples_match(&x, &y, &yaw, &expected, (1e-9, 1e-9, 1e-9));
580    }
581
582    #[test]
583    fn test_generate_trajectory_matches_upstream_biased2_row14_samples() {
584        let model = MotionModel::with_defaults();
585        let expected = parse_reference_trajectory_samples(include_str!(
586            "testdata/biased2_row14_trajectory.csv"
587        ));
588        let (x, y, yaw) = model.generate_trajectory(
589            21.125_744_384_337_9,
590            0.0,
591            0.073986407526083,
592            -0.049246868802640,
593        );
594
595        assert_trajectory_samples_match(&x, &y, &yaw, &expected, (1e-9, 1e-9, 1e-9));
596    }
597
598    #[test]
599    fn test_generate_trajectory_fingerprints_match_upstream_uniform_terminal_state_sampling_test1()
600    {
601        assert_terminal_table_fingerprint_matches(
602            include_str!("testdata/uniform_terminal_state_sampling_test1.csv"),
603            include_str!("testdata/uniform_terminal_state_sampling_test1_fingerprints.csv"),
604            0.0,
605            TrajectoryFingerprintTolerance {
606                sum_x: 1e-6,
607                sum_y: 1e-6,
608                sum_yaw: 1e-6,
609                weighted_sum_x: 1e-4,
610                weighted_sum_y: 1e-4,
611                weighted_sum_yaw: 1e-4,
612                sum_x_sq: 1e-4,
613                sum_y_sq: 1e-4,
614                sum_yaw_sq: 1e-4,
615            },
616        );
617    }
618
619    #[test]
620    fn test_generate_trajectory_exact_samples_match_upstream_uniform_terminal_state_sampling_test1_all_rows(
621    ) {
622        assert_terminal_table_exact_samples_match(
623            include_str!("testdata/uniform_terminal_state_sampling_test1.csv"),
624            include_str!("testdata/uniform_terminal_state_sampling_test1_all_samples.csv"),
625            0.0,
626            (1e-9, 1e-9, 1e-9),
627        );
628    }
629
630    #[test]
631    fn test_generate_trajectory_fingerprints_match_upstream_uniform_terminal_state_sampling_test2()
632    {
633        assert_terminal_table_fingerprint_matches(
634            include_str!("testdata/uniform_terminal_state_sampling_test2.csv"),
635            include_str!("testdata/uniform_terminal_state_sampling_test2_fingerprints.csv"),
636            0.1,
637            TrajectoryFingerprintTolerance {
638                sum_x: 1e-6,
639                sum_y: 1e-6,
640                sum_yaw: 1e-6,
641                weighted_sum_x: 1e-4,
642                weighted_sum_y: 1e-4,
643                weighted_sum_yaw: 1e-4,
644                sum_x_sq: 1e-4,
645                sum_y_sq: 1e-4,
646                sum_yaw_sq: 1e-4,
647            },
648        );
649    }
650
651    #[test]
652    fn test_generate_trajectory_exact_samples_match_upstream_uniform_terminal_state_sampling_test2_all_rows(
653    ) {
654        assert_terminal_table_exact_samples_match(
655            include_str!("testdata/uniform_terminal_state_sampling_test2.csv"),
656            include_str!("testdata/uniform_terminal_state_sampling_test2_all_samples.csv"),
657            0.1,
658            (1e-9, 1e-9, 1e-9),
659        );
660    }
661
662    #[test]
663    fn test_generate_trajectory_fingerprints_match_upstream_biased_terminal_state_sampling_test1() {
664        assert_terminal_table_fingerprint_matches(
665            include_str!("testdata/biased_terminal_state_sampling_test1.csv"),
666            include_str!("testdata/biased_terminal_state_sampling_test1_fingerprints.csv"),
667            0.0,
668            TrajectoryFingerprintTolerance {
669                sum_x: 1e-6,
670                sum_y: 1e-6,
671                sum_yaw: 1e-6,
672                weighted_sum_x: 1e-4,
673                weighted_sum_y: 1e-4,
674                weighted_sum_yaw: 1e-4,
675                sum_x_sq: 1e-4,
676                sum_y_sq: 1e-4,
677                sum_yaw_sq: 1e-4,
678            },
679        );
680    }
681
682    #[test]
683    fn test_generate_trajectory_exact_samples_match_upstream_biased_terminal_state_sampling_test1_all_rows(
684    ) {
685        assert_terminal_table_exact_samples_match(
686            include_str!("testdata/biased_terminal_state_sampling_test1.csv"),
687            include_str!("testdata/biased_terminal_state_sampling_test1_all_samples.csv"),
688            0.0,
689            (1e-9, 1e-9, 1e-9),
690        );
691    }
692
693    #[test]
694    fn test_generate_trajectory_fingerprints_match_upstream_biased_terminal_state_sampling_test2() {
695        assert_terminal_table_fingerprint_matches(
696            include_str!("testdata/biased_terminal_state_sampling_test2.csv"),
697            include_str!("testdata/biased_terminal_state_sampling_test2_fingerprints.csv"),
698            0.0,
699            TrajectoryFingerprintTolerance {
700                sum_x: 1e-6,
701                sum_y: 1e-6,
702                sum_yaw: 1e-6,
703                weighted_sum_x: 1e-4,
704                weighted_sum_y: 1e-4,
705                weighted_sum_yaw: 1e-4,
706                sum_x_sq: 1e-4,
707                sum_y_sq: 1e-4,
708                sum_yaw_sq: 1e-4,
709            },
710        );
711    }
712
713    #[test]
714    fn test_generate_trajectory_fingerprints_match_upstream_lane_state_sampling_test1() {
715        assert_terminal_table_fingerprint_matches(
716            include_str!("testdata/lane_state_sampling_test1.csv"),
717            include_str!("testdata/lane_state_sampling_test1_fingerprints.csv"),
718            0.0,
719            TrajectoryFingerprintTolerance {
720                sum_x: 1e-6,
721                sum_y: 1e-6,
722                sum_yaw: 1e-6,
723                weighted_sum_x: 1e-4,
724                weighted_sum_y: 1e-4,
725                weighted_sum_yaw: 1e-4,
726                sum_x_sq: 1e-4,
727                sum_y_sq: 1e-4,
728                sum_yaw_sq: 1e-4,
729            },
730        );
731    }
732
733    #[test]
734    fn test_generate_trajectory_exact_samples_match_upstream_biased_terminal_state_sampling_test2_all_rows(
735    ) {
736        assert_terminal_table_exact_samples_match(
737            include_str!("testdata/biased_terminal_state_sampling_test2.csv"),
738            include_str!("testdata/biased_terminal_state_sampling_test2_all_samples.csv"),
739            0.0,
740            (1e-9, 1e-9, 1e-9),
741        );
742    }
743}