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

rust_robotics_planning/
d_star_lite.rs
