1mod first_scenario;
2mod full_bucket;
3mod percentile_bucket;
4mod sampled_bucket;
5mod variance_triggered;
6
7use std::collections::VecDeque;
8use std::time::Instant;
9
10use rust_robotics_core::{
11 annotate_against_reference as annotate_reference_reports, average_coverage_ratio,
12 read_source_metrics, ControlInput, ExperimentObservation, ExperimentSamplingPlan,
13 ExperimentVariantReport, ExtensibilityMetrics, Path2D, PathTracker, Point2D, State2D,
14 VariantDescriptor,
15};
16
17use crate::{PurePursuitConfig, PurePursuitController, StanleyConfig, StanleyController};
18
19pub use first_scenario::FirstScenarioTrackingAggregation;
20pub use full_bucket::FullBucketTrackingAggregation;
21pub use percentile_bucket::PercentileBucketTrackingAggregation;
22pub use sampled_bucket::SampledBucketTrackingAggregation;
23pub use variance_triggered::VarianceTriggeredTrackingAggregation;
24
25const DT: f64 = 0.1;
26const MAX_LINEAR_SPEED: f64 = 8.0;
27const MAX_ANGULAR_SPEED: f64 = 1.4;
28
29pub type TrackingSamplingPlan = ExperimentSamplingPlan;
30
31pub trait TrackingAggregationVariant {
32 fn descriptor(&self) -> VariantDescriptor;
33 fn selected_slots(&self, total_scenarios: usize) -> Vec<usize>;
34
35 fn sampling_plan(&self, total_scenarios: usize) -> TrackingSamplingPlan {
36 TrackingSamplingPlan::static_slots(self.selected_slots(total_scenarios))
37 }
38}
39
40#[derive(Debug, Clone)]
41pub struct TrackingExperimentCase {
42 pub family_name: &'static str,
43 pub path: Path2D,
44 pub buckets: Vec<u32>,
45 base_lateral_offset: f64,
46 base_yaw_offset_deg: f64,
47 initial_speed: f64,
48 control_latency_steps: usize,
49 velocity_response_gain: f64,
50 omega_response_gain: f64,
51 goal_penalty_weight: f64,
52}
53
54#[derive(Debug, Clone)]
55pub struct TrackingObservation {
56 pub family_name: &'static str,
57 pub bucket: u32,
58 pub total_scenarios: usize,
59 pub initial_slots: Vec<usize>,
60 pub selected_slots: Vec<usize>,
61 pub escalated: bool,
62 pub pure_pursuit_bucket_median_score: f64,
63 pub stanley_bucket_median_score: f64,
64 pub pure_pursuit_min_score: f64,
65 pub pure_pursuit_max_score: f64,
66 pub stanley_min_score: f64,
67 pub stanley_max_score: f64,
68 pub stanley_wins: usize,
69}
70
71impl TrackingObservation {
72 pub fn pure_pursuit_over_stanley(&self) -> f64 {
73 self.pure_pursuit_bucket_median_score / self.stanley_bucket_median_score.max(1e-9)
74 }
75
76 pub fn winner(&self) -> &'static str {
77 if self.pure_pursuit_over_stanley() > 1.0 {
78 "Stanley"
79 } else {
80 "PurePursuit"
81 }
82 }
83
84 pub fn coverage_ratio(&self) -> f64 {
85 self.selected_slots.len() as f64 / self.total_scenarios as f64
86 }
87}
88
89impl ExperimentObservation for TrackingObservation {
90 type Key = (&'static str, u32);
91
92 fn comparison_key(&self) -> Self::Key {
93 (self.family_name, self.bucket)
94 }
95
96 fn winner_label(&self) -> &'static str {
97 TrackingObservation::winner(self)
98 }
99
100 fn ratio_value(&self) -> f64 {
101 TrackingObservation::pure_pursuit_over_stanley(self)
102 }
103
104 fn coverage_ratio(&self) -> f64 {
105 TrackingObservation::coverage_ratio(self)
106 }
107}
108
109pub type TrackingVariantReport = ExperimentVariantReport<TrackingObservation>;
110
111#[derive(Debug, Clone, Copy)]
112pub struct TrackingEvaluationConfig {
113 pub scenarios_per_bucket: usize,
114}
115
116impl Default for TrackingEvaluationConfig {
117 fn default() -> Self {
118 Self {
119 scenarios_per_bucket: 10,
120 }
121 }
122}
123
124pub fn control_tracking_process_problem() -> Vec<TrackingExperimentCase> {
125 vec![
126 TrackingExperimentCase {
127 family_name: "straight-recovery",
128 path: build_straight_path(120.0, 1.0),
129 buckets: vec![60, 120, 180],
130 base_lateral_offset: 1.2,
131 base_yaw_offset_deg: 8.0,
132 initial_speed: 0.4,
133 control_latency_steps: 0,
134 velocity_response_gain: 1.0,
135 omega_response_gain: 1.0,
136 goal_penalty_weight: 0.25,
137 },
138 TrackingExperimentCase {
139 family_name: "slalom-recovery",
140 path: build_slalom_path(110.0, 1.0),
141 buckets: vec![80, 140, 220],
142 base_lateral_offset: 1.5,
143 base_yaw_offset_deg: 11.0,
144 initial_speed: 0.5,
145 control_latency_steps: 0,
146 velocity_response_gain: 1.0,
147 omega_response_gain: 1.0,
148 goal_penalty_weight: 0.25,
149 },
150 TrackingExperimentCase {
151 family_name: "tight-turn-recovery",
152 path: build_tight_turn_path(),
153 buckets: vec![100, 180, 260],
154 base_lateral_offset: 1.8,
155 base_yaw_offset_deg: 14.0,
156 initial_speed: 0.3,
157 control_latency_steps: 0,
158 velocity_response_gain: 1.0,
159 omega_response_gain: 1.0,
160 goal_penalty_weight: 0.25,
161 },
162 ]
163}
164
165pub fn control_actuation_mismatch_process_problem() -> Vec<TrackingExperimentCase> {
166 vec![
167 TrackingExperimentCase {
168 family_name: "velocity-sag-straight",
169 path: build_straight_path(150.0, 1.0),
170 buckets: vec![80, 140, 220],
171 base_lateral_offset: 1.0,
172 base_yaw_offset_deg: 9.0,
173 initial_speed: 0.8,
174 control_latency_steps: 1,
175 velocity_response_gain: 0.78,
176 omega_response_gain: 0.92,
177 goal_penalty_weight: 0.30,
178 },
179 TrackingExperimentCase {
180 family_name: "steering-lag-slalom",
181 path: build_slalom_path(130.0, 0.8),
182 buckets: vec![100, 180, 260],
183 base_lateral_offset: 1.7,
184 base_yaw_offset_deg: 13.0,
185 initial_speed: 1.0,
186 control_latency_steps: 3,
187 velocity_response_gain: 0.94,
188 omega_response_gain: 0.68,
189 goal_penalty_weight: 0.35,
190 },
191 TrackingExperimentCase {
192 family_name: "understeer-hairpin",
193 path: build_offset_hairpin_path(),
194 buckets: vec![120, 200, 300],
195 base_lateral_offset: 2.0,
196 base_yaw_offset_deg: 16.0,
197 initial_speed: 0.9,
198 control_latency_steps: 2,
199 velocity_response_gain: 0.88,
200 omega_response_gain: 0.58,
201 goal_penalty_weight: 0.40,
202 },
203 ]
204}
205
206pub fn default_tracking_variants() -> Vec<Box<dyn TrackingAggregationVariant>> {
207 vec![
208 Box::new(FirstScenarioTrackingAggregation::new()),
209 Box::new(SampledBucketTrackingAggregation::new(vec![0, 4, 9])),
210 Box::new(PercentileBucketTrackingAggregation::new(vec![
211 0.0, 0.25, 0.5, 0.75, 1.0,
212 ])),
213 Box::new(VarianceTriggeredTrackingAggregation::new(
214 vec![0, 4, 9],
215 0.10,
216 )),
217 Box::new(FullBucketTrackingAggregation::new()),
218 ]
219}
220
221pub fn run_variant_suite(
222 variants: &[Box<dyn TrackingAggregationVariant>],
223 cases: &[TrackingExperimentCase],
224 config: TrackingEvaluationConfig,
225) -> Vec<TrackingVariantReport> {
226 let mut reports = Vec::with_capacity(variants.len());
227
228 for variant in variants {
229 let started = Instant::now();
230 let mut observations = Vec::new();
231 for case in cases {
232 for &bucket in &case.buckets {
233 observations.push(measure_bucket_observation(
234 &**variant,
235 case,
236 bucket,
237 config.scenarios_per_bucket,
238 ));
239 }
240 }
241
242 let descriptor = variant.descriptor();
243 let source_metrics = read_source_metrics(std::path::Path::new(descriptor.source_path))
244 .expect("experiment source metrics should be readable");
245 let extensibility_metrics = ExtensibilityMetrics {
246 average_coverage_ratio: average_coverage_ratio(&observations),
247 knob_count: descriptor.knob_count,
248 reports_dispersion: descriptor.reports_dispersion,
249 };
250
251 reports.push(TrackingVariantReport {
252 descriptor,
253 evaluation_runtime_ms: started.elapsed().as_secs_f64() * 1000.0,
254 observations,
255 source_metrics,
256 extensibility_metrics,
257 agreement_vs_reference: None,
258 mean_ratio_error_vs_reference: None,
259 });
260 }
261
262 annotate_reference_reports(&mut reports, "full-bucket");
263 reports
264}
265
266fn measure_bucket_observation(
267 variant: &dyn TrackingAggregationVariant,
268 case: &TrackingExperimentCase,
269 bucket: u32,
270 total_scenarios: usize,
271) -> TrackingObservation {
272 let plan = variant.sampling_plan(total_scenarios);
273 let initial_slots = normalize_slots(total_scenarios, &plan.initial_slots);
274 assert!(
275 !initial_slots.is_empty(),
276 "{} bucket {} should select at least one scenario",
277 case.family_name,
278 bucket
279 );
280
281 let mut slot_samples = measure_slot_samples(&initial_slots, case, bucket);
282 let mut selected_slots = initial_slots.clone();
283 let mut escalated = false;
284 let escalation_slots = normalize_slots(total_scenarios, &plan.escalation_slots);
285
286 if should_escalate(&slot_samples, &plan) {
287 let additional_slots: Vec<usize> = escalation_slots
288 .into_iter()
289 .filter(|slot| !selected_slots.contains(slot))
290 .collect();
291 if !additional_slots.is_empty() {
292 slot_samples.extend(measure_slot_samples(&additional_slots, case, bucket));
293 selected_slots.extend(additional_slots);
294 selected_slots.sort_unstable();
295 escalated = true;
296 }
297 }
298
299 let pure_pursuit_samples: Vec<f64> = slot_samples
300 .iter()
301 .map(|sample| sample.pure_pursuit_score)
302 .collect();
303 let stanley_samples: Vec<f64> = slot_samples
304 .iter()
305 .map(|sample| sample.stanley_score)
306 .collect();
307 let stanley_wins = slot_samples
308 .iter()
309 .filter(|sample| sample.stanley_score < sample.pure_pursuit_score)
310 .count();
311
312 TrackingObservation {
313 family_name: case.family_name,
314 bucket,
315 total_scenarios,
316 initial_slots,
317 selected_slots,
318 escalated,
319 pure_pursuit_bucket_median_score: median_value(&pure_pursuit_samples),
320 stanley_bucket_median_score: median_value(&stanley_samples),
321 pure_pursuit_min_score: min_value(&pure_pursuit_samples),
322 pure_pursuit_max_score: max_value(&pure_pursuit_samples),
323 stanley_min_score: min_value(&stanley_samples),
324 stanley_max_score: max_value(&stanley_samples),
325 stanley_wins,
326 }
327}
328
329#[derive(Debug, Clone, Copy)]
330struct SlotTrackingSample {
331 pure_pursuit_score: f64,
332 stanley_score: f64,
333}
334
335fn measure_slot_samples(
336 slots: &[usize],
337 case: &TrackingExperimentCase,
338 bucket: u32,
339) -> Vec<SlotTrackingSample> {
340 let mut samples = Vec::with_capacity(slots.len());
341 for &slot in slots {
342 let initial_state = build_initial_state(case, bucket, slot);
343 let pure_pursuit_score = simulate_pure_pursuit(case, bucket, initial_state);
344 let stanley_score = simulate_stanley(case, bucket, initial_state);
345 assert!(
346 pure_pursuit_score.is_finite(),
347 "PurePursuit score must stay finite"
348 );
349 assert!(stanley_score.is_finite(), "Stanley score must stay finite");
350 samples.push(SlotTrackingSample {
351 pure_pursuit_score,
352 stanley_score,
353 });
354 }
355 samples
356}
357
358fn should_escalate(slot_samples: &[SlotTrackingSample], plan: &TrackingSamplingPlan) -> bool {
359 if slot_samples.is_empty() || plan.escalation_slots.is_empty() {
360 return false;
361 }
362
363 let vote_split = {
364 let stanley_wins = slot_samples
365 .iter()
366 .filter(|sample| sample.stanley_score < sample.pure_pursuit_score)
367 .count();
368 stanley_wins > 0 && stanley_wins < slot_samples.len()
369 };
370 let ratio_close = plan
371 .escalate_if_ratio_margin_below
372 .map(|threshold| {
373 let pure_pursuit_samples: Vec<f64> = slot_samples
374 .iter()
375 .map(|sample| sample.pure_pursuit_score)
376 .collect();
377 let stanley_samples: Vec<f64> = slot_samples
378 .iter()
379 .map(|sample| sample.stanley_score)
380 .collect();
381 (median_value(&pure_pursuit_samples) / median_value(&stanley_samples).max(1e-9) - 1.0)
382 .abs()
383 < threshold
384 })
385 .unwrap_or(false);
386
387 (plan.escalate_if_vote_split && vote_split) || ratio_close
388}
389
390fn normalize_slots(total_scenarios: usize, slots: &[usize]) -> Vec<usize> {
391 let mut normalized = slots
392 .iter()
393 .copied()
394 .filter(|slot| *slot < total_scenarios)
395 .collect::<Vec<_>>();
396 normalized.sort_unstable();
397 normalized.dedup();
398 normalized
399}
400
401fn simulate_pure_pursuit(
402 case: &TrackingExperimentCase,
403 bucket: u32,
404 initial_state: State2D,
405) -> f64 {
406 let mut tracker = PurePursuitController::new(PurePursuitConfig {
407 look_ahead_gain: 0.12,
408 look_ahead_distance: 2.4,
409 wheelbase: 2.9,
410 kp: 1.0,
411 goal_threshold: 2.0,
412 });
413 simulate_tracker(&mut tracker, case, initial_state, bucket)
414}
415
416fn simulate_stanley(case: &TrackingExperimentCase, bucket: u32, initial_state: State2D) -> f64 {
417 let mut tracker = StanleyController::new(StanleyConfig {
418 k: 0.55,
419 wheelbase: 2.9,
420 kp: 1.0,
421 goal_threshold: 2.5,
422 });
423 simulate_tracker(&mut tracker, case, initial_state, bucket)
424}
425
426fn simulate_tracker<T: PathTracker>(
427 tracker: &mut T,
428 case: &TrackingExperimentCase,
429 mut state: State2D,
430 bucket: u32,
431) -> f64 {
432 let steps = bucket as usize;
433 let path = &case.path;
434 let goal = *path
435 .points
436 .last()
437 .expect("path should have at least one point");
438 let mut squared_error_sum = 0.0;
439 let mut control_buffer =
440 VecDeque::from(vec![ControlInput::zero(); case.control_latency_steps + 1]);
441
442 for _ in 0..steps {
443 let control = tracker.compute_control(&state, path);
444 control_buffer.push_back(control);
445 let delayed_control = control_buffer
446 .pop_front()
447 .expect("control buffer should always contain one element");
448 let actual_control = ControlInput::new(
449 delayed_control.v * case.velocity_response_gain,
450 delayed_control.omega * case.omega_response_gain,
451 );
452 state = propagate_state(state, actual_control);
453 let query = Point2D::new(state.x, state.y);
454 let nearest_idx = path.nearest_point_index(query).unwrap_or(0);
455 let nearest_point = path.points[nearest_idx];
456 let error = query.distance(&nearest_point);
457 squared_error_sum += error * error;
458 }
459
460 let rmse = (squared_error_sum / steps.max(1) as f64).sqrt();
461 let goal_distance = state.position().distance(&goal);
462 rmse + case.goal_penalty_weight * goal_distance
463}
464
465fn propagate_state(state: State2D, control: ControlInput) -> State2D {
466 let v = control.v.clamp(0.0, MAX_LINEAR_SPEED);
467 let omega = control.omega.clamp(-MAX_ANGULAR_SPEED, MAX_ANGULAR_SPEED);
468 let mut yaw = state.yaw + omega * DT;
469 while yaw > std::f64::consts::PI {
470 yaw -= 2.0 * std::f64::consts::PI;
471 }
472 while yaw < -std::f64::consts::PI {
473 yaw += 2.0 * std::f64::consts::PI;
474 }
475
476 State2D::new(
477 state.x + v * state.yaw.cos() * DT,
478 state.y + v * state.yaw.sin() * DT,
479 yaw,
480 v,
481 )
482}
483
484fn build_initial_state(case: &TrackingExperimentCase, bucket: u32, slot: usize) -> State2D {
485 let start = case.path.points[0];
486 let yaw = case.path.yaw_profile().first().copied().unwrap_or(0.0);
487 let scale = bucket as f64 / 100.0;
488 let slot_phase = slot as f64 * 0.73 + 0.4;
489 let lateral_offset = case.base_lateral_offset * scale * slot_phase.sin();
490 let backward_offset = (1.5 + slot as f64 * 0.15) * scale;
491 let yaw_offset = case.base_yaw_offset_deg.to_radians() * scale * (slot_phase * 0.7).cos();
492 let speed = (case.initial_speed + 0.08 * slot as f64).min(2.2);
493
494 let x = start.x - backward_offset * yaw.cos() - lateral_offset * yaw.sin();
495 let y = start.y - backward_offset * yaw.sin() + lateral_offset * yaw.cos();
496
497 State2D::new(x, y, yaw + yaw_offset, speed)
498}
499
500fn build_straight_path(length: f64, ds: f64) -> Path2D {
501 let steps = (length / ds).round() as usize;
502 Path2D::from_points(
503 (0..=steps)
504 .map(|i| Point2D::new(i as f64 * ds, 0.0))
505 .collect(),
506 )
507}
508
509fn build_slalom_path(length: f64, ds: f64) -> Path2D {
510 let steps = (length / ds).round() as usize;
511 Path2D::from_points(
512 (0..=steps)
513 .map(|i| {
514 let x = i as f64 * ds;
515 let y = 2.4 * (x / 8.0).sin() + 0.6 * (x / 3.5).sin();
516 Point2D::new(x, y)
517 })
518 .collect(),
519 )
520}
521
522fn build_tight_turn_path() -> Path2D {
523 let mut points = Vec::new();
524 for i in 0..=30 {
525 points.push(Point2D::new(i as f64, 0.0));
526 }
527 for i in 1..=18 {
528 let theta = -std::f64::consts::FRAC_PI_2 + i as f64 / 18.0 * std::f64::consts::FRAC_PI_2;
529 points.push(Point2D::new(
530 30.0 + 10.0 * theta.cos(),
531 10.0 + 10.0 * theta.sin(),
532 ));
533 }
534 for i in 1..=40 {
535 points.push(Point2D::new(40.0, 10.0 + i as f64));
536 }
537 Path2D::from_points(points)
538}
539
540fn build_offset_hairpin_path() -> Path2D {
541 let mut points = Vec::new();
542 for i in 0..=40 {
543 points.push(Point2D::new(i as f64, 0.4 * (i as f64 / 6.0).sin()));
544 }
545 for i in 1..=24 {
546 let theta = -std::f64::consts::FRAC_PI_2 + i as f64 / 24.0 * (std::f64::consts::PI * 0.92);
547 points.push(Point2D::new(
548 40.0 + 12.0 * theta.cos(),
549 12.0 + 12.0 * theta.sin(),
550 ));
551 }
552 for i in 1..=46 {
553 points.push(Point2D::new(
554 28.0 - i as f64 * 0.85,
555 24.0 + 0.8 * (i as f64 / 5.0).sin(),
556 ));
557 }
558 Path2D::from_points(points)
559}
560
561fn median_value(samples: &[f64]) -> f64 {
562 assert!(
563 !samples.is_empty(),
564 "median_value requires at least one sample"
565 );
566 let mut sorted = samples.to_vec();
567 sorted.sort_by(|a, b| a.partial_cmp(b).unwrap());
568 sorted[sorted.len() / 2]
569}
570
571fn min_value(samples: &[f64]) -> f64 {
572 samples.iter().copied().fold(f64::INFINITY, f64::min)
573}
574
575fn max_value(samples: &[f64]) -> f64 {
576 samples.iter().copied().fold(f64::NEG_INFINITY, f64::max)
577}
578
579#[cfg(test)]
580mod tests {
581 use super::*;
582
583 #[test]
584 fn first_variant_selects_one_slot() {
585 let variant = FirstScenarioTrackingAggregation::new();
586 assert_eq!(variant.selected_slots(10), vec![0]);
587 }
588
589 #[test]
590 fn percentile_variant_maps_percentiles_to_unique_slots() {
591 let variant = PercentileBucketTrackingAggregation::new(vec![0.0, 0.25, 0.5, 0.75, 1.0]);
592 assert_eq!(variant.selected_slots(10), vec![0, 2, 5, 7, 9]);
593 assert_eq!(variant.selected_slots(1), vec![0]);
594 }
595
596 #[test]
597 fn suite_runs_on_control_tracking_problem() {
598 let variants = default_tracking_variants();
599 let mut problem = control_tracking_process_problem();
600 problem.truncate(1);
601 problem[0].buckets = vec![60];
602 let reports = run_variant_suite(&variants, &problem, TrackingEvaluationConfig::default());
603 assert_eq!(reports.len(), 5);
604 assert!(reports
605 .iter()
606 .all(|report| !report.observations.is_empty() && report.evaluation_runtime_ms >= 0.0));
607 }
608
609 #[test]
610 fn representative_scores_are_finite() {
611 let problem = control_tracking_process_problem();
612 let initial_state = build_initial_state(&problem[1], 80, 3);
613 let pp = simulate_pure_pursuit(&problem[1], 80, initial_state);
614 let stanley = simulate_stanley(&problem[1], 80, initial_state);
615 assert!(pp.is_finite());
616 assert!(stanley.is_finite());
617 }
618
619 #[test]
620 fn actuation_mismatch_problem_runs_and_stays_finite() {
621 let variants = default_tracking_variants();
622 let mut problem = control_actuation_mismatch_process_problem();
623 problem.truncate(1);
624 problem[0].buckets = vec![80];
625 let reports = run_variant_suite(&variants, &problem, TrackingEvaluationConfig::default());
626 assert_eq!(reports.len(), 5);
627 assert!(reports.iter().all(|report| {
628 !report.observations.is_empty()
629 && report.observations.iter().all(|observation| {
630 observation.pure_pursuit_bucket_median_score.is_finite()
631 && observation.stanley_bucket_median_score.is_finite()
632 })
633 }));
634 }
635}