Skip to main content

rust_robotics_planning/
prm.rs

1#![allow(dead_code, clippy::too_many_arguments)]
2
3//! Probabilistic Road-Map (PRM) path planning
4//!
5//! Sampling-based planner that builds a roadmap of collision-free
6//! configurations and searches for a path using Dijkstra's algorithm.
7
8use rand::Rng;
9use std::cmp::Ordering;
10use std::collections::{BinaryHeap, HashMap};
11
12// Parameters
13const N_SAMPLE: usize = 500;
14const N_KNN: usize = 10;
15const MAX_EDGE_LEN: f64 = 30.0;
16
17/// Node for Dijkstra search
18#[derive(Clone)]
19struct Node {
20    x: f64,
21    y: f64,
22    cost: f64,
23    parent: Option<usize>,
24}
25
26impl Node {
27    fn new(x: f64, y: f64) -> Self {
28        Node {
29            x,
30            y,
31            cost: f64::INFINITY,
32            parent: None,
33        }
34    }
35}
36
37/// Priority queue item for Dijkstra
38#[derive(Clone)]
39struct QueueItem {
40    cost: f64,
41    index: usize,
42}
43
44impl PartialEq for QueueItem {
45    fn eq(&self, other: &Self) -> bool {
46        self.cost == other.cost
47    }
48}
49
50impl Eq for QueueItem {}
51
52impl Ord for QueueItem {
53    fn cmp(&self, other: &Self) -> Ordering {
54        other
55            .cost
56            .partial_cmp(&self.cost)
57            .unwrap_or(Ordering::Equal)
58    }
59}
60
61impl PartialOrd for QueueItem {
62    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
63        Some(self.cmp(other))
64    }
65}
66
67/// Simple KDTree for nearest neighbor search
68struct KDTree {
69    points: Vec<(f64, f64)>,
70}
71
72impl KDTree {
73    fn new(points: Vec<(f64, f64)>) -> Self {
74        KDTree { points }
75    }
76
77    fn query_knn(&self, x: f64, y: f64, k: usize) -> Vec<(usize, f64)> {
78        let mut distances: Vec<(usize, f64)> = self
79            .points
80            .iter()
81            .enumerate()
82            .map(|(i, (px, py))| {
83                let d = ((x - px).powi(2) + (y - py).powi(2)).sqrt();
84                (i, d)
85            })
86            .collect();
87
88        distances.sort_by(|a, b| a.1.partial_cmp(&b.1).unwrap());
89        distances.truncate(k + 1);
90        distances
91    }
92
93    fn min_distance(&self, x: f64, y: f64) -> f64 {
94        self.points
95            .iter()
96            .map(|(px, py)| ((x - px).powi(2) + (y - py).powi(2)).sqrt())
97            .fold(f64::INFINITY, f64::min)
98    }
99}
100
101/// PRM Planner
102pub struct PRMPlanner {
103    sample_x: Vec<f64>,
104    sample_y: Vec<f64>,
105    road_map: Vec<Vec<usize>>,
106}
107
108impl PRMPlanner {
109    /// Create a new PRM planner
110    pub fn new(
111        ox: &[f64],
112        oy: &[f64],
113        start: (f64, f64),
114        goal: (f64, f64),
115        robot_radius: f64,
116    ) -> Self {
117        let obstacle_tree = KDTree::new(ox.iter().zip(oy.iter()).map(|(&x, &y)| (x, y)).collect());
118
119        // Sample over the obstacles' bounding box, grown to include the start
120        // and goal and to at least 1 m on each side (a line of obstacles, or
121        // none, would otherwise give an empty range).
122        let xs = ox.iter().copied().chain([start.0, goal.0]);
123        let ys = oy.iter().copied().chain([start.1, goal.1]);
124        let (mut min_x, mut max_x) = xs.fold((f64::INFINITY, f64::NEG_INFINITY), |(lo, hi), v| {
125            (lo.min(v), hi.max(v))
126        });
127        let (mut min_y, mut max_y) = ys.fold((f64::INFINITY, f64::NEG_INFINITY), |(lo, hi), v| {
128            (lo.min(v), hi.max(v))
129        });
130        if max_x - min_x < 1.0 {
131            min_x -= 0.5;
132            max_x += 0.5;
133        }
134        if max_y - min_y < 1.0 {
135            min_y -= 0.5;
136            max_y += 0.5;
137        }
138
139        let (sample_x, sample_y) = Self::sample_points(
140            start,
141            goal,
142            min_x,
143            max_x,
144            min_y,
145            max_y,
146            robot_radius,
147            &obstacle_tree,
148        );
149
150        let road_map = Self::generate_road_map(&sample_x, &sample_y, robot_radius, &obstacle_tree);
151
152        PRMPlanner {
153            sample_x,
154            sample_y,
155            road_map,
156        }
157    }
158
159    fn sample_points(
160        start: (f64, f64),
161        goal: (f64, f64),
162        min_x: f64,
163        max_x: f64,
164        min_y: f64,
165        max_y: f64,
166        robot_radius: f64,
167        obstacle_tree: &KDTree,
168    ) -> (Vec<f64>, Vec<f64>) {
169        let mut rng = rand::rng();
170        let mut sample_x = Vec::new();
171        let mut sample_y = Vec::new();
172
173        // Bounded attempts: if hardly anything is free (huge robot radius),
174        // plan with the samples found instead of looping forever.
175        let mut attempts = 0;
176        while sample_x.len() < N_SAMPLE && attempts < N_SAMPLE * 200 {
177            attempts += 1;
178            let x = rng.random_range(min_x..max_x);
179            let y = rng.random_range(min_y..max_y);
180
181            let min_dist = obstacle_tree.min_distance(x, y);
182            if min_dist > robot_radius {
183                sample_x.push(x);
184                sample_y.push(y);
185            }
186        }
187
188        sample_x.push(start.0);
189        sample_y.push(start.1);
190        sample_x.push(goal.0);
191        sample_y.push(goal.1);
192
193        (sample_x, sample_y)
194    }
195
196    fn generate_road_map(
197        sample_x: &[f64],
198        sample_y: &[f64],
199        robot_radius: f64,
200        obstacle_tree: &KDTree,
201    ) -> Vec<Vec<usize>> {
202        let sample_tree = KDTree::new(
203            sample_x
204                .iter()
205                .zip(sample_y.iter())
206                .map(|(&x, &y)| (x, y))
207                .collect(),
208        );
209
210        let mut road_map: Vec<Vec<usize>> = vec![Vec::new(); sample_x.len()];
211
212        for (i, (&x, &y)) in sample_x.iter().zip(sample_y.iter()).enumerate() {
213            let neighbors = sample_tree.query_knn(x, y, N_KNN);
214
215            for (j, dist) in neighbors {
216                if i == j {
217                    continue;
218                }
219
220                if dist > MAX_EDGE_LEN {
221                    continue;
222                }
223
224                if !Self::is_collision(x, y, sample_x[j], sample_y[j], robot_radius, obstacle_tree)
225                {
226                    road_map[i].push(j);
227                }
228            }
229        }
230
231        road_map
232    }
233
234    fn is_collision(
235        x1: f64,
236        y1: f64,
237        x2: f64,
238        y2: f64,
239        robot_radius: f64,
240        obstacle_tree: &KDTree,
241    ) -> bool {
242        let dx = x2 - x1;
243        let dy = y2 - y1;
244        let d = (dx * dx + dy * dy).sqrt();
245
246        if d == 0.0 {
247            return false;
248        }
249
250        // Check at least every 5 cm: a zero (or tiny) radius would otherwise
251        // give an unbounded number of steps.
252        let step = robot_radius.max(0.05);
253        let n_steps = (d / step).ceil() as usize;
254
255        for i in 0..=n_steps {
256            let t = i as f64 / n_steps as f64;
257            let x = x1 + t * dx;
258            let y = y1 + t * dy;
259
260            let min_dist = obstacle_tree.min_distance(x, y);
261            if min_dist <= robot_radius {
262                return true;
263            }
264        }
265
266        false
267    }
268
269    /// Plan path using Dijkstra's algorithm
270    pub fn plan(&self) -> Option<(Vec<f64>, Vec<f64>)> {
271        let n = self.sample_x.len();
272        let start_idx = n - 2;
273        let goal_idx = n - 1;
274
275        let mut nodes: Vec<Node> = self
276            .sample_x
277            .iter()
278            .zip(self.sample_y.iter())
279            .map(|(&x, &y)| Node::new(x, y))
280            .collect();
281
282        nodes[start_idx].cost = 0.0;
283
284        let mut open_set = BinaryHeap::new();
285        open_set.push(QueueItem {
286            cost: 0.0,
287            index: start_idx,
288        });
289
290        let mut closed_set: HashMap<usize, bool> = HashMap::new();
291
292        while let Some(current) = open_set.pop() {
293            if current.index == goal_idx {
294                return Some(self.reconstruct_path(&nodes, goal_idx));
295            }
296
297            if closed_set.contains_key(&current.index) {
298                continue;
299            }
300            closed_set.insert(current.index, true);
301
302            for &neighbor_idx in &self.road_map[current.index] {
303                if closed_set.contains_key(&neighbor_idx) {
304                    continue;
305                }
306
307                let dx = nodes[neighbor_idx].x - nodes[current.index].x;
308                let dy = nodes[neighbor_idx].y - nodes[current.index].y;
309                let edge_cost = (dx * dx + dy * dy).sqrt();
310                let new_cost = nodes[current.index].cost + edge_cost;
311
312                if new_cost < nodes[neighbor_idx].cost {
313                    nodes[neighbor_idx].cost = new_cost;
314                    nodes[neighbor_idx].parent = Some(current.index);
315                    open_set.push(QueueItem {
316                        cost: new_cost,
317                        index: neighbor_idx,
318                    });
319                }
320            }
321        }
322
323        None
324    }
325
326    fn reconstruct_path(&self, nodes: &[Node], goal_idx: usize) -> (Vec<f64>, Vec<f64>) {
327        let mut path_x = Vec::new();
328        let mut path_y = Vec::new();
329
330        let mut current = goal_idx;
331        while let Some(parent) = nodes[current].parent {
332            path_x.push(nodes[current].x);
333            path_y.push(nodes[current].y);
334            current = parent;
335        }
336        path_x.push(nodes[current].x);
337        path_y.push(nodes[current].y);
338
339        path_x.reverse();
340        path_y.reverse();
341
342        (path_x, path_y)
343    }
344
345    /// Get sample points for external inspection
346    pub fn get_samples(&self) -> (&[f64], &[f64]) {
347        (&self.sample_x, &self.sample_y)
348    }
349
350    /// Get road map edges for external inspection
351    pub fn get_edges(&self) -> Vec<((f64, f64), (f64, f64))> {
352        let mut edges = Vec::new();
353
354        // The roadmap is directed (each sample lists its nearest neighbors);
355        // report every link once, including one-way ones.
356        for (i, neighbors) in self.road_map.iter().enumerate() {
357            for &j in neighbors {
358                if i < j || !self.road_map[j].contains(&i) {
359                    edges.push((
360                        (self.sample_x[i], self.sample_y[i]),
361                        (self.sample_x[j], self.sample_y[j]),
362                    ));
363                }
364            }
365        }
366
367        edges
368    }
369}
370
371#[cfg(test)]
372mod tests {
373    use super::*;
374
375    fn walls() -> (Vec<f64>, Vec<f64>) {
376        let (mut ox, mut oy) = (Vec::new(), Vec::new());
377        for i in 0..=20 {
378            let t = i as f64;
379            for (x, y) in [(t, 0.0), (t, 20.0), (0.0, t), (20.0, t)] {
380                ox.push(x);
381                oy.push(y);
382            }
383        }
384        (ox, oy)
385    }
386
387    #[test]
388    fn a_zero_radius_robot_still_builds_a_roadmap() {
389        let (ox, oy) = walls();
390        let prm = PRMPlanner::new(&ox, &oy, (2.0, 2.0), (18.0, 18.0), 0.0);
391        assert!(prm.plan().is_some());
392    }
393
394    #[test]
395    fn degenerate_obstacle_sets_do_not_panic_or_hang() {
396        // All obstacles on one line, and none at all.
397        let ox: Vec<f64> = (0..10).map(f64::from).collect();
398        let oy = vec![0.0; 10];
399        let prm = PRMPlanner::new(&ox, &oy, (1.0, 3.0), (8.0, 3.0), 0.5);
400        assert!(!prm.get_samples().0.is_empty());
401        let prm = PRMPlanner::new(&[], &[], (1.0, 1.0), (5.0, 4.0), 0.5);
402        assert!(!prm.get_samples().0.is_empty());
403        // A radius no sample can satisfy gives up instead of looping.
404        let (ox, oy) = walls();
405        let prm = PRMPlanner::new(&ox, &oy, (10.0, 10.0), (11.0, 10.0), 50.0);
406        assert_eq!(prm.get_samples().0.len(), 2, "only start and goal");
407    }
408
409    #[test]
410    fn every_edge_of_the_planned_path_is_in_the_roadmap() {
411        let (ox, oy) = walls();
412        let prm = PRMPlanner::new(&ox, &oy, (2.0, 2.0), (18.0, 18.0), 1.0);
413        let (px, py) = prm.plan().expect("path");
414        let edges = prm.get_edges();
415        for i in 0..px.len() - 1 {
416            let (a, b) = ((px[i], py[i]), (px[i + 1], py[i + 1]));
417            assert!(
418                edges
419                    .iter()
420                    .any(|&(u, v)| (u == a && v == b) || (u == b && v == a)),
421                "path edge {a:?} -> {b:?} is not drawn"
422            );
423        }
424    }
425
426    #[test]
427    fn test_prm_creation() {
428        let mut ox = Vec::new();
429        let mut oy = Vec::new();
430        for i in 0..20 {
431            ox.push(i as f64);
432            oy.push(0.0);
433            ox.push(i as f64);
434            oy.push(20.0);
435            ox.push(0.0);
436            oy.push(i as f64);
437            ox.push(20.0);
438            oy.push(i as f64);
439        }
440
441        let prm = PRMPlanner::new(&ox, &oy, (2.0, 2.0), (18.0, 18.0), 2.0);
442        let (sx, sy) = prm.get_samples();
443        assert!(!sx.is_empty());
444        assert_eq!(sx.len(), sy.len());
445    }
446}