1use std::collections::VecDeque;
11
12use nalgebra::Vector2;
13
14use crate::lidar_occupancy::{CellState, OccupancyGrid};
15
16#[derive(Debug, Clone, Copy, PartialEq)]
18pub struct FrontierConfig {
19 pub min_cluster_cells: usize,
21 pub clearance: f64,
24 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#[derive(Debug, Clone, PartialEq)]
41pub struct Frontier {
42 pub cells: Vec<(usize, usize)>,
44 pub centroid: Vector2<f64>,
46}
47
48impl Frontier {
49 pub fn len(&self) -> usize {
51 self.cells.len()
52 }
53
54 pub fn is_empty(&self) -> bool {
56 self.cells.is_empty()
57 }
58}
59
60#[derive(Debug, Clone, PartialEq)]
62pub struct FrontierGoal {
63 pub target: Vector2<f64>,
65 pub distance: f64,
67 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
101pub 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
140pub 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
153fn 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 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
201const EXCLUSION_RADIUS: f64 = 1.5;
203
204pub 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 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 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 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 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 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}