Skip to main content

rust_robotics_slam/
frontier_exploration.rs

1//! Frontier-based exploration (Yamauchi, 1997) on an [`OccupancyGrid`].
2//!
3//! A *frontier cell* is a free cell next to an unknown one. Frontier cells are
4//! grouped into 8-connected clusters; a breadth-first wavefront from the robot
5//! through free cells with enough clearance from obstacles finds which
6//! clusters are reachable and how far away they are. The explorer drives to
7//! the closest reachable cell of the best cluster, trading travel distance
8//! against cluster size, and is done when no reachable frontier remains.
9
10use std::collections::VecDeque;
11
12use nalgebra::Vector2;
13
14use crate::lidar_occupancy::{CellState, OccupancyGrid};
15
16/// Tuning parameters for [`find_frontiers`] and [`next_frontier_goal`].
17#[derive(Debug, Clone, Copy, PartialEq)]
18pub struct FrontierConfig {
19    /// Clusters with fewer cells are ignored (raster noise, wall cracks).
20    pub min_cluster_cells: usize,
21    /// Only cells at least this far from an occupied cell are traversable
22    /// \[m\]; use the robot radius plus a margin.
23    pub clearance: f64,
24    /// Score = path length − `size_weight` × √(cluster cells) \[m\]: larger
25    /// frontiers promise more new map per meter driven.
26    pub size_weight: f64,
27}
28
29impl Default for FrontierConfig {
30    fn default() -> Self {
31        Self {
32            min_cluster_cells: 8,
33            clearance: 0.5,
34            size_weight: 0.5,
35        }
36    }
37}
38
39/// One connected group of frontier cells.
40#[derive(Debug, Clone, PartialEq)]
41pub struct Frontier {
42    /// Grid cells `(x, y)` of the cluster.
43    pub cells: Vec<(usize, usize)>,
44    /// World centroid of the cluster.
45    pub centroid: Vector2<f64>,
46}
47
48impl Frontier {
49    /// Number of cells in the cluster.
50    pub fn len(&self) -> usize {
51        self.cells.len()
52    }
53
54    /// Whether the cluster has no cells.
55    pub fn is_empty(&self) -> bool {
56        self.cells.is_empty()
57    }
58}
59
60/// A chosen exploration target.
61#[derive(Debug, Clone, PartialEq)]
62pub struct FrontierGoal {
63    /// The reachable frontier cell to drive to (world).
64    pub target: Vector2<f64>,
65    /// Wavefront path length from the robot to `target` \[m\].
66    pub distance: f64,
67    /// The cluster `target` belongs to.
68    pub frontier: Frontier,
69}
70
71const NEIGHBORS_4: [(i64, i64); 4] = [(1, 0), (-1, 0), (0, 1), (0, -1)];
72const NEIGHBORS_8: [(i64, i64); 8] = [
73    (1, 0),
74    (-1, 0),
75    (0, 1),
76    (0, -1),
77    (1, 1),
78    (1, -1),
79    (-1, 1),
80    (-1, -1),
81];
82
83fn offset(
84    grid: &OccupancyGrid,
85    (x, y): (usize, usize),
86    (dx, dy): (i64, i64),
87) -> Option<(usize, usize)> {
88    let (width, height) = grid.size();
89    let (nx, ny) = (x as i64 + dx, y as i64 + dy);
90    (nx >= 0 && ny >= 0 && (nx as usize) < width && (ny as usize) < height)
91        .then_some((nx as usize, ny as usize))
92}
93
94fn is_frontier_cell(grid: &OccupancyGrid, cell: (usize, usize)) -> bool {
95    grid.state(cell.0, cell.1) == CellState::Free
96        && NEIGHBORS_4.iter().any(|&delta| {
97            offset(grid, cell, delta).is_some_and(|(x, y)| grid.state(x, y) == CellState::Unknown)
98        })
99}
100
101/// Every frontier cluster with at least `config.min_cluster_cells` cells.
102pub fn find_frontiers(grid: &OccupancyGrid, config: &FrontierConfig) -> Vec<Frontier> {
103    let (width, height) = grid.size();
104    let mut visited = vec![false; width * height];
105    let mut frontiers = Vec::new();
106    for y in 0..height {
107        for x in 0..width {
108            if visited[y * width + x] || !is_frontier_cell(grid, (x, y)) {
109                continue;
110            }
111            visited[y * width + x] = true;
112            let mut cells = Vec::new();
113            let mut queue = VecDeque::from([(x, y)]);
114            while let Some(cell) = queue.pop_front() {
115                cells.push(cell);
116                for delta in NEIGHBORS_8 {
117                    if let Some(next) = offset(grid, cell, delta) {
118                        let index = next.1 * width + next.0;
119                        if !visited[index] && is_frontier_cell(grid, next) {
120                            visited[index] = true;
121                            queue.push_back(next);
122                        }
123                    }
124                }
125            }
126            if cells.len() >= config.min_cluster_cells {
127                let sum = cells.iter().fold(Vector2::zeros(), |sum, &(x, y)| {
128                    sum + grid.cell_center(x, y)
129                });
130                frontiers.push(Frontier {
131                    centroid: sum / cells.len() as f64,
132                    cells,
133                });
134            }
135        }
136    }
137    frontiers
138}
139
140/// Whether any frontier cell lies within `radius` of `point` — a cheap check
141/// that a chosen goal is still worth driving to as the map grows.
142pub fn is_frontier_near(grid: &OccupancyGrid, point: Vector2<f64>, radius: f64) -> bool {
143    let Some((cx, cy)) = grid.cell_of(point) else {
144        return false;
145    };
146    let reach = (radius / grid.resolution()).ceil() as i64;
147    (-reach..=reach)
148        .flat_map(|dx| (-reach..=reach).map(move |dy| (dx, dy)))
149        .filter_map(|delta| offset(grid, (cx, cy), delta))
150        .any(|cell| is_frontier_cell(grid, cell))
151}
152
153/// Wavefront distances \[m\] from `start` through free cells with clearance
154/// (`f64::INFINITY` where unreachable). The start cell is always seeded so a
155/// robot hugging a wall can still leave it.
156fn wavefront(grid: &OccupancyGrid, start: Vector2<f64>, clearance: f64) -> Vec<f64> {
157    let (width, height) = grid.size();
158    let mut distance = vec![f64::INFINITY; width * height];
159    let Some(start) = grid.cell_of(start) else {
160        return distance;
161    };
162    let field = grid.distance_field(clearance + grid.resolution());
163    let open = |(x, y): (usize, usize)| {
164        grid.state(x, y) == CellState::Free && f64::from(field[y * width + x]) >= clearance
165    };
166    // Dijkstra over the 8-connected grid; the weights are only 1 and √2, so
167    // a binary heap over ordered integers (millimeters) is exact enough.
168    let mut heap = std::collections::BinaryHeap::new();
169    distance[start.1 * width + start.0] = 0.0;
170    heap.push(std::cmp::Reverse((0_u64, start)));
171    while let Some(std::cmp::Reverse((millimeters, cell))) = heap.pop() {
172        let here = distance[cell.1 * width + cell.0];
173        if (millimeters as f64) > here * 1000.0 + 0.5 {
174            continue;
175        }
176        for (dx, dy) in NEIGHBORS_8 {
177            let Some(next) = offset(grid, cell, (dx, dy)) else {
178                continue;
179            };
180            if !open(next) {
181                continue;
182            }
183            let step = if dx != 0 && dy != 0 {
184                std::f64::consts::SQRT_2
185            } else {
186                1.0
187            } * grid.resolution();
188            let index = next.1 * width + next.0;
189            if here + step < distance[index] {
190                distance[index] = here + step;
191                heap.push(std::cmp::Reverse((
192                    ((here + step) * 1000.0).round() as u64,
193                    next,
194                )));
195            }
196        }
197    }
198    distance
199}
200
201/// Frontier cells within this distance of an excluded point are skipped \[m\].
202const EXCLUSION_RADIUS: f64 = 1.5;
203
204/// The best reachable frontier from `robot`, skipping frontier cells within
205/// 1.5 m of a point in `excluded` (e.g. goals the robot already failed to
206/// reach). `None` when exploration is complete.
207pub fn next_frontier_goal(
208    grid: &OccupancyGrid,
209    robot: Vector2<f64>,
210    excluded: &[Vector2<f64>],
211    config: &FrontierConfig,
212) -> Option<FrontierGoal> {
213    let (width, _) = grid.size();
214    let distance = wavefront(grid, robot, config.clearance);
215    find_frontiers(grid, config)
216        .into_iter()
217        .filter_map(|frontier| {
218            let &(x, y) = frontier
219                .cells
220                .iter()
221                .filter(|&&(x, y)| {
222                    let center = grid.cell_center(x, y);
223                    excluded
224                        .iter()
225                        .all(|point| (point - center).norm() > EXCLUSION_RADIUS)
226                })
227                .min_by(|a, b| {
228                    distance[a.1 * width + a.0].total_cmp(&distance[b.1 * width + b.0])
229                })?;
230            let reach = distance[y * width + x];
231            reach.is_finite().then(|| FrontierGoal {
232                target: grid.cell_center(x, y),
233                distance: reach,
234                frontier,
235            })
236        })
237        .min_by(|a, b| {
238            let score = |goal: &FrontierGoal| {
239                goal.distance - config.size_weight * (goal.frontier.len() as f64).sqrt()
240            };
241            score(a).total_cmp(&score(b))
242        })
243}
244
245#[cfg(test)]
246mod tests {
247    use super::*;
248    use crate::lidar_occupancy::OccupancyConfig;
249    use crate::scan_to_map::{ray_cast_ranges, LineSegment};
250    use rust_robotics_core::Pose2D;
251
252    /// Two 6 × 4 m rooms joined by a 1.6 m door at x = 0.
253    fn walls() -> Vec<LineSegment> {
254        let v = Vector2::new;
255        vec![
256            LineSegment::new(v(-6.0, -2.0), v(6.0, -2.0)),
257            LineSegment::new(v(6.0, -2.0), v(6.0, 2.0)),
258            LineSegment::new(v(6.0, 2.0), v(-6.0, 2.0)),
259            LineSegment::new(v(-6.0, 2.0), v(-6.0, -2.0)),
260            LineSegment::new(v(0.0, -2.0), v(0.0, -0.8)),
261            LineSegment::new(v(0.0, 0.8), v(0.0, 2.0)),
262        ]
263    }
264
265    fn grid_after(poses: &[Pose2D]) -> OccupancyGrid {
266        let mut grid = OccupancyGrid::new(
267            Vector2::new(-7.0, -3.0),
268            Vector2::new(7.0, 3.0),
269            OccupancyConfig::default(),
270        );
271        for pose in poses {
272            // A short-range LiDAR, so one room is not seen through the door.
273            grid.insert_ranges(*pose, &ray_cast_ranges(*pose, &walls(), 360, 4.0), 4.0);
274        }
275        grid
276    }
277
278    #[test]
279    fn the_unseen_room_is_a_reachable_frontier() {
280        // Every corner of the left room is within the 4 m range from here.
281        let robot = Pose2D::new(-3.0, 0.0, 0.0);
282        let grid = grid_after(&[robot]);
283        let frontiers = find_frontiers(&grid, &FrontierConfig::default());
284        assert!(!frontiers.is_empty());
285        let goal = next_frontier_goal(
286            &grid,
287            Vector2::new(robot.x, robot.y),
288            &[],
289            &FrontierConfig::default(),
290        )
291        .expect("goal");
292        // The 4 m LiDAR ends at x = 0 (the door), so the frontier is there.
293        assert!(goal.target.x > -0.5, "goal {:?}", goal.target);
294        assert!(goal.distance.is_finite() && goal.distance > 2.0);
295    }
296
297    #[test]
298    fn a_seen_frontier_stops_being_one() {
299        let robot = Pose2D::new(-3.0, 0.0, 0.0);
300        let grid = grid_after(&[robot]);
301        let goal = next_frontier_goal(
302            &grid,
303            Vector2::new(robot.x, robot.y),
304            &[],
305            &FrontierConfig::default(),
306        )
307        .expect("goal");
308        assert!(is_frontier_near(&grid, goal.target, 0.3));
309        // Look through the door from the doorway.
310        let grid = grid_after(&[robot, Pose2D::new(goal.target.x, goal.target.y, 0.0)]);
311        assert!(!is_frontier_near(&grid, goal.target, 0.3));
312    }
313
314    #[test]
315    fn excluded_goals_are_skipped() {
316        let robot = Vector2::new(-4.0, 0.0);
317        let grid = grid_after(&[Pose2D::new(robot.x, robot.y, 0.0)]);
318        let config = FrontierConfig::default();
319        let first = next_frontier_goal(&grid, robot, &[], &config).expect("goal");
320        let second = next_frontier_goal(&grid, robot, &[first.target], &config);
321        assert!(second.map_or(true, |goal| (goal.target - first.target).norm() > 1.5));
322    }
323
324    #[test]
325    fn exploration_is_complete_when_everything_was_seen() {
326        let poses: Vec<Pose2D> = [-5.0, -3.0, -1.0, 1.0, 3.0, 5.0]
327            .iter()
328            .flat_map(|&x| [Pose2D::new(x, -1.0, 0.0), Pose2D::new(x, 1.0, 0.0)])
329            .collect();
330        let grid = grid_after(&poses);
331        let goal = next_frontier_goal(
332            &grid,
333            Vector2::new(-4.0, 0.0),
334            &[],
335            &FrontierConfig::default(),
336        );
337        assert!(goal.is_none(), "unexpected frontier {goal:?}");
338    }
339}