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