1use alloc::vec::Vec;
7#[cfg(not(feature = "std"))]
8#[allow(unused_imports)]
9use num_traits::Float;
11use rust_robotics_core::{ControlInput, Path2D, PathTracker, Point2D, State2D};
12
13#[derive(Debug, Clone, Copy)]
15pub struct VehicleState {
16 pub x: f64,
17 pub y: f64,
18 pub yaw: f64,
19 pub v: f64,
20 pub rear_x: f64,
21 pub rear_y: 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 let rear_x = x - (wheelbase / 2.0) * yaw.cos();
28 let rear_y = y - (wheelbase / 2.0) * yaw.sin();
29 VehicleState {
30 x,
31 y,
32 yaw,
33 v,
34 rear_x,
35 rear_y,
36 wheelbase,
37 }
38 }
39
40 pub fn update(&mut self, a: f64, delta: f64, dt: f64) {
41 self.x += self.v * self.yaw.cos() * dt;
42 self.y += self.v * self.yaw.sin() * dt;
43 self.yaw += self.v / self.wheelbase * delta.tan() * dt;
44 self.v += a * dt;
45 self.rear_x = self.x - (self.wheelbase / 2.0) * self.yaw.cos();
46 self.rear_y = self.y - (self.wheelbase / 2.0) * self.yaw.sin();
47 }
48
49 pub fn calc_distance(&self, px: f64, py: f64) -> f64 {
50 let dx = self.rear_x - px;
51 let dy = self.rear_y - py;
52 (dx * dx + dy * dy).sqrt()
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
66#[derive(Debug, Clone)]
68pub struct PurePursuitConfig {
69 pub look_ahead_gain: f64,
71 pub look_ahead_distance: f64,
73 pub wheelbase: f64,
75 pub kp: f64,
77 pub goal_threshold: f64,
79}
80
81impl Default for PurePursuitConfig {
82 fn default() -> Self {
83 Self {
84 look_ahead_gain: 0.1,
85 look_ahead_distance: 2.0,
86 wheelbase: 2.9,
87 kp: 1.0,
88 goal_threshold: 2.0,
89 }
90 }
91}
92
93pub struct PurePursuitController {
95 config: PurePursuitConfig,
96 path: Path2D,
97 old_nearest_index: Option<usize>,
98}
99
100impl PurePursuitController {
101 pub fn new(config: PurePursuitConfig) -> Self {
103 PurePursuitController {
104 config,
105 path: Path2D::new(),
106 old_nearest_index: None,
107 }
108 }
109
110 pub fn with_params(k: f64, lfc: f64, wb: f64) -> Self {
112 let config = PurePursuitConfig {
113 look_ahead_gain: k,
114 look_ahead_distance: lfc,
115 wheelbase: wb,
116 ..Default::default()
117 };
118 Self::new(config)
119 }
120
121 pub fn set_path(&mut self, path: Path2D) {
123 self.path = path;
124 self.old_nearest_index = None;
125 }
126
127 pub fn get_path(&self) -> &Path2D {
129 &self.path
130 }
131
132 pub fn compute_steering(&mut self, state: &VehicleState) -> f64 {
134 if self.path.is_empty() {
135 return 0.0;
136 }
137
138 let (target_idx, lf) = self.search_target_index(state);
139
140 let (tx, ty) = if target_idx < self.path.len() {
141 let p = &self.path.points[target_idx];
142 (p.x, p.y)
143 } else {
144 let p = self.path.points.last().unwrap();
145 (p.x, p.y)
146 };
147
148 let alpha = (ty - state.rear_y).atan2(tx - state.rear_x) - state.yaw;
149 let ld = lf.min(state.calc_distance(tx, ty)).max(1e-3);
153 (2.0 * state.wheelbase * alpha.sin() / ld).atan2(1.0)
154 }
155
156 fn search_target_index(&mut self, state: &VehicleState) -> (usize, f64) {
158 let query = Point2D::new(state.rear_x, state.rear_y);
159 let from = self.old_nearest_index.unwrap_or(0);
162 let mut ind = self
163 .path
164 .nearest_point_index_forward(query, from)
165 .unwrap_or(from);
166
167 self.old_nearest_index = Some(ind);
168
169 let lf = self.config.look_ahead_gain * state.v + self.config.look_ahead_distance;
171
172 while ind < self.path.len() {
174 let p = &self.path.points[ind];
175 if state.calc_distance(p.x, p.y) >= lf {
176 break;
177 }
178 if ind + 1 >= self.path.len() {
179 break;
180 }
181 ind += 1;
182 }
183
184 (ind, lf)
185 }
186
187 pub fn compute_acceleration(&self, target_speed: f64, current_speed: f64) -> f64 {
189 self.config.kp * (target_speed - current_speed)
190 }
191
192 pub fn is_goal_reached(&self, state: &VehicleState) -> bool {
194 if let Some(goal) = self.path.points.last() {
195 let dx = state.x - goal.x;
196 let dy = state.y - goal.y;
197 (dx * dx + dy * dy).sqrt() < self.config.goal_threshold
198 } else {
199 true
200 }
201 }
202
203 pub fn planning(
205 &mut self,
206 waypoints: Vec<(f64, f64)>,
207 target_speed: f64,
208 ) -> Option<Vec<(f64, f64)>> {
209 if waypoints.len() < 2 {
210 return None;
211 }
212
213 let path =
215 Path2D::from_points(waypoints.iter().map(|&(x, y)| Point2D::new(x, y)).collect());
216 self.set_path(path);
217
218 let init_pos = &self.path.points[0];
220 let mut state = VehicleState::new(
221 init_pos.x,
222 init_pos.y - 3.0,
223 0.0,
224 0.0,
225 self.config.wheelbase,
226 );
227
228 let mut trajectory = vec![(state.x, state.y)];
229 let dt = 0.1;
230 let t_max = 100.0;
231 let mut time = 0.0;
232
233 while time < t_max {
234 let ai = self.compute_acceleration(target_speed, state.v);
235 let di = self.compute_steering(&state);
236 state.update(ai, di, dt);
237 time += dt;
238
239 trajectory.push((state.x, state.y));
240
241 if self.is_goal_reached(&state) {
242 break;
243 }
244 }
245
246 Some(trajectory)
247 }
248}
249
250impl PathTracker for PurePursuitController {
251 fn compute_control(&mut self, current_state: &State2D, path: &Path2D) -> ControlInput {
252 if self.path != *path {
254 self.set_path(path.clone());
255 }
256
257 let vehicle_state = VehicleState::new(
258 current_state.x,
259 current_state.y,
260 current_state.yaw,
261 current_state.v,
262 self.config.wheelbase,
263 );
264
265 let delta = self.compute_steering(&vehicle_state);
266
267 let target_speed = 5.0; let v = current_state.v + self.compute_acceleration(target_speed, current_state.v) * 0.1;
270
271 let omega = v * delta.tan() / self.config.wheelbase;
273
274 ControlInput::new(v, omega)
275 }
276
277 fn is_goal_reached(&self, current_state: &State2D, goal: Point2D) -> bool {
278 let dx = current_state.x - goal.x;
279 let dy = current_state.y - goal.y;
280 (dx * dx + dy * dy).sqrt() < self.config.goal_threshold
281 }
282}
283
284#[cfg(test)]
285mod tests {
286 use super::*;
287
288 #[test]
289 fn test_pure_pursuit_creation() {
290 let config = PurePursuitConfig::default();
291 let controller = PurePursuitController::new(config);
292 assert!(controller.path.is_empty());
293 }
294
295 #[test]
296 fn test_pure_pursuit_set_path() {
297 let mut controller = PurePursuitController::with_params(0.1, 2.0, 2.9);
298 let path = Path2D::from_points(vec![
299 Point2D::new(0.0, 0.0),
300 Point2D::new(1.0, 0.0),
301 Point2D::new(2.0, 0.0),
302 ]);
303 controller.set_path(path);
304 assert_eq!(controller.get_path().len(), 3);
305 }
306
307 #[test]
308 fn test_pure_pursuit_planning() {
309 let mut controller = PurePursuitController::with_params(0.1, 2.0, 2.9);
310 let waypoints: Vec<(f64, f64)> = (0..10).map(|i| (i as f64 * 2.0, 0.0)).collect();
311
312 let result = controller.planning(waypoints, 5.0);
313 assert!(result.is_some());
314 assert!(!result.unwrap().is_empty());
315 }
316}