Skip to main content

rust_robotics_control/
pure_pursuit.rs

1//! Pure Pursuit path tracking algorithm
2//!
3//! A geometric path tracking controller that computes the steering angle
4//! to follow a reference path by looking ahead to a target point.
5
6use alloc::vec::Vec;
7#[cfg(not(feature = "std"))]
8#[allow(unused_imports)]
9// f64 math via libm on no_std targets; on std hosts the inherent methods win
10use num_traits::Float;
11use rust_robotics_core::{ControlInput, Path2D, PathTracker, Point2D, State2D};
12
13/// Vehicle state for Pure Pursuit
14#[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) // default wheelbase
63    }
64}
65
66/// Configuration for Pure Pursuit controller
67#[derive(Debug, Clone)]
68pub struct PurePursuitConfig {
69    /// Look-ahead gain (k)
70    pub look_ahead_gain: f64,
71    /// Look-ahead distance offset (Lfc)
72    pub look_ahead_distance: f64,
73    /// Vehicle wheelbase
74    pub wheelbase: f64,
75    /// Speed proportional gain
76    pub kp: f64,
77    /// Goal distance threshold
78    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
93/// Pure Pursuit path tracking controller
94pub struct PurePursuitController {
95    config: PurePursuitConfig,
96    path: Path2D,
97    old_nearest_index: Option<usize>,
98}
99
100impl PurePursuitController {
101    /// Create a new Pure Pursuit controller
102    pub fn new(config: PurePursuitConfig) -> Self {
103        PurePursuitController {
104            config,
105            path: Path2D::new(),
106            old_nearest_index: None,
107        }
108    }
109
110    /// Create with simplified parameters (legacy interface)
111    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    /// Set the reference path
122    pub fn set_path(&mut self, path: Path2D) {
123        self.path = path;
124        self.old_nearest_index = None;
125    }
126
127    /// Get the current reference path
128    pub fn get_path(&self) -> &Path2D {
129        &self.path
130    }
131
132    /// Compute steering angle for current state
133    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        // Near the end the target (the last point) can be closer than the
150        // look-ahead distance: steer for the real distance, or the arc is
151        // too wide and the vehicle circles the goal.
152        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    /// Search for target point index on path
157    fn search_target_index(&mut self, state: &VehicleState) -> (usize, f64) {
158        let query = Point2D::new(state.rear_x, state.rear_y);
159        // Follow the path forward from where the vehicle was (from the
160        // start on the first call), so loops and crossings are not skipped.
161        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        // Calculate look-ahead distance
170        let lf = self.config.look_ahead_gain * state.v + self.config.look_ahead_distance;
171
172        // Find target point at look-ahead distance
173        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    /// Proportional speed control
188    pub fn compute_acceleration(&self, target_speed: f64, current_speed: f64) -> f64 {
189        self.config.kp * (target_speed - current_speed)
190    }
191
192    /// Check if goal is reached
193    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    /// Simulate path tracking (legacy interface)
204    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        // Set path
214        let path =
215            Path2D::from_points(waypoints.iter().map(|&(x, y)| Point2D::new(x, y)).collect());
216        self.set_path(path);
217
218        // Initial state
219        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        // Adopt a new reference path (comparing contents, not just length)
253        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        // Compute speed control (assume constant target speed for now)
268        let target_speed = 5.0; // m/s
269        let v = current_state.v + self.compute_acceleration(target_speed, current_state.v) * 0.1;
270
271        // Convert steering angle to angular velocity
272        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}