1use crate::spline_course::calc_spline_course;
11use alloc::vec::Vec;
12use core::f64::consts::PI;
13#[cfg(not(feature = "std"))]
14#[allow(unused_imports)]
15use num_traits::Float;
17use rust_robotics_core::normalize_angle;
18use rust_robotics_core::{ControlInput, Path2D, PathTracker, Point2D, State2D};
19
20#[derive(Debug, Clone, Copy)]
22pub struct VehicleState {
23 pub x: f64,
24 pub y: f64,
25 pub yaw: f64,
26 pub v: f64,
27 pub wheelbase: f64,
28}
29
30impl VehicleState {
31 pub fn new(x: f64, y: f64, yaw: f64, v: f64, wheelbase: f64) -> Self {
32 VehicleState {
33 x,
34 y,
35 yaw,
36 v,
37 wheelbase,
38 }
39 }
40
41 pub fn update(&mut self, a: f64, delta: f64, dt: f64) {
42 self.x += self.v * self.yaw.cos() * dt;
43 self.y += self.v * self.yaw.sin() * dt;
44 self.yaw += self.v / self.wheelbase * delta.tan() * dt;
45 self.v += a * dt;
46 }
47
48 pub fn front_axle(&self) -> (f64, f64) {
50 let fx = self.x + self.wheelbase * self.yaw.cos();
51 let fy = self.y + self.wheelbase * self.yaw.sin();
52 (fx, fy)
53 }
54
55 pub fn to_state2d(&self) -> State2D {
56 State2D::new(self.x, self.y, self.yaw, self.v)
57 }
58}
59
60impl From<State2D> for VehicleState {
61 fn from(s: State2D) -> Self {
62 VehicleState::new(s.x, s.y, s.yaw, s.v, 2.9) }
64}
65
66const MAX_STEER: f64 = 0.5 * PI - 0.1;
68
69#[derive(Debug, Clone)]
71pub struct StanleyConfig {
72 pub k: f64,
74 pub wheelbase: f64,
76 pub kp: f64,
78 pub goal_threshold: f64,
80}
81
82impl Default for StanleyConfig {
83 fn default() -> Self {
84 Self {
85 k: 0.5,
86 wheelbase: 2.9,
87 kp: 1.0,
88 goal_threshold: 3.0,
89 }
90 }
91}
92
93pub struct StanleyController {
95 config: StanleyConfig,
96 path: Path2D,
97 path_yaw: Vec<f64>,
98 last_target_idx: usize,
99}
100
101impl StanleyController {
102 pub fn new(config: StanleyConfig) -> Self {
104 StanleyController {
105 config,
106 path: Path2D::new(),
107 path_yaw: Vec::new(),
108 last_target_idx: 0,
109 }
110 }
111
112 pub fn with_params(k: f64, wheelbase: f64) -> Self {
114 let config = StanleyConfig {
115 k,
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 = path;
126 self.last_target_idx = 0;
127 }
128
129 pub fn set_path_with_yaw(&mut self, path: Path2D, yaw: Vec<f64>) {
131 self.path = path;
132 self.path_yaw = yaw;
133 self.last_target_idx = 0;
134 }
135
136 pub fn get_path(&self) -> &Path2D {
138 &self.path
139 }
140
141 fn compute_path_yaw(&self, path: &Path2D) -> Vec<f64> {
143 path.yaw_profile()
144 }
145
146 fn calc_target_index(&self, state: &VehicleState) -> (usize, f64) {
148 let (fx, fy) = state.front_axle();
149 let query = Point2D::new(fx, fy);
150 let min_idx = self
151 .path
152 .nearest_point_index_forward(query, self.last_target_idx)
153 .unwrap_or(0);
154
155 let target_point = &self.path.points[min_idx];
157 let diff_x = fx - target_point.x;
158 let diff_y = fy - target_point.y;
159 let error_front_axle =
160 -(state.yaw + 0.5 * PI).cos() * diff_x - (state.yaw + 0.5 * PI).sin() * diff_y;
161
162 (min_idx, error_front_axle)
163 }
164
165 pub fn compute_steering(&mut self, state: &VehicleState) -> f64 {
167 if self.path.is_empty() {
168 return 0.0;
169 }
170
171 let (mut target_idx, error_front_axle) = self.calc_target_index(state);
172
173 if self.last_target_idx >= target_idx {
174 target_idx = self.last_target_idx;
175 }
176 self.last_target_idx = target_idx;
177
178 let theta_e = normalize_angle(self.path_yaw[target_idx] - state.yaw);
180
181 let theta_d = (self.config.k * error_front_axle).atan2(state.v.max(0.1));
183
184 (theta_e + theta_d).clamp(-MAX_STEER, MAX_STEER)
187 }
188
189 pub fn compute_acceleration(&self, target_speed: f64, current_speed: f64) -> f64 {
191 self.config.kp * (target_speed - current_speed)
192 }
193
194 pub fn is_goal_reached_vehicle(&self, state: &VehicleState) -> bool {
196 if let Some(goal) = self.path.points.last() {
197 let dx = state.x - goal.x;
198 let dy = state.y - goal.y;
199 (dx * dx + dy * dy).sqrt() < self.config.goal_threshold
200 } else {
201 true
202 }
203 }
204
205 pub fn planning(
207 &mut self,
208 waypoints: Vec<(f64, f64)>,
209 target_speed: f64,
210 ds: f64,
211 ) -> Option<Vec<(f64, f64)>> {
212 if waypoints.len() < 2 {
213 return None;
214 }
215
216 let ax: Vec<f64> = waypoints.iter().map(|p| p.0).collect();
218 let ay: Vec<f64> = waypoints.iter().map(|p| p.1).collect();
219 let (cx, cy, cyaw, _, _) = calc_spline_course(&ax, &ay, ds);
220
221 let path = Path2D::from_points(
223 cx.iter()
224 .zip(cy.iter())
225 .map(|(&x, &y)| Point2D::new(x, y))
226 .collect(),
227 );
228 self.set_path_with_yaw(path, cyaw);
229
230 let init_yaw = 20.0_f64.to_radians();
232 let mut state = VehicleState::new(
233 waypoints[0].0,
234 waypoints[0].1 + 5.0,
235 init_yaw,
236 0.0,
237 self.config.wheelbase,
238 );
239
240 let mut trajectory = vec![(state.x, state.y)];
241 let dt = 0.1;
242 let t_max = 100.0;
243 let mut time = 0.0;
244
245 while time < t_max {
246 let ai = self.compute_acceleration(target_speed, state.v);
247 let di = self.compute_steering(&state);
248 state.update(ai, di, dt);
249 time += dt;
250
251 trajectory.push((state.x, state.y));
252
253 if self.is_goal_reached_vehicle(&state) {
254 break;
255 }
256 }
257
258 Some(trajectory)
259 }
260}
261
262impl PathTracker for StanleyController {
263 fn compute_control(&mut self, current_state: &State2D, path: &Path2D) -> ControlInput {
264 if self.path != *path {
266 self.set_path(path.clone());
267 }
268
269 let vehicle_state = VehicleState::new(
270 current_state.x,
271 current_state.y,
272 current_state.yaw,
273 current_state.v,
274 self.config.wheelbase,
275 );
276
277 let delta = self.compute_steering(&vehicle_state);
278
279 let target_speed = 5.0; let v = current_state.v + self.compute_acceleration(target_speed, current_state.v) * 0.1;
282
283 let omega = v * delta.tan() / self.config.wheelbase;
285
286 ControlInput::new(v, omega)
287 }
288
289 fn is_goal_reached(&self, current_state: &State2D, goal: Point2D) -> bool {
290 let dx = current_state.x - goal.x;
291 let dy = current_state.y - goal.y;
292 (dx * dx + dy * dy).sqrt() < self.config.goal_threshold
293 }
294}
295
296#[cfg(test)]
297mod tests {
298 use super::*;
299
300 #[test]
301 fn a_large_heading_error_still_steers_back_toward_the_path() {
302 let path = Path2D::from_points((0..=40).map(|i| Point2D::new(i as f64, 0.0)).collect());
305 for (yaw, expected_sign) in [(1.6, -1.0), (1.8, -1.0), (-1.6, 1.0), (-1.8, 1.0)] {
306 let mut controller = StanleyController::new(StanleyConfig::default());
307 let command = controller.compute_control(&State2D::new(5.0, 0.0, yaw, 2.0), &path);
308 assert!(
309 command.omega * expected_sign > 0.0,
310 "yaw {yaw}: omega {}",
311 command.omega
312 );
313 }
314 }
315
316 #[test]
317 fn test_stanley_creation() {
318 let config = StanleyConfig::default();
319 let controller = StanleyController::new(config);
320 assert!(controller.path.is_empty());
321 }
322
323 #[test]
324 fn test_stanley_set_path() {
325 let mut controller = StanleyController::with_params(0.5, 2.9);
326 let path = Path2D::from_points(vec![
327 Point2D::new(0.0, 0.0),
328 Point2D::new(1.0, 0.0),
329 Point2D::new(2.0, 0.0),
330 ]);
331 controller.set_path(path);
332 assert_eq!(controller.get_path().len(), 3);
333 assert_eq!(controller.path_yaw.len(), 3);
334 }
335
336 #[test]
337 fn test_stanley_normalize_angle() {
338 assert!((normalize_angle(3.0 * PI) - PI).abs() < 0.01);
339 assert!((normalize_angle(-3.0 * PI) + PI).abs() < 0.01);
340 }
341
342 #[test]
343 fn test_stanley_planning() {
344 let mut controller = StanleyController::with_params(0.5, 2.9);
345 let waypoints = vec![(0.0, 0.0), (50.0, 0.0), (100.0, 0.0)];
346
347 let result = controller.planning(waypoints, 5.0, 0.5);
348 assert!(result.is_some());
349 assert!(!result.unwrap().is_empty());
350 }
351}