1use crate::controller_arena::{
16 apply_shared_response, build_tracker, cross_track_error, invalid, metrics_from_samples,
17 normalize_angle, propagate, state_is_finite, ArenaControllerKind, ArenaMetrics, ArenaSample,
18 ArenaScenario,
19};
20use rust_robotics_core::{
21 ControlInput, Path2D, PathTracker, Point2D, RoboticsError, RoboticsResult, State2D,
22};
23
24#[derive(Debug, Clone, Copy, PartialEq)]
26pub enum MetaControlPolicy {
27 Fixed(ArenaControllerKind),
29 SwitchOnError {
32 error_threshold: f64,
33 error_mode: ArenaControllerKind,
34 cruise_mode: ArenaControllerKind,
35 },
36 SwitchOnCurvature {
39 curvature_threshold: f64,
40 tight_mode: ArenaControllerKind,
41 smooth_mode: ArenaControllerKind,
42 },
43}
44
45impl MetaControlPolicy {
46 pub fn label(self) -> &'static str {
47 match self {
48 MetaControlPolicy::Fixed(kind) => match kind {
49 ArenaControllerKind::PurePursuit => "fixed: pure pursuit",
50 ArenaControllerKind::Stanley => "fixed: stanley",
51 ArenaControllerKind::LqrSteer => "fixed: lqr steer",
52 },
53 MetaControlPolicy::SwitchOnError { .. } => "meta: switch on error",
54 MetaControlPolicy::SwitchOnCurvature { .. } => "meta: switch on curvature",
55 }
56 }
57
58 pub fn validate(self) -> RoboticsResult<()> {
59 match self {
60 MetaControlPolicy::Fixed(_) => Ok(()),
61 MetaControlPolicy::SwitchOnError {
62 error_threshold, ..
63 } => {
64 if !error_threshold.is_finite() || error_threshold <= 0.0 {
65 Err(invalid(
66 "switch-on-error threshold must be finite and positive",
67 ))
68 } else {
69 Ok(())
70 }
71 }
72 MetaControlPolicy::SwitchOnCurvature {
73 curvature_threshold,
74 ..
75 } => {
76 if !curvature_threshold.is_finite() || curvature_threshold <= 0.0 {
77 Err(invalid(
78 "switch-on-curvature threshold must be finite and positive",
79 ))
80 } else {
81 Ok(())
82 }
83 }
84 }
85 }
86}
87
88pub struct MetaController {
92 policy: MetaControlPolicy,
93 goal_tolerance: f64,
94 trackers: [Box<dyn PathTracker>; 3],
95 active: Option<ArenaControllerKind>,
96}
97
98impl MetaController {
99 pub fn new(scenario: &ArenaScenario, policy: MetaControlPolicy) -> RoboticsResult<Self> {
100 policy.validate()?;
101 scenario.validate()?;
102 Ok(Self {
103 policy,
104 goal_tolerance: scenario.goal_tolerance,
105 trackers: ArenaControllerKind::ALL.map(|kind| build_tracker(kind, scenario)),
106 active: None,
107 })
108 }
109
110 pub fn policy(&self) -> MetaControlPolicy {
111 self.policy
112 }
113
114 pub fn active_mode(&self) -> Option<ArenaControllerKind> {
117 self.active
118 }
119
120 pub fn select_mode(&self, state: &State2D, path: &Path2D) -> ArenaControllerKind {
123 match self.policy {
124 MetaControlPolicy::Fixed(kind) => kind,
125 MetaControlPolicy::SwitchOnError {
126 error_threshold,
127 error_mode,
128 cruise_mode,
129 } => {
130 let error = cross_track_error(path, *state);
131 if error > error_threshold {
132 error_mode
133 } else {
134 cruise_mode
135 }
136 }
137 MetaControlPolicy::SwitchOnCurvature {
138 curvature_threshold,
139 tight_mode,
140 smooth_mode,
141 } => {
142 let curvature = path_curvature_at(path, state);
143 if curvature > curvature_threshold {
144 tight_mode
145 } else {
146 smooth_mode
147 }
148 }
149 }
150 }
151
152 fn tracker_for(&mut self, kind: ArenaControllerKind) -> &mut Box<dyn PathTracker> {
153 &mut self.trackers[kind_index(kind)]
154 }
155}
156
157impl PathTracker for MetaController {
158 fn compute_control(&mut self, current_state: &State2D, path: &Path2D) -> ControlInput {
159 let kind = self.select_mode(current_state, path);
160 self.active = Some(kind);
161 self.tracker_for(kind).compute_control(current_state, path)
162 }
163
164 fn is_goal_reached(&self, current_state: &State2D, goal: Point2D) -> bool {
165 current_state.position().distance(&goal) < self.goal_tolerance
166 }
167}
168
169#[derive(Debug, Clone, Copy, PartialEq)]
172pub struct MetaControlSample {
173 pub time: f64,
174 pub state: State2D,
175 pub command: ControlInput,
176 pub cross_track_error: f64,
177 pub active_mode: ArenaControllerKind,
178}
179
180#[derive(Debug, Clone, PartialEq)]
182pub struct MetaControlRun {
183 pub policy: MetaControlPolicy,
184 pub samples: Vec<MetaControlSample>,
185 pub metrics: ArenaMetrics,
186 pub switch_count: usize,
187}
188
189impl MetaControlRun {
190 pub fn mode_counts(&self) -> [usize; 3] {
193 let mut counts = [0usize; 3];
194 for sample in &self.samples {
195 counts[kind_index(sample.active_mode)] += 1;
196 }
197 counts
198 }
199
200 pub fn mode_share(&self, mode: ArenaControllerKind) -> f64 {
202 let total = self.samples.len().max(1) as f64;
203 let used = self
204 .samples
205 .iter()
206 .filter(|sample| sample.active_mode == mode)
207 .count() as f64;
208 used / total
209 }
210}
211
212pub fn run_meta_control(
214 scenario: &ArenaScenario,
215 policy: MetaControlPolicy,
216) -> RoboticsResult<MetaControlRun> {
217 policy.validate()?;
218 scenario.validate()?;
219 let goal = *scenario
220 .path
221 .points
222 .last()
223 .ok_or_else(|| invalid("meta-control path must contain at least two points"))?;
224
225 let mut controller = MetaController::new(scenario, policy)?;
226 let mut state = scenario.initial_state;
227 let initial_mode = controller.select_mode(&state, &scenario.path);
228 let mut samples = Vec::with_capacity(scenario.max_steps + 1);
229 samples.push(MetaControlSample {
230 time: 0.0,
231 state,
232 command: ControlInput::zero(),
233 cross_track_error: cross_track_error(&scenario.path, state),
234 active_mode: initial_mode,
235 });
236
237 let mut switches = 0usize;
238 let mut previous_mode: Option<ArenaControllerKind> = None;
239 for step in 1..=scenario.max_steps {
240 let requested = controller.compute_control(&state, &scenario.path);
241 let mode = controller
242 .active_mode()
243 .expect("compute_control always selects a mode");
244 if !requested.v.is_finite() || !requested.omega.is_finite() {
245 return Err(RoboticsError::NumericalError(format!(
246 "{} returned a non-finite command",
247 mode.label()
248 )));
249 }
250
251 if let Some(previous) = previous_mode {
252 if previous != mode {
253 switches += 1;
254 }
255 }
256 previous_mode = Some(mode);
257
258 let command = apply_shared_response(scenario, state, requested);
259 state = propagate(state, command, scenario.dt);
260 let sample = MetaControlSample {
261 time: step as f64 * scenario.dt,
262 state,
263 command,
264 cross_track_error: cross_track_error(&scenario.path, state),
265 active_mode: mode,
266 };
267 if !meta_sample_is_finite(&sample) {
268 return Err(RoboticsError::NumericalError(format!(
269 "{} produced a non-finite state",
270 mode.label()
271 )));
272 }
273 samples.push(sample);
274
275 if state.position().distance(&goal) <= scenario.goal_tolerance {
276 break;
277 }
278 }
279
280 let metrics = metrics_from_samples(&to_arena_samples(&samples), goal)?;
281 Ok(MetaControlRun {
282 policy,
283 samples,
284 metrics,
285 switch_count: switches,
286 })
287}
288
289pub fn meta_metrics_from_samples(
292 samples: &[MetaControlSample],
293 goal: Point2D,
294) -> RoboticsResult<ArenaMetrics> {
295 metrics_from_samples(&to_arena_samples(samples), goal)
296}
297
298fn to_arena_samples(samples: &[MetaControlSample]) -> Vec<ArenaSample> {
299 samples
300 .iter()
301 .map(|sample| ArenaSample {
302 time: sample.time,
303 state: sample.state,
304 command: sample.command,
305 cross_track_error: sample.cross_track_error,
306 })
307 .collect()
308}
309
310fn kind_index(kind: ArenaControllerKind) -> usize {
311 match kind {
312 ArenaControllerKind::PurePursuit => 0,
313 ArenaControllerKind::Stanley => 1,
314 ArenaControllerKind::LqrSteer => 2,
315 }
316}
317
318fn meta_sample_is_finite(sample: &MetaControlSample) -> bool {
319 sample.time.is_finite()
320 && state_is_finite(sample.state)
321 && sample.command.v.is_finite()
322 && sample.command.omega.is_finite()
323 && sample.cross_track_error.is_finite()
324}
325
326fn path_curvature_at(path: &Path2D, state: &State2D) -> f64 {
328 let Some(index) = path.nearest_point_index(state.position()) else {
329 return 0.0;
330 };
331 let yaw = path.yaw_profile();
332 let n = yaw.len();
333 if n < 3 {
334 return 0.0;
335 }
336 let i = index.clamp(1, n - 2);
337 let d1 = normalize_angle(yaw[i + 1] - yaw[i]);
338 let d2 = normalize_angle(yaw[i] - yaw[i - 1]);
339 let ds = path.points[i].distance(&path.points[i - 1]).max(1e-3);
340 (d1.abs() + d2.abs()).min(core::f64::consts::PI) / (2.0 * ds)
341}
342
343#[cfg(test)]
344mod tests {
345 use super::*;
346
347 const TOLERANCE: f64 = 1e-12;
348
349 fn assert_metrics_close(left: ArenaMetrics, right: ArenaMetrics) {
350 assert!((left.cross_track_rmse - right.cross_track_rmse).abs() <= TOLERANCE);
351 assert!((left.final_goal_distance - right.final_goal_distance).abs() <= TOLERANCE);
352 assert!((left.max_cross_track_error - right.max_cross_track_error).abs() <= TOLERANCE);
353 assert!(
354 (left.angular_command_smoothness - right.angular_command_smoothness).abs() <= TOLERANCE
355 );
356 }
357
358 #[test]
359 fn fixed_policy_reproduces_arena_run() {
360 for preset in crate::controller_arena::ArenaPreset::ALL {
361 let scenario = preset.scenario(3.0, 0.85);
362 for kind in ArenaControllerKind::ALL {
363 let arena_run = crate::run_controller_arena(&scenario, &[kind])
364 .unwrap()
365 .pop()
366 .unwrap();
367 let meta_run = run_meta_control(&scenario, MetaControlPolicy::Fixed(kind)).unwrap();
368 assert_eq!(meta_run.samples.len(), arena_run.samples.len());
369 for (meta, arena) in meta_run.samples.iter().zip(arena_run.samples.iter()) {
370 assert_eq!(meta.state, arena.state);
371 assert_eq!(meta.command, arena.command);
372 assert!((meta.cross_track_error - arena.cross_track_error).abs() <= TOLERANCE);
373 }
374 assert_metrics_close(meta_run.metrics, arena_run.metrics);
375 assert_eq!(meta_run.switch_count, 0);
376 }
377 }
378 }
379
380 #[test]
381 fn error_policy_switches_to_correction_mode_then_cruises() {
382 let scenario = crate::controller_arena::ArenaPreset::StraightRecovery.scenario(3.0, 0.85);
383 let policy = MetaControlPolicy::SwitchOnError {
384 error_threshold: 0.5,
385 error_mode: ArenaControllerKind::Stanley,
386 cruise_mode: ArenaControllerKind::PurePursuit,
387 };
388 let run = run_meta_control(&scenario, policy).unwrap();
389 assert_eq!(run.samples[0].active_mode, ArenaControllerKind::Stanley);
393 assert!(run.mode_counts()[1] > 0, "Stanley never used");
394 assert!(run.mode_counts()[0] > 0, "Pure Pursuit never used");
395 assert!(run.switch_count >= 1);
396 assert!(run.samples.last().unwrap().cross_track_error.abs() < 0.5);
397 }
398
399 #[test]
400 fn curvature_policy_uses_tight_mode_on_hairpin_arc() {
401 let scenario = crate::controller_arena::ArenaPreset::HairpinRecovery.scenario(3.0, 0.85);
402 let policy = MetaControlPolicy::SwitchOnCurvature {
403 curvature_threshold: 0.10,
404 tight_mode: ArenaControllerKind::LqrSteer,
405 smooth_mode: ArenaControllerKind::PurePursuit,
406 };
407 let run = run_meta_control(&scenario, policy).unwrap();
408 assert!(run.mode_counts()[2] > 0, "LQR Steer never used on the arc");
411 assert!(
412 run.mode_counts()[0] > 0,
413 "Pure Pursuit never used on straights"
414 );
415 assert!(run.switch_count >= 1);
416 }
417
418 #[test]
419 fn repeated_runs_are_deterministic() {
420 let scenario = crate::controller_arena::ArenaPreset::SlalomRecovery.scenario(2.75, 0.72);
421 let policy = MetaControlPolicy::SwitchOnError {
422 error_threshold: 0.4,
423 error_mode: ArenaControllerKind::Stanley,
424 cruise_mode: ArenaControllerKind::PurePursuit,
425 };
426 let first = run_meta_control(&scenario, policy).unwrap();
427 let second = run_meta_control(&scenario, policy).unwrap();
428 assert_eq!(first, second);
429 }
430
431 #[test]
432 fn invalid_policies_are_rejected() {
433 let scenario = crate::controller_arena::ArenaPreset::StraightRecovery.scenario(3.0, 1.0);
434 assert!(run_meta_control(
435 &scenario,
436 MetaControlPolicy::SwitchOnError {
437 error_threshold: 0.0,
438 error_mode: ArenaControllerKind::Stanley,
439 cruise_mode: ArenaControllerKind::PurePursuit,
440 }
441 )
442 .is_err());
443 assert!(run_meta_control(
444 &scenario,
445 MetaControlPolicy::SwitchOnCurvature {
446 curvature_threshold: f64::NAN,
447 tight_mode: ArenaControllerKind::LqrSteer,
448 smooth_mode: ArenaControllerKind::PurePursuit,
449 }
450 )
451 .is_err());
452 }
453
454 #[test]
455 fn switch_count_matches_adjacent_mode_changes() {
456 let scenario = crate::controller_arena::ArenaPreset::SlalomRecovery.scenario(3.0, 0.85);
457 let policy = MetaControlPolicy::SwitchOnCurvature {
458 curvature_threshold: 0.05,
459 tight_mode: ArenaControllerKind::Stanley,
460 smooth_mode: ArenaControllerKind::LqrSteer,
461 };
462 let run = run_meta_control(&scenario, policy).unwrap();
463 let adjacent = run
464 .samples
465 .windows(2)
466 .filter(|pair| pair[0].active_mode != pair[1].active_mode)
467 .count();
468 assert_eq!(run.switch_count, adjacent);
469 }
470
471 #[test]
472 fn stored_metrics_match_recomputation() {
473 let scenario = crate::controller_arena::ArenaPreset::StraightRecovery.scenario(3.5, 1.0);
474 let goal = *scenario.path.points.last().unwrap();
475 let policy = MetaControlPolicy::SwitchOnError {
476 error_threshold: 0.5,
477 error_mode: ArenaControllerKind::Stanley,
478 cruise_mode: ArenaControllerKind::PurePursuit,
479 };
480 let run = run_meta_control(&scenario, policy).unwrap();
481 let recomputed = meta_metrics_from_samples(&run.samples, goal).unwrap();
482 assert_metrics_close(run.metrics, recomputed);
483 }
484}