1#[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#[derive(Debug, Clone)]
32pub struct MotionModelConfig {
33 pub wheelbase: f64,
35 pub ds: f64,
37 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, }
48 }
49}
50
51pub 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 pub fn wheelbase(&self) -> f64 {
67 self.config.wheelbase
68 }
69
70 pub fn ds(&self) -> f64 {
72 self.config.ds
73 }
74
75 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 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 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 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}