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 rust_robotics_core::{ControlInput, Path2D, PathTracker, Point2D, State2D};
7
8/// Vehicle state for Pure Pursuit
9#[derive(Debug, Clone, Copy)]
10pub struct VehicleState {
11    pub x: f64,
12    pub y: f64,
13    pub yaw: f64,
14    pub v: f64,
15    pub rear_x: f64,
16    pub rear_y: f64,
17    pub wheelbase: f64,
18}
19
20impl VehicleState {
21    pub fn new(x: f64, y: f64, yaw: f64, v: f64, wheelbase: f64) -> Self {
22        let rear_x = x - (wheelbase / 2.0) * yaw.cos();
23        let rear_y = y - (wheelbase / 2.0) * yaw.sin();
24        VehicleState {
25            x,
26            y,
27            yaw,
28            v,
29            rear_x,
30            rear_y,
31            wheelbase,
32        }
33    }
34
35    pub fn update(&mut self, a: f64, delta: f64, dt: f64) {
36        self.x += self.v * self.yaw.cos() * dt;
37        self.y += self.v * self.yaw.sin() * dt;
38        self.yaw += self.v / self.wheelbase * delta.tan() * dt;
39        self.v += a * dt;
40        self.rear_x = self.x - (self.wheelbase / 2.0) * self.yaw.cos();
41        self.rear_y = self.y - (self.wheelbase / 2.0) * self.yaw.sin();
42    }
43
44    pub fn calc_distance(&self, px: f64, py: f64) -> f64 {
45        let dx = self.rear_x - px;
46        let dy = self.rear_y - py;
47        (dx * dx + dy * dy).sqrt()
48    }
49
50    pub fn to_state2d(&self) -> State2D {
51        State2D::new(self.x, self.y, self.yaw, self.v)
52    }
53}
54
55impl From<State2D> for VehicleState {
56    fn from(s: State2D) -> Self {
57        VehicleState::new(s.x, s.y, s.yaw, s.v, 2.9) // default wheelbase
58    }
59}
60
61/// Configuration for Pure Pursuit controller
62#[derive(Debug, Clone)]
63pub struct PurePursuitConfig {
64    /// Look-ahead gain (k)
65    pub look_ahead_gain: f64,
66    /// Look-ahead distance offset (Lfc)
67    pub look_ahead_distance: f64,
68    /// Vehicle wheelbase
69    pub wheelbase: f64,
70    /// Speed proportional gain
71    pub kp: f64,
72    /// Goal distance threshold
73    pub goal_threshold: f64,
74}
75
76impl Default for PurePursuitConfig {
77    fn default() -> Self {
78        Self {
79            look_ahead_gain: 0.1,
80            look_ahead_distance: 2.0,
81            wheelbase: 2.9,
82            kp: 1.0,
83            goal_threshold: 2.0,
84        }
85    }
86}
87
88/// Pure Pursuit path tracking controller
89pub struct PurePursuitController {
90    config: PurePursuitConfig,
91    path: Path2D,
92    old_nearest_index: Option<usize>,
93}
94
95impl PurePursuitController {
96    /// Create a new Pure Pursuit controller
97    pub fn new(config: PurePursuitConfig) -> Self {
98        PurePursuitController {
99            config,
100            path: Path2D::new(),
101            old_nearest_index: None,
102        }
103    }
104
105    /// Create with simplified parameters (legacy interface)
106    pub fn with_params(k: f64, lfc: f64, wb: f64) -> Self {
107        let config = PurePursuitConfig {
108            look_ahead_gain: k,
109            look_ahead_distance: lfc,
110            wheelbase: wb,
111            ..Default::default()
112        };
113        Self::new(config)
114    }
115
116    /// Set the reference path
117    pub fn set_path(&mut self, path: Path2D) {
118        self.path = path;
119        self.old_nearest_index = None;
120    }
121
122    /// Get the current reference path
123    pub fn get_path(&self) -> &Path2D {
124        &self.path
125    }
126
127    /// Compute steering angle for current state
128    pub fn compute_steering(&mut self, state: &VehicleState) -> f64 {
129        if self.path.is_empty() {
130            return 0.0;
131        }
132
133        let (target_idx, lf) = self.search_target_index(state);
134
135        let (tx, ty) = if target_idx < self.path.len() {
136            let p = &self.path.points[target_idx];
137            (p.x, p.y)
138        } else {
139            let p = self.path.points.last().unwrap();
140            (p.x, p.y)
141        };
142
143        let alpha = (ty - state.rear_y).atan2(tx - state.rear_x) - state.yaw;
144        (2.0 * state.wheelbase * alpha.sin() / lf).atan2(1.0)
145    }
146
147    /// Search for target point index on path
148    fn search_target_index(&mut self, state: &VehicleState) -> (usize, f64) {
149        let query = Point2D::new(state.rear_x, state.rear_y);
150        let mut ind = match self.old_nearest_index {
151            None => self.path.nearest_point_index(query).unwrap_or(0),
152            Some(prev_idx) => self
153                .path
154                .nearest_point_index_from(query, prev_idx)
155                .unwrap_or(prev_idx),
156        };
157
158        self.old_nearest_index = Some(ind);
159
160        // Calculate look-ahead distance
161        let lf = self.config.look_ahead_gain * state.v + self.config.look_ahead_distance;
162
163        // Find target point at look-ahead distance
164        while ind < self.path.len() {
165            let p = &self.path.points[ind];
166            if state.calc_distance(p.x, p.y) >= lf {
167                break;
168            }
169            if ind + 1 >= self.path.len() {
170                break;
171            }
172            ind += 1;
173        }
174
175        (ind, lf)
176    }
177
178    /// Proportional speed control
179    pub fn compute_acceleration(&self, target_speed: f64, current_speed: f64) -> f64 {
180        self.config.kp * (target_speed - current_speed)
181    }
182
183    /// Check if goal is reached
184    pub fn is_goal_reached(&self, state: &VehicleState) -> bool {
185        if let Some(goal) = self.path.points.last() {
186            let dx = state.x - goal.x;
187            let dy = state.y - goal.y;
188            (dx * dx + dy * dy).sqrt() < self.config.goal_threshold
189        } else {
190            true
191        }
192    }
193
194    /// Simulate path tracking (legacy interface)
195    pub fn planning(
196        &mut self,
197        waypoints: Vec<(f64, f64)>,
198        target_speed: f64,
199    ) -> Option<Vec<(f64, f64)>> {
200        if waypoints.len() < 2 {
201            return None;
202        }
203
204        // Set path
205        let path =
206            Path2D::from_points(waypoints.iter().map(|&(x, y)| Point2D::new(x, y)).collect());
207        self.set_path(path);
208
209        // Initial state
210        let init_pos = &self.path.points[0];
211        let mut state = VehicleState::new(
212            init_pos.x,
213            init_pos.y - 3.0,
214            0.0,
215            0.0,
216            self.config.wheelbase,
217        );
218
219        let mut trajectory = vec![(state.x, state.y)];
220        let dt = 0.1;
221        let t_max = 100.0;
222        let mut time = 0.0;
223
224        while time < t_max {
225            let ai = self.compute_acceleration(target_speed, state.v);
226            let di = self.compute_steering(&state);
227            state.update(ai, di, dt);
228            time += dt;
229
230            trajectory.push((state.x, state.y));
231
232            if self.is_goal_reached(&state) {
233                break;
234            }
235        }
236
237        Some(trajectory)
238    }
239}
240
241impl PathTracker for PurePursuitController {
242    fn compute_control(&mut self, current_state: &State2D, path: &Path2D) -> ControlInput {
243        // Set path if different
244        if self.path.len() != path.len() {
245            self.set_path(path.clone());
246        }
247
248        let vehicle_state = VehicleState::new(
249            current_state.x,
250            current_state.y,
251            current_state.yaw,
252            current_state.v,
253            self.config.wheelbase,
254        );
255
256        let delta = self.compute_steering(&vehicle_state);
257
258        // Compute speed control (assume constant target speed for now)
259        let target_speed = 5.0; // m/s
260        let v = current_state.v + self.compute_acceleration(target_speed, current_state.v) * 0.1;
261
262        // Convert steering angle to angular velocity
263        let omega = v * delta.tan() / self.config.wheelbase;
264
265        ControlInput::new(v, omega)
266    }
267
268    fn is_goal_reached(&self, current_state: &State2D, goal: Point2D) -> bool {
269        let dx = current_state.x - goal.x;
270        let dy = current_state.y - goal.y;
271        (dx * dx + dy * dy).sqrt() < self.config.goal_threshold
272    }
273}
274
275#[cfg(test)]
276mod tests {
277    use super::*;
278
279    #[test]
280    fn test_pure_pursuit_creation() {
281        let config = PurePursuitConfig::default();
282        let controller = PurePursuitController::new(config);
283        assert!(controller.path.is_empty());
284    }
285
286    #[test]
287    fn test_pure_pursuit_set_path() {
288        let mut controller = PurePursuitController::with_params(0.1, 2.0, 2.9);
289        let path = Path2D::from_points(vec![
290            Point2D::new(0.0, 0.0),
291            Point2D::new(1.0, 0.0),
292            Point2D::new(2.0, 0.0),
293        ]);
294        controller.set_path(path);
295        assert_eq!(controller.get_path().len(), 3);
296    }
297
298    #[test]
299    fn test_pure_pursuit_planning() {
300        let mut controller = PurePursuitController::with_params(0.1, 2.0, 2.9);
301        let waypoints: Vec<(f64, f64)> = (0..10).map(|i| (i as f64 * 2.0, 0.0)).collect();
302
303        let result = controller.planning(waypoints, 5.0);
304        assert!(result.is_some());
305        assert!(!result.unwrap().is_empty());
306    }
307}
rust_robotics_control/pure_pursuit.rs

rust_robotics_control/
pure_pursuit.rs
