Skip to main content

rust_robotics_planning/
dijkstra.rs

1#![allow(dead_code)]
2
3//! Dijkstra path planning algorithm
4//!
5//! Grid-based shortest path planning using Dijkstra's algorithm.
6
7use itertools::iproduct;
8use ordered_float::NotNan;
9use std::cmp::Reverse;
10use std::collections::{BTreeMap, BinaryHeap};
11
12use crate::a_star::{AStarConfig, AStarPlanner};
13use crate::grid_nalgebra;
14use rust_robotics_core::{Obstacles, Path2D, PathPlanner, Point2D, RoboticsError, RoboticsResult};
15
16/// Configuration for [`DijkstraPlanner`].
17#[derive(Debug, Clone)]
18pub struct DijkstraConfig {
19    /// Grid resolution \[m\]
20    pub resolution: f64,
21    /// Robot radius for obstacle inflation \[m\]
22    pub robot_radius: f64,
23}
24
25impl Default for DijkstraConfig {
26    fn default() -> Self {
27        Self {
28            resolution: 1.0,
29            robot_radius: 0.5,
30        }
31    }
32}
33
34/// World-coordinate Dijkstra planner implementing [`PathPlanner`].
35///
36/// Dijkstra is uniform-cost search: A* with a zero heuristic. This planner
37/// shares the A* grid, obstacle inflation, and 8-connected motion model, so
38/// its paths are optimal on the same grid that [`AStarPlanner`] searches.
39/// The legacy matrix API ([`dijkstra_plan`]) is kept unchanged.
40pub struct DijkstraPlanner {
41    inner: AStarPlanner,
42}
43
44impl DijkstraPlanner {
45    fn a_star_config(config: &DijkstraConfig) -> AStarConfig {
46        AStarConfig {
47            resolution: config.resolution,
48            robot_radius: config.robot_radius,
49            heuristic_weight: 0.0,
50        }
51    }
52
53    /// Builds the planner from obstacle coordinates.
54    pub fn try_new(ox: &[f64], oy: &[f64], config: DijkstraConfig) -> RoboticsResult<Self> {
55        Ok(Self {
56            inner: AStarPlanner::try_new(ox, oy, Self::a_star_config(&config))?,
57        })
58    }
59
60    /// Builds the planner from obstacle points.
61    pub fn from_obstacle_points(
62        obstacles: &Obstacles,
63        config: DijkstraConfig,
64    ) -> RoboticsResult<Self> {
65        Ok(Self {
66            inner: AStarPlanner::from_obstacle_points(obstacles, Self::a_star_config(&config))?,
67        })
68    }
69
70    /// Plans a shortest path from `start` to `goal`.
71    pub fn plan(&self, start: Point2D, goal: Point2D) -> RoboticsResult<Path2D> {
72        PathPlanner::plan(&self.inner, start, goal)
73    }
74
75    /// Plans a path, observing expanded cells in world coordinates.
76    ///
77    /// Uses the same observer contract as [`AStarPlanner::plan_with_observer`],
78    /// including expansions made before an unsuccessful search returns.
79    pub fn plan_with_observer(
80        &self,
81        start: Point2D,
82        goal: Point2D,
83        on_expand: impl FnMut(Point2D),
84    ) -> RoboticsResult<Path2D> {
85        self.inner.plan_with_observer(start, goal, on_expand)
86    }
87}
88
89impl PathPlanner for DijkstraPlanner {
90    fn plan(&self, start: Point2D, goal: Point2D) -> Result<Path2D, RoboticsError> {
91        DijkstraPlanner::plan(self, start, goal)
92    }
93}
94
95/// Returns a closure that computes adjacent cell coordinates for a given cell
96/// while excluding out-of-bound cells and the cell itself.
97fn calculate_neighbors(nrows: usize, ncols: usize) -> impl Fn(usize, usize) -> Vec<(usize, usize)> {
98    move |i, j| {
99        let delta = [-1, 0, 1];
100        iproduct!(delta, delta)
101            .filter_map(|(di, dj)| {
102                let ni = i as i32 + di;
103                let nj = j as i32 + dj;
104                if ni >= 0
105                    && ni < nrows as i32
106                    && nj >= 0
107                    && nj < ncols as i32
108                    && (ni, nj) != (i as i32, j as i32)
109                {
110                    Some((ni as usize, nj as usize))
111                } else {
112                    None
113                }
114            })
115            .collect()
116    }
117}
118
119/// Check if a cell or its neighbors contain an obstacle
120pub fn has_collision(obstacle_map: &grid_nalgebra::Map, ni: usize, nj: usize) -> bool {
121    let (height, width) = obstacle_map.shape();
122    let get_neighbors = calculate_neighbors(height, width);
123
124    let neighbors = get_neighbors(ni, nj);
125
126    if neighbors.iter().all(|&(i, j)| obstacle_map[(i, j)] != 1) && obstacle_map[(ni, nj)] != 1 {
127        return false;
128    }
129
130    true
131}
132
133/// Run Dijkstra's algorithm on a grid obstacle map.
134///
135/// Returns the shortest path from `start` to `goal` as a list of (row, col) coordinates,
136/// or `None` if no path exists.
137pub fn dijkstra_plan(
138    obstacle_map: &grid_nalgebra::Map,
139    start: (usize, usize),
140    goal: (usize, usize),
141) -> Option<Vec<(usize, usize)>> {
142    let (height, width) = obstacle_map.shape();
143    let get_neighbors = calculate_neighbors(height, width);
144
145    let mut dist = BTreeMap::new();
146    let mut prev: BTreeMap<(usize, usize), Option<(usize, usize)>> = BTreeMap::new();
147    let mut pq = BinaryHeap::new();
148
149    pq.push((Reverse(NotNan::new(0.0).unwrap()), start));
150    dist.insert(start, 0.0);
151
152    while let Some((distance, current)) = pq.pop() {
153        if current == goal {
154            if current == start {
155                return Some(vec![start]);
156            }
157            // Reconstruct path
158            let mut path = vec![current];
159            while let Some(previous) = prev.get(path.last().unwrap()).unwrap_or(&None) {
160                path.push(*previous);
161                if *path.last().unwrap() == start {
162                    path.reverse();
163                    return Some(path);
164                }
165            }
166            return None;
167        }
168
169        let (ci, cj) = current;
170        let neighbors = get_neighbors(ci, cj);
171
172        for (ni, nj) in neighbors {
173            if !has_collision(obstacle_map, ni, nj) {
174                let delta_x = ci as f64 - ni as f64;
175                let delta_y = cj as f64 - nj as f64;
176                let edge_distance = (delta_x * delta_x + delta_y * delta_y).sqrt();
177                let alt = distance.0.into_inner() + edge_distance;
178
179                if *dist.get(&(ni, nj)).unwrap_or(&f64::MAX) > alt {
180                    dist.insert((ni, nj), alt);
181                    prev.insert((ni, nj), Some((ci, cj)));
182                    pq.push((Reverse(NotNan::new(alt).unwrap()), (ni, nj)));
183                }
184            }
185        }
186    }
187
188    None
189}
190
191#[cfg(test)]
192mod tests {
193    use super::*;
194
195    #[test]
196    fn start_on_the_goal_is_a_one_cell_path() {
197        let matrix = nalgebra::DMatrix::from_element(3, 3, 0);
198        let map = grid_nalgebra::Map::new(matrix, 1).unwrap();
199        assert_eq!(dijkstra_plan(&map, (1, 1), (1, 1)), Some(vec![(1, 1)]));
200    }
201
202    #[test]
203    fn test_dijkstra_simple() {
204        let matrix = nalgebra::DMatrix::from_row_slice(
205            5,
206            5,
207            &[
208                0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
209            ],
210        );
211        let map = grid_nalgebra::Map::new(matrix, 1).unwrap();
212        let result = dijkstra_plan(&map, (0, 0), (4, 4));
213        assert!(result.is_some());
214        let path = result.unwrap();
215        assert_eq!(*path.first().unwrap(), (0, 0));
216        assert_eq!(*path.last().unwrap(), (4, 4));
217    }
218
219    #[test]
220    fn test_has_collision() {
221        let matrix = nalgebra::DMatrix::from_row_slice(
222            5,
223            5,
224            &[
225                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
226            ],
227        );
228        let map = grid_nalgebra::Map::new(matrix, 1).unwrap();
229        // Cell (2,2) is an obstacle
230        assert!(has_collision(&map, 2, 2));
231        // Cell (0,0) is far from obstacle, no collision
232        assert!(!has_collision(&map, 0, 0));
233        // Cell (1,1) is adjacent to obstacle (2,2), so it has collision
234        assert!(has_collision(&map, 1, 1));
235    }
236}