1use crate::spline_course::calc_spline_course;
12use rust_robotics_core::normalize_angle;
13use rust_robotics_core::{ControlInput, Path2D, PathTracker, Point2D, State2D};
14
15#[derive(Debug, Clone, Copy)]
17pub struct VehicleState {
18 pub x: f64,
19 pub y: f64,
20 pub yaw: f64,
21 pub v: f64,
22 pub wheelbase: f64,
23}
24
25impl VehicleState {
26 pub fn new(x: f64, y: f64, yaw: f64, v: f64, wheelbase: f64) -> Self {
27 VehicleState {
28 x,
29 y,
30 yaw,
31 v,
32 wheelbase,
33 }
34 }
35
36 pub fn update(&mut self, a: f64, delta: f64, dt: f64) {
38 self.x += self.v * self.yaw.cos() * dt;
39 self.y += self.v * self.yaw.sin() * dt;
40 self.yaw += self.v / self.wheelbase * delta.tan() * dt;
41 self.v += a * dt;
42 }
43
44 pub fn rear_axle(&self) -> (f64, f64) {
46 (self.x, self.y)
47 }
48
49 pub fn to_state2d(&self) -> State2D {
50 State2D::new(self.x, self.y, self.yaw, self.v)
51 }
52}
53
54impl From<State2D> for VehicleState {
55 fn from(s: State2D) -> Self {
56 VehicleState::new(s.x, s.y, s.yaw, s.v, 2.9) }
58}
59
60#[derive(Debug, Clone)]
62pub struct RearWheelFeedbackConfig {
63 pub kth: f64,
65 pub ke: f64,
67 pub wheelbase: f64,
69 pub kp: f64,
71 pub goal_threshold: f64,
73 pub max_steer: f64,
75}
76
77impl Default for RearWheelFeedbackConfig {
78 fn default() -> Self {
79 Self {
80 kth: 1.0,
81 ke: 0.5,
82 wheelbase: 2.9,
83 kp: 1.0,
84 goal_threshold: 0.5,
85 max_steer: 45.0_f64.to_radians(),
86 }
87 }
88}
89
90pub struct RearWheelFeedbackController {
92 config: RearWheelFeedbackConfig,
93 path: Path2D,
94 path_yaw: Vec<f64>,
95 path_curvature: Vec<f64>,
96 last_target_idx: usize,
97}
98
99impl RearWheelFeedbackController {
100 pub fn new(config: RearWheelFeedbackConfig) -> Self {
102 RearWheelFeedbackController {
103 config,
104 path: Path2D::new(),
105 path_yaw: Vec::new(),
106 path_curvature: Vec::new(),
107 last_target_idx: 0,
108 }
109 }
110
111 pub fn with_params(kth: f64, ke: f64, wheelbase: f64) -> Self {
113 let config = RearWheelFeedbackConfig {
114 kth,
115 ke,
116 wheelbase,
117 ..Default::default()
118 };
119 Self::new(config)
120 }
121
122 pub fn set_path(&mut self, path: Path2D) {
124 self.path_yaw = self.compute_path_yaw(&path);
125 self.path_curvature = self.compute_path_curvature(&path);
126 self.path = path;
127 self.last_target_idx = 0;
128 }
129
130 pub fn set_path_with_info(&mut self, path: Path2D, yaw: Vec<f64>, curvature: Vec<f64>) {
132 self.path = path;
133 self.path_yaw = yaw;
134 self.path_curvature = curvature;
135 self.last_target_idx = 0;
136 }
137
138 pub fn get_path(&self) -> &Path2D {
140 &self.path
141 }
142
143 fn compute_path_yaw(&self, path: &Path2D) -> Vec<f64> {
145 path.yaw_profile()
146 }
147
148 fn compute_path_curvature(&self, path: &Path2D) -> Vec<f64> {
150 let n = path.len();
151 if n < 3 {
152 return vec![0.0; n];
153 }
154
155 let mut curvature = Vec::with_capacity(n);
156
157 curvature.push(0.0);
159
160 for i in 1..n - 1 {
162 let p0 = &path.points[i - 1];
163 let p1 = &path.points[i];
164 let p2 = &path.points[i + 1];
165
166 let dx1 = p1.x - p0.x;
167 let dy1 = p1.y - p0.y;
168 let dx2 = p2.x - p1.x;
169 let dy2 = p2.y - p1.y;
170
171 let ds1 = (dx1 * dx1 + dy1 * dy1).sqrt();
172 let ds2 = (dx2 * dx2 + dy2 * dy2).sqrt();
173
174 if ds1 > 1e-6 && ds2 > 1e-6 {
175 let yaw1 = dy1.atan2(dx1);
176 let yaw2 = dy2.atan2(dx2);
177 let dyaw = normalize_angle(yaw2 - yaw1);
178 let ds = (ds1 + ds2) / 2.0;
179 curvature.push(dyaw / ds);
180 } else {
181 curvature.push(0.0);
182 }
183 }
184
185 curvature.push(*curvature.last().unwrap_or(&0.0));
187
188 curvature
189 }
190
191 fn calc_target_index(&self, state: &VehicleState) -> (usize, f64) {
193 let (rx, ry) = state.rear_axle();
194 let query = Point2D::new(rx, ry);
195 let min_idx = self
196 .path
197 .nearest_point_index_from(query, self.last_target_idx)
198 .unwrap_or(0);
199
200 let target_point = &self.path.points[min_idx];
202 let dx = rx - target_point.x;
203 let dy = ry - target_point.y;
204
205 let path_yaw = if min_idx < self.path_yaw.len() {
207 self.path_yaw[min_idx]
208 } else {
209 0.0
210 };
211
212 let error = -path_yaw.sin() * dx + path_yaw.cos() * dy;
214
215 (min_idx, error)
216 }
217
218 pub fn compute_steering(&mut self, state: &VehicleState) -> f64 {
224 if self.path.is_empty() {
225 return 0.0;
226 }
227
228 let (mut target_idx, e) = self.calc_target_index(state);
229
230 if self.last_target_idx >= target_idx {
232 target_idx = self.last_target_idx;
233 }
234 self.last_target_idx = target_idx;
235
236 let yaw_ref = if target_idx < self.path_yaw.len() {
238 self.path_yaw[target_idx]
239 } else {
240 *self.path_yaw.last().unwrap_or(&0.0)
241 };
242
243 let k = if target_idx < self.path_curvature.len() {
244 self.path_curvature[target_idx]
245 } else {
246 *self.path_curvature.last().unwrap_or(&0.0)
247 };
248
249 let th_e = normalize_angle(state.yaw - yaw_ref);
251
252 let v = state.v;
254 let v_abs = v.abs().max(0.1); let sinc = if th_e.abs() < 1e-6 {
259 1.0
260 } else {
261 th_e.sin() / th_e
262 };
263
264 let denom = 1.0 - k * e;
266 let denom_safe = if denom.abs() < 0.01 {
267 0.01_f64.copysign(denom)
268 } else {
269 denom
270 };
271
272 let omega = v * k * th_e.cos() / denom_safe
273 - self.config.kth * v_abs * th_e
274 - self.config.ke * v * sinc * e;
275
276 let delta = if v.abs() > 0.01 {
278 (self.config.wheelbase * omega / v).atan()
279 } else {
280 0.0
281 };
282
283 delta.clamp(-self.config.max_steer, self.config.max_steer)
285 }
286
287 pub fn compute_acceleration(&self, target_speed: f64, current_speed: f64) -> f64 {
289 self.config.kp * (target_speed - current_speed)
290 }
291
292 pub fn is_goal_reached_vehicle(&self, state: &VehicleState) -> bool {
294 if let Some(goal) = self.path.points.last() {
295 let dx = state.x - goal.x;
296 let dy = state.y - goal.y;
297 (dx * dx + dy * dy).sqrt() < self.config.goal_threshold
298 } else {
299 true
300 }
301 }
302
303 pub fn planning(
305 &mut self,
306 waypoints: Vec<(f64, f64)>,
307 target_speed: f64,
308 ds: f64,
309 ) -> Option<Vec<(f64, f64)>> {
310 if waypoints.len() < 2 {
311 return None;
312 }
313
314 let ax: Vec<f64> = waypoints.iter().map(|p| p.0).collect();
316 let ay: Vec<f64> = waypoints.iter().map(|p| p.1).collect();
317 let (cx, cy, cyaw, ck, _) = calc_spline_course(&ax, &ay, ds);
318
319 let path = Path2D::from_points(
321 cx.iter()
322 .zip(cy.iter())
323 .map(|(&x, &y)| Point2D::new(x, y))
324 .collect(),
325 );
326 self.set_path_with_info(path, cyaw, ck);
327
328 let mut state = VehicleState::new(
330 waypoints[0].0,
331 waypoints[0].1,
332 self.path_yaw.first().copied().unwrap_or(0.0),
333 0.0,
334 self.config.wheelbase,
335 );
336
337 let mut trajectory = vec![(state.x, state.y)];
338 let dt = 0.1;
339 let t_max = 100.0;
340 let mut time = 0.0;
341
342 while time < t_max {
343 let ai = self.compute_acceleration(target_speed, state.v);
344 let di = self.compute_steering(&state);
345 state.update(ai, di, dt);
346 time += dt;
347
348 trajectory.push((state.x, state.y));
349
350 if self.is_goal_reached_vehicle(&state) {
351 break;
352 }
353 }
354
355 Some(trajectory)
356 }
357}
358
359impl PathTracker for RearWheelFeedbackController {
360 fn compute_control(&mut self, current_state: &State2D, path: &Path2D) -> ControlInput {
361 if self.path != *path {
363 self.set_path(path.clone());
364 }
365
366 let vehicle_state = VehicleState::new(
367 current_state.x,
368 current_state.y,
369 current_state.yaw,
370 current_state.v,
371 self.config.wheelbase,
372 );
373
374 let delta = self.compute_steering(&vehicle_state);
375
376 let target_speed = 5.0; let v = current_state.v + self.compute_acceleration(target_speed, current_state.v) * 0.1;
379
380 let omega = v * delta.tan() / self.config.wheelbase;
382
383 ControlInput::new(v, omega)
384 }
385
386 fn is_goal_reached(&self, current_state: &State2D, goal: Point2D) -> bool {
387 let dx = current_state.x - goal.x;
388 let dy = current_state.y - goal.y;
389 (dx * dx + dy * dy).sqrt() < self.config.goal_threshold
390 }
391}
392
393#[cfg(test)]
394mod tests {
395 use super::*;
396 use std::f64::consts::PI;
397
398 #[test]
399 fn test_rear_wheel_feedback_creation() {
400 let config = RearWheelFeedbackConfig::default();
401 let controller = RearWheelFeedbackController::new(config);
402 assert!(controller.path.is_empty());
403 }
404
405 #[test]
406 fn test_rear_wheel_feedback_config_defaults() {
407 let config = RearWheelFeedbackConfig::default();
408 assert!((config.kth - 1.0).abs() < 1e-10);
409 assert!((config.ke - 0.5).abs() < 1e-10);
410 assert!((config.wheelbase - 2.9).abs() < 1e-10);
411 }
412
413 #[test]
414 fn test_rear_wheel_feedback_set_path() {
415 let mut controller = RearWheelFeedbackController::with_params(1.0, 0.5, 2.9);
416 let path = Path2D::from_points(vec![
417 Point2D::new(0.0, 0.0),
418 Point2D::new(1.0, 0.0),
419 Point2D::new(2.0, 0.0),
420 ]);
421 controller.set_path(path);
422 assert_eq!(controller.get_path().len(), 3);
423 assert_eq!(controller.path_yaw.len(), 3);
424 assert_eq!(controller.path_curvature.len(), 3);
425 }
426
427 #[test]
428 fn test_rear_wheel_feedback_normalize_angle() {
429 assert!((normalize_angle(3.0 * PI) - PI).abs() < 0.01);
430 assert!((normalize_angle(-3.0 * PI) + PI).abs() < 0.01);
431 assert!((normalize_angle(0.5)).abs() - 0.5 < 0.01);
432 }
433
434 #[test]
435 fn test_rear_wheel_feedback_steering_straight_path() {
436 let mut controller = RearWheelFeedbackController::with_params(1.0, 0.5, 2.9);
437 let path = Path2D::from_points(vec![
438 Point2D::new(0.0, 0.0),
439 Point2D::new(10.0, 0.0),
440 Point2D::new(20.0, 0.0),
441 Point2D::new(30.0, 0.0),
442 ]);
443 controller.set_path(path);
444
445 let state = VehicleState::new(5.0, 0.0, 0.0, 5.0, 2.9);
447 let steering = controller.compute_steering(&state);
448
449 assert!(steering.abs() < 0.1);
451 }
452
453 #[test]
454 fn test_rear_wheel_feedback_steering_lateral_error() {
455 let mut controller = RearWheelFeedbackController::with_params(1.0, 0.5, 2.9);
456 let path = Path2D::from_points(vec![
457 Point2D::new(0.0, 0.0),
458 Point2D::new(10.0, 0.0),
459 Point2D::new(20.0, 0.0),
460 Point2D::new(30.0, 0.0),
461 ]);
462 controller.set_path(path);
463
464 let state = VehicleState::new(5.0, 2.0, 0.1, 5.0, 2.9);
465 let steering = controller.compute_steering(&state);
466
467 assert!(
468 steering < 0.0,
469 "Steering should be negative to correct errors, got: {}",
470 steering
471 );
472 }
473
474 #[test]
475 fn test_rear_wheel_feedback_planning() {
476 let mut controller = RearWheelFeedbackController::with_params(1.0, 0.5, 2.9);
477 let waypoints = vec![(0.0, 0.0), (50.0, 0.0), (100.0, 0.0)];
478
479 let result = controller.planning(waypoints, 5.0, 0.5);
480 assert!(result.is_some());
481 assert!(!result.unwrap().is_empty());
482 }
483
484 #[test]
485 fn test_rear_wheel_feedback_curved_path() {
486 let mut controller = RearWheelFeedbackController::with_params(1.0, 0.5, 2.9);
487 let waypoints = vec![(0.0, 0.0), (10.0, 0.0), (20.0, 5.0), (30.0, 10.0)];
488
489 let result = controller.planning(waypoints, 3.0, 0.5);
490 assert!(result.is_some());
491
492 let trajectory = result.unwrap();
493 assert!(!trajectory.is_empty());
494
495 let last = trajectory.last().unwrap();
497 let dx = last.0 - 30.0;
498 let dy = last.1 - 10.0;
499 let dist = (dx * dx + dy * dy).sqrt();
500 assert!(dist < 2.0, "Final distance to goal: {}", dist);
501 }
502
503 #[test]
504 fn test_vehicle_state_update() {
505 let mut state = VehicleState::new(0.0, 0.0, 0.0, 1.0, 2.9);
506 state.update(0.0, 0.0, 1.0);
507
508 assert!((state.x - 1.0).abs() < 0.01);
510 assert!(state.y.abs() < 0.01);
511 assert!(state.yaw.abs() < 0.01);
512 }
513
514 #[test]
515 fn test_path_tracker_trait() {
516 let mut controller = RearWheelFeedbackController::new(RearWheelFeedbackConfig::default());
517 let path = Path2D::from_points(vec![
518 Point2D::new(0.0, 0.0),
519 Point2D::new(10.0, 0.0),
520 Point2D::new(20.0, 0.0),
521 ]);
522
523 let state = State2D::new(5.0, 0.0, 0.0, 5.0);
524 let control = controller.compute_control(&state, &path);
525
526 assert!(control.v.is_finite());
528 assert!(control.omega.is_finite());
529 }
530}