Skip to main content

rust_robotics_planning/
d_star_lite.rs

1#![allow(dead_code, clippy::legacy_numeric_constants)]
2
3//! D* Lite path planning algorithm
4//!
5//! Incremental heuristic search algorithm that efficiently replans
6//! when the graph changes (e.g., new obstacles detected).
7
8use std::cmp::Ordering;
9use std::collections::{BinaryHeap, HashSet};
10
11#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, PartialOrd, Ord)]
12struct Node {
13    x: i32,
14    y: i32,
15}
16
17impl Node {
18    pub fn new(x: i32, y: i32) -> Self {
19        Node { x, y }
20    }
21
22    fn add(&self, other: &Node) -> Node {
23        Node {
24            x: self.x + other.x,
25            y: self.y + other.y,
26        }
27    }
28
29    fn distance_to(&self, other: &Node) -> f64 {
30        let dx = (self.x - other.x) as f64;
31        let dy = (self.y - other.y) as f64;
32        (dx * dx + dy * dy).sqrt()
33    }
34}
35
36#[derive(Debug, Clone, Copy, PartialEq)]
37struct Key {
38    k1: f64,
39    k2: f64,
40}
41
42impl Eq for Key {}
43
44impl PartialOrd for Key {
45    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
46        Some(self.cmp(other))
47    }
48}
49
50impl Ord for Key {
51    fn cmp(&self, other: &Self) -> Ordering {
52        match self.k1.partial_cmp(&other.k1) {
53            Some(Ordering::Equal) => self.k2.partial_cmp(&other.k2).unwrap_or(Ordering::Equal),
54            Some(ord) => ord,
55            None => Ordering::Equal,
56        }
57    }
58}
59
60#[derive(Debug)]
61pub struct DStarLite {
62    x_min_world: i32,
63    y_min_world: i32,
64    x_max: i32,
65    y_max: i32,
66    start: Node,
67    goal: Node,
68    km: f64,
69    rhs: nalgebra::DMatrix<f64>,
70    g: nalgebra::DMatrix<f64>,
71    u: BinaryHeap<(Key, Node)>,
72    obstacles: HashSet<(i32, i32)>,
73    detected_obstacles: HashSet<(i32, i32)>,
74    motions: [Node; 8],
75    detected_obstacles_for_plotting: Vec<(f64, f64)>,
76    initialized: bool,
77}
78
79impl DStarLite {
80    pub fn new(ox: &[i32], oy: &[i32]) -> Self {
81        let x_min_world = *ox.iter().min().unwrap_or(&0);
82        let y_min_world = *oy.iter().min().unwrap_or(&0);
83        let x_max = ox.iter().max().unwrap_or(&0) - x_min_world + 1;
84        let y_max = oy.iter().max().unwrap_or(&0) - y_min_world + 1;
85
86        let obstacles: HashSet<_> = ox
87            .iter()
88            .zip(oy.iter())
89            .map(|(&x, &y)| (x - x_min_world, y - y_min_world))
90            .collect();
91
92        let g = nalgebra::DMatrix::from_element(x_max as usize, y_max as usize, f64::INFINITY);
93        let rhs = nalgebra::DMatrix::from_element(x_max as usize, y_max as usize, f64::INFINITY);
94
95        let motions = [
96            Node::new(1, 0),
97            Node::new(0, 1),
98            Node::new(-1, 0),
99            Node::new(0, -1),
100            Node::new(1, 1),
101            Node::new(1, -1),
102            Node::new(-1, 1),
103            Node::new(-1, -1),
104        ];
105
106        DStarLite {
107            x_min_world,
108            y_min_world,
109            x_max,
110            y_max,
111            start: Node::new(0, 0),
112            goal: Node::new(0, 0),
113            km: 0.0,
114            rhs,
115            g,
116            u: BinaryHeap::new(),
117            obstacles,
118            detected_obstacles: HashSet::new(),
119            motions,
120            detected_obstacles_for_plotting: Vec::new(),
121            initialized: false,
122        }
123    }
124
125    fn is_valid(&self, node: &Node) -> bool {
126        node.x >= 0 && node.x < self.x_max && node.y >= 0 && node.y < self.y_max
127    }
128
129    fn is_obstacle(&self, node: &Node) -> bool {
130        self.obstacles.contains(&(node.x, node.y))
131            || self.detected_obstacles.contains(&(node.x, node.y))
132    }
133
134    fn get_neighbors(&self, node: &Node) -> Vec<Node> {
135        self.motions
136            .iter()
137            .map(|m| node.add(m))
138            .filter(|n| self.is_valid(n))
139            .collect()
140    }
141
142    fn calculate_key(&self, node: &Node) -> Key {
143        let min_rhs_g = self.g[(node.x as usize, node.y as usize)]
144            .min(self.rhs[(node.x as usize, node.y as usize)]);
145        let h = self.heuristic(&self.start, node);
146        Key {
147            k1: min_rhs_g + h + self.km,
148            k2: min_rhs_g,
149        }
150    }
151
152    fn heuristic(&self, a: &Node, b: &Node) -> f64 {
153        let dx = (a.x - b.x).abs() as f64;
154        let dy = (a.y - b.y).abs() as f64;
155        (dx * dx + dy * dy).sqrt()
156    }
157
158    fn c(&self, _u: &Node, v: &Node) -> f64 {
159        if self.is_obstacle(v) {
160            return f64::INFINITY;
161        }
162        self.heuristic(_u, v)
163    }
164
165    fn initialize(&mut self, start: Node, goal: Node) {
166        self.start = Node::new(start.x - self.x_min_world, start.y - self.y_min_world);
167        self.goal = Node::new(goal.x - self.x_min_world, goal.y - self.y_min_world);
168
169        if !self.initialized {
170            self.initialized = true;
171            self.u.clear();
172            self.km = 0.0;
173            self.rhs = nalgebra::DMatrix::from_element(
174                self.x_max as usize,
175                self.y_max as usize,
176                f64::INFINITY,
177            );
178            self.g = nalgebra::DMatrix::from_element(
179                self.x_max as usize,
180                self.y_max as usize,
181                f64::INFINITY,
182            );
183            self.rhs[(self.goal.x as usize, self.goal.y as usize)] = 0.0;
184            self.u.push((self.calculate_key(&self.goal), self.goal));
185            self.detected_obstacles.clear();
186        }
187    }
188
189    fn update_vertex(&mut self, u: Node) {
190        if u.x != self.goal.x || u.y != self.goal.y {
191            let mut min_rhs = f64::INFINITY;
192            for s in self.get_neighbors(&u) {
193                let cost = self.c(&u, &s) + self.g[(s.x as usize, s.y as usize)];
194                if cost < min_rhs {
195                    min_rhs = cost;
196                }
197            }
198            self.rhs[(u.x as usize, u.y as usize)] = min_rhs;
199        }
200
201        self.u = std::mem::take(&mut self.u)
202            .into_iter()
203            .filter(|(_, node)| node.x != u.x || node.y != u.y)
204            .collect();
205
206        if (self.g[(u.x as usize, u.y as usize)] - self.rhs[(u.x as usize, u.y as usize)]).abs()
207            > 1e-6
208        {
209            self.u.push((self.calculate_key(&u), u));
210        }
211    }
212
213    fn compute_shortest_path(&mut self) {
214        while !self.u.is_empty() {
215            let (k_old, u) = self.u.pop().unwrap();
216
217            if k_old < self.calculate_key(&u) {
218                self.u.push((self.calculate_key(&u), u));
219            } else if self.g[(u.x as usize, u.y as usize)] > self.rhs[(u.x as usize, u.y as usize)]
220            {
221                self.g[(u.x as usize, u.y as usize)] = self.rhs[(u.x as usize, u.y as usize)];
222
223                for s in self.get_neighbors(&u) {
224                    self.update_vertex(s);
225                }
226            } else {
227                self.g[(u.x as usize, u.y as usize)] = f64::INFINITY;
228                self.update_vertex(u);
229                for s in self.get_neighbors(&u) {
230                    self.update_vertex(s);
231                }
232            }
233        }
234    }
235
236    fn plan(&mut self, start: Node, goal: Node) -> Option<Vec<(i32, i32)>> {
237        self.initialize(Node::new(start.x, start.y), Node::new(goal.x, goal.y));
238        self.compute_shortest_path();
239        self.reconstruct_path()
240    }
241
242    /// Plan a path from integer coordinates
243    pub fn plan_xy(&mut self, sx: i32, sy: i32, gx: i32, gy: i32) -> Option<Vec<(i32, i32)>> {
244        self.plan(Node::new(sx, sy), Node::new(gx, gy))
245    }
246
247    fn reconstruct_path(&self) -> Option<Vec<(i32, i32)>> {
248        let mut path = Vec::new();
249        let mut current = self.start;
250
251        while current.x != self.goal.x || current.y != self.goal.y {
252            path.push((current.x + self.x_min_world, current.y + self.y_min_world));
253
254            let mut min_cost = f64::INFINITY;
255            let mut next_node = current;
256
257            for neighbor in self.get_neighbors(&current) {
258                let cost = self.c(&current, &neighbor)
259                    + self.g[(neighbor.x as usize, neighbor.y as usize)];
260                if cost < min_cost {
261                    min_cost = cost;
262                    next_node = neighbor;
263                }
264            }
265
266            if next_node.x == current.x && next_node.y == current.y {
267                return None;
268            }
269
270            current = next_node;
271        }
272
273        path.push((
274            self.goal.x + self.x_min_world,
275            self.goal.y + self.y_min_world,
276        ));
277
278        Some(path)
279    }
280
281    pub fn add_obstacle(&mut self, x: i32, y: i32) {
282        let node = Node::new(x - self.x_min_world, y - self.y_min_world);
283        if self.is_valid(&node) {
284            self.detected_obstacles.insert((node.x, node.y));
285            self.detected_obstacles_for_plotting
286                .push((x as f64, y as f64));
287            self.update_vertex(node);
288        }
289    }
290}
291
292#[cfg(test)]
293mod tests {
294    use super::*;
295
296    #[test]
297    fn test_d_star_lite_creation() {
298        let ox: Vec<i32> = (0..10).collect();
299        let oy: Vec<i32> = vec![0; 10];
300        let dstar = DStarLite::new(&ox, &oy);
301        assert!(dstar.x_max > 0);
302        assert!(dstar.y_max > 0);
303    }
304}