1use rust_robotics_core::{
9 ControlInput, Path2D, PathTracker, Point2D, RoboticsError, RoboticsResult, State2D,
10};
11
12use crate::{
13 LQRSteerConfig, LQRSteerController, PurePursuitConfig, PurePursuitController, StanleyConfig,
14 StanleyController,
15};
16
17const DEFAULT_DT: f64 = 0.1;
18const DEFAULT_MAX_LINEAR_SPEED: f64 = 8.0;
19const DEFAULT_MAX_ANGULAR_SPEED: f64 = 1.4;
20const DEFAULT_GOAL_TOLERANCE: f64 = 1.0;
21const SPEED_RESPONSE_PER_SECOND: f64 = 1.8;
22const MIN_CURVATURE_SPEED: f64 = 1e-6;
23const END_LINE_RADIUS: f64 = 5.0;
26
27#[derive(Debug, Clone, Copy, PartialEq, Eq)]
29pub enum ArenaControllerKind {
30 PurePursuit,
31 Stanley,
32 LqrSteer,
33}
34
35impl ArenaControllerKind {
36 pub const ALL: [Self; 3] = [Self::PurePursuit, Self::Stanley, Self::LqrSteer];
37
38 pub fn label(self) -> &'static str {
39 match self {
40 Self::PurePursuit => "Pure Pursuit",
41 Self::Stanley => "Stanley",
42 Self::LqrSteer => "LQR Steer",
43 }
44 }
45}
46
47#[derive(Debug, Clone, Copy, PartialEq, Eq)]
49pub enum ArenaPreset {
50 StraightRecovery,
51 SlalomRecovery,
52 HairpinRecovery,
53}
54
55impl ArenaPreset {
56 pub const ALL: [Self; 3] = [
57 Self::StraightRecovery,
58 Self::SlalomRecovery,
59 Self::HairpinRecovery,
60 ];
61
62 pub fn label(self) -> &'static str {
63 match self {
64 Self::StraightRecovery => "Straight recovery",
65 Self::SlalomRecovery => "Slalom recovery",
66 Self::HairpinRecovery => "Hairpin recovery",
67 }
68 }
69
70 pub fn slug(self) -> &'static str {
71 match self {
72 Self::StraightRecovery => "straight",
73 Self::SlalomRecovery => "slalom",
74 Self::HairpinRecovery => "hairpin",
75 }
76 }
77
78 pub fn from_slug(value: &str) -> Option<Self> {
79 match value {
80 "straight" => Some(Self::StraightRecovery),
81 "slalom" => Some(Self::SlalomRecovery),
82 "hairpin" => Some(Self::HairpinRecovery),
83 _ => None,
84 }
85 }
86
87 pub fn scenario(self, target_speed: f64, turn_rate_response_gain: f64) -> ArenaScenario {
89 let (path, max_steps) = match self {
90 Self::StraightRecovery => (straight_path(), 220),
91 Self::SlalomRecovery => (slalom_path(), 300),
92 Self::HairpinRecovery => (hairpin_path(), 420),
93 };
94 let start = path.points[0];
95 let start_yaw = path.yaw_profile().first().copied().unwrap_or(0.0);
96 ArenaScenario {
97 preset: self,
98 path,
99 initial_state: State2D::new(
100 start.x - 2.0 * start_yaw.cos() - 1.5 * start_yaw.sin(),
101 start.y - 2.0 * start_yaw.sin() + 1.5 * start_yaw.cos(),
102 start_yaw + 0.12,
103 0.5,
104 ),
105 dt: DEFAULT_DT,
106 target_speed,
107 turn_rate_response_gain,
108 max_steps,
109 max_linear_speed: DEFAULT_MAX_LINEAR_SPEED,
110 max_angular_speed: DEFAULT_MAX_ANGULAR_SPEED,
111 goal_tolerance: DEFAULT_GOAL_TOLERANCE,
112 }
113 }
114}
115
116#[derive(Debug, Clone)]
118pub struct ArenaScenario {
119 pub preset: ArenaPreset,
120 pub path: Path2D,
121 pub initial_state: State2D,
122 pub dt: f64,
123 pub target_speed: f64,
124 pub turn_rate_response_gain: f64,
126 pub max_steps: usize,
127 pub max_linear_speed: f64,
128 pub max_angular_speed: f64,
129 pub goal_tolerance: f64,
130}
131
132impl ArenaScenario {
133 pub(crate) fn validate(&self) -> RoboticsResult<()> {
134 if self.path.len() < 2 {
135 return Err(invalid("arena path must contain at least two points"));
136 }
137 if !self
138 .path
139 .points
140 .iter()
141 .all(|point| point.x.is_finite() && point.y.is_finite())
142 {
143 return Err(invalid("arena path coordinates must be finite"));
144 }
145 for (name, value) in [
146 ("dt", self.dt),
147 ("target_speed", self.target_speed),
148 ("turn_rate_response_gain", self.turn_rate_response_gain),
149 ("max_linear_speed", self.max_linear_speed),
150 ("max_angular_speed", self.max_angular_speed),
151 ("goal_tolerance", self.goal_tolerance),
152 ] {
153 if !value.is_finite() {
154 return Err(invalid(&format!("{name} must be finite")));
155 }
156 }
157 if self.dt <= 0.0
158 || self.target_speed <= 0.0
159 || !(0.0..=1.0).contains(&self.turn_rate_response_gain)
160 || self.max_steps == 0
161 || self.max_linear_speed <= 0.0
162 || self.max_angular_speed <= 0.0
163 || self.goal_tolerance <= 0.0
164 {
165 return Err(invalid(
166 "arena dynamics require positive limits and a response gain in 0..=1",
167 ));
168 }
169 if !state_is_finite(self.initial_state) {
170 return Err(invalid("arena initial state must be finite"));
171 }
172 Ok(())
173 }
174}
175
176#[derive(Debug, Clone, Copy, PartialEq)]
178pub struct ArenaSample {
179 pub time: f64,
180 pub state: State2D,
181 pub command: ControlInput,
182 pub cross_track_error: f64,
183}
184
185#[derive(Debug, Clone, Copy, PartialEq)]
187pub struct ArenaMetrics {
188 pub cross_track_rmse: f64,
189 pub final_goal_distance: f64,
190 pub max_cross_track_error: f64,
191 pub angular_command_smoothness: f64,
192}
193
194#[derive(Debug, Clone, PartialEq)]
196pub struct ArenaRun {
197 pub controller: ArenaControllerKind,
198 pub samples: Vec<ArenaSample>,
199 pub metrics: ArenaMetrics,
200}
201
202pub fn run_controller_arena(
204 scenario: &ArenaScenario,
205 controllers: &[ArenaControllerKind],
206) -> RoboticsResult<Vec<ArenaRun>> {
207 scenario.validate()?;
208 if controllers.is_empty() {
209 return Err(invalid("at least one arena controller is required"));
210 }
211 controllers
212 .iter()
213 .copied()
214 .map(|kind| run_one(scenario, kind))
215 .collect()
216}
217
218pub fn metrics_from_samples(
221 samples: &[ArenaSample],
222 goal: Point2D,
223) -> RoboticsResult<ArenaMetrics> {
224 if samples.is_empty() {
225 return Err(invalid("arena metrics require at least one sample"));
226 }
227 if !samples.iter().all(sample_is_finite) {
228 return Err(RoboticsError::NumericalError(
229 "arena samples contain a non-finite value".to_string(),
230 ));
231 }
232
233 let squared_error_sum = samples
234 .iter()
235 .map(|sample| sample.cross_track_error * sample.cross_track_error)
236 .sum::<f64>();
237 let max_cross_track_error = samples
238 .iter()
239 .map(|sample| sample.cross_track_error.abs())
240 .fold(0.0_f64, f64::max);
241 let squared_command_delta_sum = samples
242 .windows(2)
243 .map(|pair| {
244 let delta = pair[1].command.omega - pair[0].command.omega;
245 delta * delta
246 })
247 .sum::<f64>();
248 let command_delta_count = samples.len().saturating_sub(1).max(1);
249 let final_state = samples
250 .last()
251 .expect("non-empty samples were checked")
252 .state;
253
254 Ok(ArenaMetrics {
255 cross_track_rmse: (squared_error_sum / samples.len() as f64).sqrt(),
256 final_goal_distance: final_state.position().distance(&goal),
257 max_cross_track_error,
258 angular_command_smoothness: (squared_command_delta_sum / command_delta_count as f64).sqrt(),
259 })
260}
261
262fn run_one(scenario: &ArenaScenario, kind: ArenaControllerKind) -> RoboticsResult<ArenaRun> {
263 let mut tracker = build_tracker(kind, scenario);
264 let goal = *scenario
265 .path
266 .points
267 .last()
268 .expect("validated path has at least two points");
269 let points = &scenario.path.points;
275 let before_goal = points[points.len() - 2];
276 let end_direction = (goal.x - before_goal.x, goal.y - before_goal.y);
277 let past_end = |s: State2D| (s.x - goal.x) * end_direction.0 + (s.y - goal.y) * end_direction.1;
278 let mut state = scenario.initial_state;
279 let mut samples = Vec::with_capacity(scenario.max_steps + 1);
280 samples.push(ArenaSample {
281 time: 0.0,
282 state,
283 command: ControlInput::zero(),
284 cross_track_error: cross_track_error(&scenario.path, state),
285 });
286
287 for step in 1..=scenario.max_steps {
288 let requested = tracker.compute_control(&state, &scenario.path);
289 if !requested.v.is_finite() || !requested.omega.is_finite() {
290 return Err(RoboticsError::NumericalError(format!(
291 "{} returned a non-finite command",
292 kind.label()
293 )));
294 }
295
296 let command = apply_shared_response(scenario, state, requested);
299 state = propagate(state, command, scenario.dt);
300 let sample = ArenaSample {
301 time: step as f64 * scenario.dt,
302 state,
303 command,
304 cross_track_error: cross_track_error(&scenario.path, state),
305 };
306 if !sample_is_finite(&sample) {
307 return Err(RoboticsError::NumericalError(format!(
308 "{} produced a non-finite state",
309 kind.label()
310 )));
311 }
312 let previous = samples.last().expect("initial sample").state;
313 samples.push(sample);
314
315 let to_goal = state.position().distance(&goal);
316 if to_goal <= scenario.goal_tolerance {
317 break;
318 }
319 if past_end(previous) <= 0.0 && past_end(state) > 0.0 && to_goal < END_LINE_RADIUS {
320 break;
321 }
322 }
323
324 let metrics = metrics_from_samples(&samples, goal)?;
325 Ok(ArenaRun {
326 controller: kind,
327 samples,
328 metrics,
329 })
330}
331
332pub(crate) fn apply_shared_response(
337 scenario: &ArenaScenario,
338 state: State2D,
339 requested: ControlInput,
340) -> ControlInput {
341 let curvature = if requested.v.abs() > MIN_CURVATURE_SPEED {
342 requested.omega / requested.v
343 } else {
344 0.0
345 };
346 let speed_alpha = (SPEED_RESPONSE_PER_SECOND * scenario.dt).clamp(0.0, 1.0);
347 let v = (state.v + (scenario.target_speed - state.v) * speed_alpha)
348 .clamp(0.0, scenario.max_linear_speed);
349 let omega = (curvature * v * scenario.turn_rate_response_gain)
350 .clamp(-scenario.max_angular_speed, scenario.max_angular_speed);
351 ControlInput::new(v, omega)
352}
353
354pub(crate) fn build_tracker(
355 kind: ArenaControllerKind,
356 scenario: &ArenaScenario,
357) -> Box<dyn PathTracker> {
358 match kind {
359 ArenaControllerKind::PurePursuit => {
360 Box::new(PurePursuitController::new(PurePursuitConfig {
361 look_ahead_gain: 0.12,
362 look_ahead_distance: 2.4,
363 wheelbase: 2.9,
364 kp: 1.0,
365 goal_threshold: scenario.goal_tolerance,
366 }))
367 }
368 ArenaControllerKind::Stanley => Box::new(StanleyController::new(StanleyConfig {
369 k: 0.55,
370 wheelbase: 2.9,
371 kp: 1.0,
372 goal_threshold: scenario.goal_tolerance,
373 })),
374 ArenaControllerKind::LqrSteer => {
375 let mut tracker = LQRSteerController::new(LQRSteerConfig {
376 wheelbase: 2.9,
377 max_steer: 45.0_f64.to_radians(),
378 kp: 1.0,
379 q_diag: [1.0, 1.0, 1.0, 1.0],
380 r: 1.0,
381 dt: scenario.dt,
382 goal_threshold: scenario.goal_tolerance,
383 });
384 tracker.set_path_with_speed(scenario.path.clone(), scenario.target_speed);
385 Box::new(tracker)
386 }
387 }
388}
389
390pub(crate) fn propagate(state: State2D, command: ControlInput, dt: f64) -> State2D {
391 let x = state.x + command.v * state.yaw.cos() * dt;
392 let y = state.y + command.v * state.yaw.sin() * dt;
393 let yaw = normalize_angle(state.yaw + command.omega * dt);
394 State2D::new(x, y, yaw, command.v)
395}
396
397pub(crate) use rust_robotics_core::normalize_angle;
398
399pub(crate) fn cross_track_error(path: &Path2D, state: State2D) -> f64 {
400 let position = state.position();
401 path.nearest_point_index(position)
402 .map(|index| position.distance(&path.points[index]))
403 .unwrap_or(f64::INFINITY)
404}
405
406fn straight_path() -> Path2D {
407 Path2D::from_points(
408 (0..=100)
409 .map(|index| Point2D::new(index as f64 * 0.5, 0.0))
410 .collect(),
411 )
412}
413
414fn slalom_path() -> Path2D {
415 Path2D::from_points(
416 (0..=120)
417 .map(|index| {
418 let x = index as f64 * 0.5;
419 Point2D::new(x, 3.0 * (x / 7.0).sin())
420 })
421 .collect(),
422 )
423}
424
425fn hairpin_path() -> Path2D {
426 let mut points = Vec::new();
427 for index in 0..=40 {
428 points.push(Point2D::new(index as f64 * 0.5, 0.0));
429 }
430 let radius = 6.0;
431 for index in 1..=38 {
432 let theta = -std::f64::consts::FRAC_PI_2 + std::f64::consts::PI * index as f64 / 38.0;
433 points.push(Point2D::new(
434 20.0 + radius * theta.cos(),
435 6.0 + radius * theta.sin(),
436 ));
437 }
438 for index in 1..=40 {
439 points.push(Point2D::new(20.0 - index as f64 * 0.5, 12.0));
440 }
441 Path2D::from_points(points)
442}
443
444pub(crate) fn state_is_finite(state: State2D) -> bool {
445 state.x.is_finite() && state.y.is_finite() && state.yaw.is_finite() && state.v.is_finite()
446}
447
448pub(crate) fn sample_is_finite(sample: &ArenaSample) -> bool {
449 sample.time.is_finite()
450 && state_is_finite(sample.state)
451 && sample.command.v.is_finite()
452 && sample.command.omega.is_finite()
453 && sample.cross_track_error.is_finite()
454}
455
456pub(crate) fn invalid(message: &str) -> RoboticsError {
457 RoboticsError::InvalidParameter(message.to_string())
458}
459
460#[cfg(test)]
461mod tests {
462 use super::*;
463
464 fn straight(target_speed: f64) -> ArenaScenario {
465 ArenaPreset::StraightRecovery.scenario(target_speed, 0.85)
466 }
467
468 #[test]
469 fn pure_pursuit_finishes_when_the_goal_is_inside_its_look_ahead() {
470 let mut scenario = straight(3.0);
473 scenario.initial_state = State2D::new(45.0, 2.5, 0.0, 0.5);
474 let run = run_controller_arena(&scenario, &[ArenaControllerKind::PurePursuit])
475 .unwrap()
476 .remove(0);
477 assert!(
478 run.samples.len() < scenario.max_steps / 2,
479 "{} steps",
480 run.samples.len()
481 );
482 assert!(run.metrics.final_goal_distance < 1.5);
483 }
484
485 #[test]
486 fn trackers_follow_a_loop_whose_end_is_next_to_its_start() {
487 let radius = 10.0;
489 let path = Path2D::from_points(
490 (0..=340)
491 .map(|deg| {
492 let a = (deg as f64).to_radians() - std::f64::consts::FRAC_PI_2;
493 Point2D::new(radius * a.cos(), radius + radius * a.sin())
494 })
495 .collect(),
496 );
497 let length: f64 = path.points.windows(2).map(|w| w[0].distance(&w[1])).sum();
498 let mut scenario = straight(3.0);
499 scenario.initial_state = State2D::new(-1.0, 0.0, 0.0, 0.5);
500 scenario.max_steps = 600;
501 scenario.path = path;
502 for run in run_controller_arena(&scenario, &ArenaControllerKind::ALL).unwrap() {
503 let duration = run.samples.last().unwrap().time;
504 assert!(
505 duration > 0.8 * length / 3.0,
506 "{} skipped the loop: done after {duration:.1} s",
507 run.controller.label()
508 );
509 assert!(
510 run.metrics.final_goal_distance < 1.5,
511 "{}",
512 run.controller.label()
513 );
514 assert!(
515 run.metrics.max_cross_track_error < 2.0,
516 "{}",
517 run.controller.label()
518 );
519 }
520 }
521
522 #[test]
523 fn a_run_that_overshoots_the_end_stops_there() {
524 let mut scenario = straight(3.0);
528 scenario.initial_state = State2D::new(47.0, 1.8, 0.0, 0.5);
529 for run in run_controller_arena(&scenario, &ArenaControllerKind::ALL).unwrap() {
530 assert!(
531 run.metrics.final_goal_distance < 5.0,
532 "{} ended {:.1} m away",
533 run.controller.label(),
534 run.metrics.final_goal_distance
535 );
536 }
537 }
538
539 const TOLERANCE: f64 = 1e-12;
540
541 #[test]
542 fn presets_are_finite_and_continuous() {
543 for preset in ArenaPreset::ALL {
544 let scenario = preset.scenario(3.0, 0.85);
545 scenario.validate().unwrap();
546 assert!(scenario.path.len() >= 100);
547 assert!(scenario.path.total_length() > 45.0);
548 assert!(scenario.path.points.windows(2).all(|pair| {
549 let spacing = pair[0].distance(&pair[1]);
550 spacing > 0.0 && spacing < 0.7
551 }));
552 }
553 }
554
555 #[test]
556 fn every_controller_runs_every_preset_with_shared_initial_state() {
557 for preset in ArenaPreset::ALL {
558 let scenario = preset.scenario(3.0, 0.85);
559 let runs = run_controller_arena(&scenario, &ArenaControllerKind::ALL).unwrap();
560 assert_eq!(runs.len(), ArenaControllerKind::ALL.len());
561 for run in runs {
562 assert!(
563 run.samples.len() > 1,
564 "{} has no trace",
565 run.controller.label()
566 );
567 assert_eq!(run.samples[0].state, scenario.initial_state);
568 assert!(run.samples.iter().all(sample_is_finite));
569 assert!(run.samples.iter().all(|sample| {
570 sample.command.v.abs() <= scenario.max_linear_speed + TOLERANCE
571 && sample.command.omega.abs() <= scenario.max_angular_speed + TOLERANCE
572 }));
573 assert!(metrics_are_finite(run.metrics));
574 }
575 }
576 }
577
578 #[test]
579 fn repeated_runs_are_deterministic() {
580 for preset in ArenaPreset::ALL {
581 let scenario = preset.scenario(2.75, 0.72);
582 let first = run_controller_arena(&scenario, &ArenaControllerKind::ALL).unwrap();
583 let second = run_controller_arena(&scenario, &ArenaControllerKind::ALL).unwrap();
584 assert_eq!(first, second);
585 }
586 }
587
588 #[test]
589 fn stored_metrics_match_recomputation() {
590 for preset in ArenaPreset::ALL {
591 let scenario = preset.scenario(3.5, 1.0);
592 let goal = *scenario.path.points.last().unwrap();
593 for run in run_controller_arena(&scenario, &ArenaControllerKind::ALL).unwrap() {
594 let recomputed = metrics_from_samples(&run.samples, goal).unwrap();
595 assert_metrics_close(run.metrics, recomputed);
596 }
597 }
598 }
599
600 #[test]
601 fn invalid_scenarios_and_empty_controller_lists_are_rejected() {
602 let mut scenario = ArenaPreset::StraightRecovery.scenario(3.0, 1.0);
603 assert!(run_controller_arena(&scenario, &[]).is_err());
604 scenario.turn_rate_response_gain = 1.1;
605 assert!(run_controller_arena(&scenario, &ArenaControllerKind::ALL).is_err());
606 scenario = ArenaPreset::StraightRecovery.scenario(3.0, 1.0);
607 scenario.path = Path2D::new();
608 assert!(run_controller_arena(&scenario, &ArenaControllerKind::ALL).is_err());
609 }
610
611 fn metrics_are_finite(metrics: ArenaMetrics) -> bool {
612 metrics.cross_track_rmse.is_finite()
613 && metrics.final_goal_distance.is_finite()
614 && metrics.max_cross_track_error.is_finite()
615 && metrics.angular_command_smoothness.is_finite()
616 }
617
618 fn assert_metrics_close(left: ArenaMetrics, right: ArenaMetrics) {
619 assert!((left.cross_track_rmse - right.cross_track_rmse).abs() <= TOLERANCE);
620 assert!((left.final_goal_distance - right.final_goal_distance).abs() <= TOLERANCE);
621 assert!((left.max_cross_track_error - right.max_cross_track_error).abs() <= TOLERANCE);
622 assert!(
623 (left.angular_command_smoothness - right.angular_command_smoothness).abs() <= TOLERANCE
624 );
625 }
626}