Skip to main content

rust_robotics_planning/
frontier_navigator.rs

1//! Long Range Navigator-lite: affordance-scored frontier navigation.
2//!
3//! This reproduces the decision core of long-range navigation past the local
4//! map: a robot with a limited, occlusion-aware sensor cannot see a distant
5//! goal, so it repeatedly picks the *frontier* (a known-free cell bordering
6//! unknown space) that best advances toward the goal, then hands off to a local
7//! planner that drives there over the known-free map. As the robot moves it
8//! reveals more of the world, re-evaluates frontiers, and eventually sees a path
9//! to the goal.
10//!
11//! - Sensing is occlusion-aware: cells are revealed only along a clear line of
12//!   sight within the sensor range, so obstacles cast unknown shadows whose
13//!   edges become frontiers.
14//! - Each frontier is scored by an affordance combining goal progress, the
15//!   known-free travel cost to reach it, whether it is in direct line of sight,
16//!   and how much unknown space it borders (information gain).
17//! - The local-planner handoff is a Dijkstra distance field over the known-free
18//!   map: the robot follows its gradient toward the chosen frontier for a
19//!   bounded step budget before re-sensing.
20//!
21//! Everything is deterministic for a fixed world and configuration.
22
23use std::cmp::Ordering;
24use std::collections::{BinaryHeap, VecDeque};
25
26use rust_robotics_core::{RoboticsError, RoboticsResult};
27
28const SQRT2: f64 = std::f64::consts::SQRT_2;
29
30/// Known-map cell state.
31#[derive(Debug, Clone, Copy, PartialEq, Eq)]
32pub enum Knowledge {
33    Unknown,
34    Free,
35    Occupied,
36}
37
38/// A static occupancy world the robot explores.
39#[derive(Debug, Clone, PartialEq)]
40pub struct FrontierNavWorld {
41    pub width: i32,
42    pub height: i32,
43    /// Ground-truth occupancy indexed `[x][y]`.
44    pub occupied: Vec<Vec<bool>>,
45    pub start: (i32, i32),
46    pub goal: (i32, i32),
47}
48
49impl FrontierNavWorld {
50    /// Empty (all-free) world of the given size.
51    pub fn new(
52        width: i32,
53        height: i32,
54        start: (i32, i32),
55        goal: (i32, i32),
56    ) -> RoboticsResult<Self> {
57        if width <= 0 || height <= 0 {
58            return Err(RoboticsError::InvalidParameter(
59                "frontier-nav world dimensions must be positive".to_string(),
60            ));
61        }
62        Ok(Self {
63            width,
64            height,
65            occupied: vec![vec![false; height as usize]; width as usize],
66            start,
67            goal,
68        })
69    }
70
71    fn in_bounds(&self, x: i32, y: i32) -> bool {
72        x >= 0 && y >= 0 && x < self.width && y < self.height
73    }
74
75    fn is_occupied(&self, x: i32, y: i32) -> bool {
76        !self.in_bounds(x, y) || self.occupied[x as usize][y as usize]
77    }
78
79    /// Set a rectangular block of cells occupied (clamped to bounds).
80    pub fn fill_rect(&mut self, min_x: i32, max_x: i32, min_y: i32, max_y: i32) {
81        for x in min_x.max(0)..=max_x.min(self.width - 1) {
82            for y in min_y.max(0)..=max_y.min(self.height - 1) {
83                self.occupied[x as usize][y as usize] = true;
84            }
85        }
86    }
87}
88
89/// One frontier selection, retained for inspection and rendering.
90#[derive(Debug, Clone, Copy, PartialEq)]
91pub struct FrontierChoice {
92    pub robot: (i32, i32),
93    pub frontier: (i32, i32),
94    pub score: f64,
95    pub line_of_sight: bool,
96}
97
98/// Configuration for the frontier navigator.
99#[derive(Debug, Clone, PartialEq)]
100pub struct FrontierNavConfig {
101    /// Sensor range in cells.
102    pub sensor_range: f64,
103    /// Cells the robot moves toward a frontier before re-sensing.
104    pub step_budget: usize,
105    /// Maximum frontier-selection iterations.
106    pub max_iterations: usize,
107    /// Weight on goal progress from the frontier.
108    pub goal_weight: f64,
109    /// Weight on the known-free travel cost to the frontier.
110    pub cost_weight: f64,
111    /// Bonus for a frontier in direct line of sight.
112    pub line_of_sight_weight: f64,
113    /// Weight on bordered unknown space (information gain).
114    pub openness_weight: f64,
115}
116
117impl Default for FrontierNavConfig {
118    fn default() -> Self {
119        Self {
120            sensor_range: 5.0,
121            step_budget: 3,
122            max_iterations: 200,
123            goal_weight: 1.0,
124            cost_weight: 0.35,
125            line_of_sight_weight: 1.5,
126            openness_weight: 0.15,
127        }
128    }
129}
130
131/// Result of a frontier-navigation rollout.
132#[derive(Debug, Clone, PartialEq)]
133pub struct FrontierNavReport {
134    pub path: Vec<(i32, i32)>,
135    pub reached_goal: bool,
136    pub path_length: f64,
137    pub frontier_selections: usize,
138    pub iterations: usize,
139    pub known_free_cells: usize,
140    pub selected_frontiers: Vec<FrontierChoice>,
141    /// Final known map indexed `[x][y]`.
142    pub known: Vec<Vec<Knowledge>>,
143}
144
145/// One frontier cluster and the terms actually used to score it.
146#[derive(Debug, Clone, PartialEq)]
147pub struct FrontierCandidate {
148    pub representative: (i32, i32),
149    pub members: Vec<(i32, i32)>,
150    /// Sum of unknown four-neighbors, including repeated neighbors.
151    pub openness: usize,
152    pub normalized_openness: f64,
153    pub goal_gain: f64,
154    pub travel_cost: Option<f64>,
155    pub line_of_sight: bool,
156    /// None when the representative is unreachable over known-free cells.
157    pub score: Option<f64>,
158}
159
160/// A recorded decision, or the reason the actual rollout stopped.
161#[derive(Debug, Clone, Copy, PartialEq, Eq)]
162pub enum FrontierNavPhase {
163    Frontier,
164    Goal,
165    Arrived,
166    NoFrontiers,
167    NoReachableFrontier,
168    NoProgress,
169    IterationLimit,
170}
171
172/// Actual knowledge and alternatives at one planning boundary.
173#[derive(Debug, Clone, PartialEq)]
174pub struct FrontierNavStep {
175    pub robot: (i32, i32),
176    /// Index into the returned report's path; knowledge stays unchanged until
177    /// a later step records another sensing operation.
178    pub path_index: usize,
179    pub known: Vec<Vec<Knowledge>>,
180    pub candidates: Vec<FrontierCandidate>,
181    pub selected: Option<usize>,
182    pub phase: FrontierNavPhase,
183}
184
185struct Trace {
186    enabled: bool,
187    steps: Vec<FrontierNavStep>,
188}
189impl Trace {
190    fn capture(
191        &mut self,
192        phase: FrontierNavPhase,
193        at: ((i32, i32), usize),
194        known: &[Vec<Knowledge>],
195        candidates: &[FrontierCandidate],
196        selected: Option<usize>,
197    ) {
198        if self.enabled {
199            self.steps.push(FrontierNavStep {
200                robot: at.0,
201                path_index: at.1,
202                known: known.to_vec(),
203                candidates: candidates.to_vec(),
204                selected,
205                phase,
206            });
207        }
208    }
209}
210
211/// Drive the affordance-scored frontier navigator across the world.
212pub fn simulate_frontier_navigation(
213    world: &FrontierNavWorld,
214    config: &FrontierNavConfig,
215) -> RoboticsResult<FrontierNavReport> {
216    simulate_observed(world, config, false).map(|(report, _)| report)
217}
218
219/// Runs the same navigator and records its actual decisions, candidate scores,
220/// known maps, path boundaries, and final stopping reason.
221pub fn simulate_frontier_navigation_with_trace(
222    world: &FrontierNavWorld,
223    config: &FrontierNavConfig,
224) -> RoboticsResult<(FrontierNavReport, Vec<FrontierNavStep>)> {
225    simulate_observed(world, config, true)
226}
227
228fn simulate_observed(
229    world: &FrontierNavWorld,
230    config: &FrontierNavConfig,
231    record: bool,
232) -> RoboticsResult<(FrontierNavReport, Vec<FrontierNavStep>)> {
233    validate(world, config)?;
234    let mut trace = Trace {
235        enabled: record,
236        steps: Vec::new(),
237    };
238    let mut outcome = FrontierNavPhase::IterationLimit;
239
240    let (w, h) = (world.width, world.height);
241    let mut known = vec![vec![Knowledge::Unknown; h as usize]; w as usize];
242    let mut robot = world.start;
243    let mut path = vec![robot];
244    let mut path_length = 0.0;
245    let mut selected_frontiers: Vec<FrontierChoice> = Vec::new();
246    let mut reached_goal = false;
247    let mut iterations = 0;
248
249    sense(world, &mut known, robot, config.sensor_range);
250
251    for _ in 0..config.max_iterations {
252        iterations += 1;
253
254        // If the goal is known free and reachable, hand off to the local planner.
255        if known[world.goal.0 as usize][world.goal.1 as usize] == Knowledge::Free {
256            if let Some(dist) = dijkstra_from(&known, world.goal) {
257                if dist[robot.0 as usize][robot.1 as usize].is_finite() {
258                    trace.capture(
259                        FrontierNavPhase::Goal,
260                        (robot, path.len() - 1),
261                        &known,
262                        &[],
263                        None,
264                    );
265                    follow_gradient(
266                        &known,
267                        &dist,
268                        &mut robot,
269                        world.goal,
270                        usize::MAX,
271                        &mut path,
272                        &mut path_length,
273                    );
274                    if robot == world.goal {
275                        reached_goal = true;
276                        outcome = FrontierNavPhase::Arrived;
277                        break;
278                    }
279                }
280            }
281        }
282
283        let frontiers = compute_frontiers(&known);
284        if frontiers.is_empty() {
285            outcome = FrontierNavPhase::NoFrontiers;
286            break;
287        }
288
289        // Travel-cost field from the robot over known-free cells.
290        let reach = match dijkstra_from(&known, robot) {
291            Some(field) => field,
292            None => {
293                outcome = FrontierNavPhase::NoReachableFrontier;
294                break;
295            }
296        };
297
298        let goal_distance = euclid(robot, world.goal);
299        let max_openness = frontiers
300            .iter()
301            .map(|f| f.openness)
302            .max()
303            .unwrap_or(1)
304            .max(1) as f64;
305
306        let mut best: Option<(f64, usize)> = None;
307        let mut candidates = Vec::new();
308        for (index, frontier) in frontiers.iter().enumerate() {
309            let reach_cost = reach[frontier.rep.0 as usize][frontier.rep.1 as usize];
310            let los = line_of_sight(&known, robot, frontier.rep);
311            let goal_gain = goal_distance - euclid(frontier.rep, world.goal);
312            let openness = frontier.openness as f64 / max_openness;
313            let score = config.goal_weight * goal_gain - config.cost_weight * reach_cost
314                + config.line_of_sight_weight * if los { 1.0 } else { 0.0 }
315                + config.openness_weight * openness;
316            if record {
317                candidates.push(FrontierCandidate {
318                    representative: frontier.rep,
319                    members: frontier.members.clone(),
320                    openness: frontier.openness,
321                    normalized_openness: openness,
322                    goal_gain,
323                    travel_cost: reach_cost.is_finite().then_some(reach_cost),
324                    line_of_sight: los,
325                    score: reach_cost.is_finite().then_some(score),
326                });
327            }
328            if !reach_cost.is_finite() {
329                continue;
330            }
331            let better = match best {
332                None => true,
333                Some((best_score, best_index)) => {
334                    score > best_score
335                        || (score == best_score && frontier.rep < frontiers[best_index].rep)
336                }
337            };
338            if better {
339                best = Some((score, index));
340            }
341        }
342
343        let Some((score, index)) = best else {
344            outcome = FrontierNavPhase::NoReachableFrontier;
345            break;
346        };
347        let frontier = &frontiers[index];
348        trace.capture(
349            FrontierNavPhase::Frontier,
350            (robot, path.len() - 1),
351            &known,
352            &candidates,
353            Some(index),
354        );
355        let target = frontier.rep;
356        selected_frontiers.push(FrontierChoice {
357            robot,
358            frontier: target,
359            score,
360            line_of_sight: line_of_sight(&known, robot, target),
361        });
362
363        // Local-planner handoff: move toward the frontier over known-free cells.
364        let field = match dijkstra_from(&known, target) {
365            Some(field) => field,
366            None => {
367                outcome = FrontierNavPhase::NoReachableFrontier;
368                break;
369            }
370        };
371        let before = robot;
372        follow_gradient(
373            &known,
374            &field,
375            &mut robot,
376            target,
377            config.step_budget,
378            &mut path,
379            &mut path_length,
380        );
381        if robot == before {
382            outcome = FrontierNavPhase::NoProgress;
383            break; // wedged: no progress possible.
384        }
385        sense(world, &mut known, robot, config.sensor_range);
386    }
387
388    let known_free_cells = known
389        .iter()
390        .flatten()
391        .filter(|c| **c == Knowledge::Free)
392        .count();
393
394    trace.capture(outcome, (robot, path.len() - 1), &known, &[], None);
395    Ok((
396        FrontierNavReport {
397            path,
398            reached_goal,
399            path_length,
400            frontier_selections: selected_frontiers.len(),
401            iterations,
402            known_free_cells,
403            selected_frontiers,
404            known,
405        },
406        trace.steps,
407    ))
408}
409
410fn validate(world: &FrontierNavWorld, config: &FrontierNavConfig) -> RoboticsResult<()> {
411    if !world.in_bounds(world.start.0, world.start.1)
412        || world.is_occupied(world.start.0, world.start.1)
413    {
414        return Err(RoboticsError::InvalidParameter(
415            "frontier-nav start must be a free in-bounds cell".to_string(),
416        ));
417    }
418    if !world.in_bounds(world.goal.0, world.goal.1) || world.is_occupied(world.goal.0, world.goal.1)
419    {
420        return Err(RoboticsError::InvalidParameter(
421            "frontier-nav goal must be a free in-bounds cell".to_string(),
422        ));
423    }
424    if !config.sensor_range.is_finite() || config.sensor_range <= 0.0 {
425        return Err(RoboticsError::InvalidParameter(
426            "frontier-nav sensor range must be finite and positive".to_string(),
427        ));
428    }
429    if config.step_budget == 0 || config.max_iterations == 0 {
430        return Err(RoboticsError::InvalidParameter(
431            "frontier-nav step budget and iterations must be positive".to_string(),
432        ));
433    }
434    Ok(())
435}
436
437/// A cluster of contiguous frontier cells.
438#[derive(Debug, Clone, PartialEq)]
439struct FrontierCluster {
440    rep: (i32, i32),
441    openness: usize,
442    members: Vec<(i32, i32)>,
443}
444
445/// Reveal cells within `range` of `from` along a clear line of sight.
446fn sense(world: &FrontierNavWorld, known: &mut [Vec<Knowledge>], from: (i32, i32), range: f64) {
447    let r = range.ceil() as i32;
448    for dx in -r..=r {
449        for dy in -r..=r {
450            let x = from.0 + dx;
451            let y = from.1 + dy;
452            if !world.in_bounds(x, y) {
453                continue;
454            }
455            if ((dx * dx + dy * dy) as f64).sqrt() > range {
456                continue;
457            }
458            if visible(world, from, (x, y)) {
459                known[x as usize][y as usize] = if world.is_occupied(x, y) {
460                    Knowledge::Occupied
461                } else {
462                    Knowledge::Free
463                };
464            }
465        }
466    }
467}
468
469/// Whether `target` is visible from `from` (no occupied cell strictly between).
470fn visible(world: &FrontierNavWorld, from: (i32, i32), target: (i32, i32)) -> bool {
471    let steps = (((target.0 - from.0).abs()).max((target.1 - from.1).abs()) * 3).max(1);
472    for i in 1..=steps {
473        let t = i as f64 / steps as f64;
474        let x = (from.0 as f64 + t * (target.0 - from.0) as f64).round() as i32;
475        let y = (from.1 as f64 + t * (target.1 - from.1) as f64).round() as i32;
476        if (x, y) == target {
477            return true;
478        }
479        if world.is_occupied(x, y) {
480            return false; // an earlier occupied cell blocks the ray.
481        }
482    }
483    true
484}
485
486/// Line of sight over the *known* map (blocked by known-occupied cells).
487fn line_of_sight(known: &[Vec<Knowledge>], from: (i32, i32), target: (i32, i32)) -> bool {
488    let steps = (((target.0 - from.0).abs()).max((target.1 - from.1).abs()) * 3).max(1);
489    for i in 1..=steps {
490        let t = i as f64 / steps as f64;
491        let x = (from.0 as f64 + t * (target.0 - from.0) as f64).round() as i32;
492        let y = (from.1 as f64 + t * (target.1 - from.1) as f64).round() as i32;
493        if (x, y) == target {
494            return true;
495        }
496        if known[x as usize][y as usize] == Knowledge::Occupied {
497            return false;
498        }
499    }
500    true
501}
502
503/// Frontier cells: known-free cells bordering unknown space, clustered.
504fn compute_frontiers(known: &[Vec<Knowledge>]) -> Vec<FrontierCluster> {
505    let w = known.len() as i32;
506    let h = known[0].len() as i32;
507    let is_free = |x: i32, y: i32| known[x as usize][y as usize] == Knowledge::Free;
508    let unknown_neighbors = |x: i32, y: i32| {
509        let mut count = 0;
510        for (dx, dy) in [(1, 0), (-1, 0), (0, 1), (0, -1)] {
511            let nx = x + dx;
512            let ny = y + dy;
513            if nx >= 0
514                && ny >= 0
515                && nx < w
516                && ny < h
517                && known[nx as usize][ny as usize] == Knowledge::Unknown
518            {
519                count += 1;
520            }
521        }
522        count
523    };
524
525    let mut is_frontier = vec![vec![false; h as usize]; w as usize];
526    for x in 0..w {
527        for y in 0..h {
528            if is_free(x, y) && unknown_neighbors(x, y) > 0 {
529                is_frontier[x as usize][y as usize] = true;
530            }
531        }
532    }
533
534    // Cluster frontier cells with 8-connected BFS.
535    let mut visited = vec![vec![false; h as usize]; w as usize];
536    let mut clusters = Vec::new();
537    for x in 0..w {
538        for y in 0..h {
539            if !is_frontier[x as usize][y as usize] || visited[x as usize][y as usize] {
540                continue;
541            }
542            let mut queue = VecDeque::new();
543            queue.push_back((x, y));
544            visited[x as usize][y as usize] = true;
545            let mut members = Vec::new();
546            let mut openness = 0;
547            while let Some((cx, cy)) = queue.pop_front() {
548                members.push((cx, cy));
549                openness += unknown_neighbors(cx, cy);
550                for dx in -1..=1 {
551                    for dy in -1..=1 {
552                        if dx == 0 && dy == 0 {
553                            continue;
554                        }
555                        let nx = cx + dx;
556                        let ny = cy + dy;
557                        if nx >= 0
558                            && ny >= 0
559                            && nx < w
560                            && ny < h
561                            && is_frontier[nx as usize][ny as usize]
562                            && !visited[nx as usize][ny as usize]
563                        {
564                            visited[nx as usize][ny as usize] = true;
565                            queue.push_back((nx, ny));
566                        }
567                    }
568                }
569            }
570            // Representative: member closest to the cluster centroid.
571            let cx = members.iter().map(|m| m.0).sum::<i32>() as f64 / members.len() as f64;
572            let cy = members.iter().map(|m| m.1).sum::<i32>() as f64 / members.len() as f64;
573            let rep = *members
574                .iter()
575                .min_by(|a, b| {
576                    let da = (a.0 as f64 - cx).powi(2) + (a.1 as f64 - cy).powi(2);
577                    let db = (b.0 as f64 - cx).powi(2) + (b.1 as f64 - cy).powi(2);
578                    da.partial_cmp(&db)
579                        .unwrap_or(Ordering::Equal)
580                        .then_with(|| a.cmp(b))
581                })
582                .unwrap();
583            clusters.push(FrontierCluster {
584                rep,
585                openness,
586                members,
587            });
588        }
589    }
590    clusters
591}
592
593/// Dijkstra distance field from `source` over known-free cells (8-connected).
594fn dijkstra_from(known: &[Vec<Knowledge>], source: (i32, i32)) -> Option<Vec<Vec<f64>>> {
595    let w = known.len() as i32;
596    let h = known[0].len() as i32;
597    if known[source.0 as usize][source.1 as usize] != Knowledge::Free {
598        return None;
599    }
600    let mut dist = vec![vec![f64::INFINITY; h as usize]; w as usize];
601    dist[source.0 as usize][source.1 as usize] = 0.0;
602    let mut heap = BinaryHeap::new();
603    heap.push(DijkstraNode {
604        cost: 0.0,
605        cell: source,
606    });
607    while let Some(DijkstraNode { cost, cell }) = heap.pop() {
608        if cost > dist[cell.0 as usize][cell.1 as usize] {
609            continue;
610        }
611        for dx in -1..=1 {
612            for dy in -1..=1 {
613                if dx == 0 && dy == 0 {
614                    continue;
615                }
616                let nx = cell.0 + dx;
617                let ny = cell.1 + dy;
618                if nx < 0 || ny < 0 || nx >= w || ny >= h {
619                    continue;
620                }
621                if known[nx as usize][ny as usize] != Knowledge::Free {
622                    continue;
623                }
624                let step = if dx != 0 && dy != 0 { SQRT2 } else { 1.0 };
625                let next = cost + step;
626                if next < dist[nx as usize][ny as usize] {
627                    dist[nx as usize][ny as usize] = next;
628                    heap.push(DijkstraNode {
629                        cost: next,
630                        cell: (nx, ny),
631                    });
632                }
633            }
634        }
635    }
636    Some(dist)
637}
638
639/// Step the robot down a distance field toward `target` for up to `budget` cells.
640fn follow_gradient(
641    known: &[Vec<Knowledge>],
642    field: &[Vec<f64>],
643    robot: &mut (i32, i32),
644    target: (i32, i32),
645    budget: usize,
646    path: &mut Vec<(i32, i32)>,
647    path_length: &mut f64,
648) {
649    let w = known.len() as i32;
650    let h = known[0].len() as i32;
651    for _ in 0..budget {
652        if *robot == target {
653            break;
654        }
655        let here = field[robot.0 as usize][robot.1 as usize];
656        let mut best: Option<(f64, (i32, i32), f64)> = None;
657        for dx in -1..=1 {
658            for dy in -1..=1 {
659                if dx == 0 && dy == 0 {
660                    continue;
661                }
662                let nx = robot.0 + dx;
663                let ny = robot.1 + dy;
664                if nx < 0 || ny < 0 || nx >= w || ny >= h {
665                    continue;
666                }
667                if known[nx as usize][ny as usize] != Knowledge::Free {
668                    continue;
669                }
670                let d = field[nx as usize][ny as usize];
671                if !d.is_finite() || d >= here {
672                    continue;
673                }
674                let step = if dx != 0 && dy != 0 { SQRT2 } else { 1.0 };
675                let better = match best {
676                    None => true,
677                    Some((best_d, best_cell, _)) => {
678                        d < best_d || (d == best_d && (nx, ny) < best_cell)
679                    }
680                };
681                if better {
682                    best = Some((d, (nx, ny), step));
683                }
684            }
685        }
686        let Some((_, next, step)) = best else { break };
687        *robot = next;
688        path.push(next);
689        *path_length += step;
690    }
691}
692
693#[derive(Debug, Clone, Copy)]
694struct DijkstraNode {
695    cost: f64,
696    cell: (i32, i32),
697}
698
699impl PartialEq for DijkstraNode {
700    fn eq(&self, other: &Self) -> bool {
701        self.cost == other.cost
702    }
703}
704impl Eq for DijkstraNode {}
705impl PartialOrd for DijkstraNode {
706    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
707        Some(self.cmp(other))
708    }
709}
710impl Ord for DijkstraNode {
711    fn cmp(&self, other: &Self) -> Ordering {
712        // Min-heap on cost; tie-break by cell for determinism.
713        other
714            .cost
715            .partial_cmp(&self.cost)
716            .unwrap_or(Ordering::Equal)
717            .then_with(|| other.cell.cmp(&self.cell))
718    }
719}
720
721fn euclid(a: (i32, i32), b: (i32, i32)) -> f64 {
722    let dx = (a.0 - b.0) as f64;
723    let dy = (a.1 - b.1) as f64;
724    (dx * dx + dy * dy).sqrt()
725}
726
727#[cfg(test)]
728mod tests {
729    use super::*;
730    use std::collections::HashSet;
731
732    /// A world with a wall between start and goal, leaving a gap at the top.
733    fn walled_world() -> FrontierNavWorld {
734        let mut world = FrontierNavWorld::new(24, 16, (1, 8), (22, 8)).unwrap();
735        // Vertical wall at x = 12 from y = 0 up to y = 12 (gap above y = 12).
736        world.fill_rect(12, 13, 0, 12);
737        world
738    }
739
740    #[test]
741    fn trace_preserves_rollout_and_records_only_actual_sensing_boundaries() {
742        let world = walled_world();
743        for range in [3.0, 5.0, 7.0] {
744            let config = FrontierNavConfig {
745                sensor_range: range,
746                ..FrontierNavConfig::default()
747            };
748            let legacy = simulate_frontier_navigation(&world, &config).unwrap();
749            let (report, steps) = simulate_frontier_navigation_with_trace(&world, &config).unwrap();
750            assert_eq!(legacy, report);
751            assert!(!steps.is_empty());
752            assert_eq!(steps[0].path_index, 0);
753            assert_eq!(steps.last().unwrap().robot, *report.path.last().unwrap());
754            assert_eq!(steps.last().unwrap().known, report.known);
755            let mut observed = vec![vec![Knowledge::Unknown; 16]; 24];
756            sense(&world, &mut observed, world.start, range);
757            assert_eq!(steps[0].known, observed);
758            let mut length = 0.;
759            for pair in steps.windows(2) {
760                let (a, b) = (&pair[0], &pair[1]);
761                assert!(a.path_index <= b.path_index);
762                assert_eq!(report.path[a.path_index], a.robot);
763                for segment in report.path[a.path_index..=b.path_index].windows(2) {
764                    let (p, q) = (segment[0], segment[1]);
765                    assert_eq!(a.known[q.0 as usize][q.1 as usize], Knowledge::Free);
766                    assert!((p.0 - q.0).abs() <= 1 && (p.1 - q.1).abs() <= 1 && p != q);
767                    length += euclid(p, q);
768                }
769                if a.phase == FrontierNavPhase::Frontier && a.path_index != b.path_index {
770                    sense(&world, &mut observed, b.robot, range);
771                }
772                assert_eq!(b.known, observed);
773            }
774            assert_eq!(length, report.path_length);
775            assert_eq!(
776                steps
777                    .iter()
778                    .filter(|s| s.phase == FrontierNavPhase::Frontier)
779                    .count(),
780                report.frontier_selections
781            );
782        }
783    }
784
785    #[test]
786    fn trace_exposes_candidate_partition_scores_ties_and_terminal_reasons() {
787        let world = walled_world();
788        let config = FrontierNavConfig::default();
789        let (_, steps) = simulate_frontier_navigation_with_trace(&world, &config).unwrap();
790        for s in steps
791            .iter()
792            .filter(|s| s.phase == FrontierNavPhase::Frontier)
793        {
794            let mut actual = HashSet::new();
795            let mut expected = HashSet::new();
796            for x in 0..world.width {
797                for y in 0..world.height {
798                    if s.known[x as usize][y as usize] == Knowledge::Free
799                        && [(1, 0), (-1, 0), (0, 1), (0, -1)].iter().any(|(dx, dy)| {
800                            let (a, b) = (x + dx, y + dy);
801                            world.in_bounds(a, b)
802                                && s.known[a as usize][b as usize] == Knowledge::Unknown
803                        })
804                    {
805                        expected.insert((x, y));
806                    }
807                }
808            }
809            let maximum = s
810                .candidates
811                .iter()
812                .map(|c| c.openness)
813                .max()
814                .unwrap()
815                .max(1) as f64;
816            for c in &s.candidates {
817                assert!(c.members.contains(&c.representative));
818                let mut openness = 0;
819                for &(x, y) in &c.members {
820                    assert!(actual.insert((x, y)));
821                    for (dx, dy) in [(1, 0), (-1, 0), (0, 1), (0, -1)] {
822                        let (a, b) = (x + dx, y + dy);
823                        if world.in_bounds(a, b)
824                            && s.known[a as usize][b as usize] == Knowledge::Unknown
825                        {
826                            openness += 1;
827                        }
828                    }
829                }
830                assert_eq!(openness, c.openness);
831                assert_eq!(c.normalized_openness, openness as f64 / maximum);
832                assert_eq!(
833                    c.goal_gain,
834                    euclid(s.robot, world.goal) - euclid(c.representative, world.goal)
835                );
836                assert_eq!(
837                    c.score,
838                    c.travel_cost.map(|cost| config.goal_weight * c.goal_gain
839                        - config.cost_weight * cost
840                        + config.line_of_sight_weight * if c.line_of_sight { 1. } else { 0. }
841                        + config.openness_weight * c.normalized_openness)
842                );
843            }
844            assert_eq!(actual, expected);
845            let chosen = &s.candidates[s.selected.unwrap()];
846            for c in &s.candidates {
847                if let Some(score) = c.score {
848                    assert!(chosen.score.unwrap() >= score);
849                    if chosen.score == c.score {
850                        assert!(chosen.representative <= c.representative);
851                    }
852                }
853            }
854        }
855        let limit = FrontierNavConfig {
856            max_iterations: 1,
857            ..config.clone()
858        };
859        let (_, trace) = simulate_frontier_navigation_with_trace(&world, &limit).unwrap();
860        assert_eq!(
861            trace.last().unwrap().phase,
862            FrontierNavPhase::IterationLimit
863        );
864        // A one-cell-high corridor makes the complete blocking wall visible
865        // immediately, so there is no unseen wall edge left to investigate.
866        let mut sealed = FrontierNavWorld::new(8, 1, (1, 0), (6, 0)).unwrap();
867        sealed.fill_rect(3, 3, 0, 0);
868        let (_, trace) = simulate_frontier_navigation_with_trace(
869            &sealed,
870            &FrontierNavConfig {
871                sensor_range: 100.,
872                ..config
873            },
874        )
875        .unwrap();
876        assert_eq!(trace.last().unwrap().phase, FrontierNavPhase::NoFrontiers);
877        assert_eq!(trace.last().unwrap().path_index, 0);
878    }
879
880    #[test]
881    fn reaches_goal_around_occlusion() {
882        let report =
883            simulate_frontier_navigation(&walled_world(), &FrontierNavConfig::default()).unwrap();
884        assert!(report.reached_goal, "did not reach goal");
885        assert!(*report.path.last().unwrap() == (22, 8));
886        // Had to choose at least one frontier to get around the wall.
887        assert!(report.frontier_selections >= 1);
888    }
889
890    #[test]
891    fn open_world_reaches_goal() {
892        let world = FrontierNavWorld::new(20, 12, (1, 6), (18, 6)).unwrap();
893        let report = simulate_frontier_navigation(&world, &FrontierNavConfig::default()).unwrap();
894        assert!(report.reached_goal);
895    }
896
897    #[test]
898    fn is_deterministic() {
899        let world = walled_world();
900        let config = FrontierNavConfig::default();
901        let first = simulate_frontier_navigation(&world, &config).unwrap();
902        let second = simulate_frontier_navigation(&world, &config).unwrap();
903        assert_eq!(first.path, second.path);
904        assert_eq!(first.frontier_selections, second.frontier_selections);
905    }
906
907    #[test]
908    fn sensing_is_occlusion_aware() {
909        // A short wall at x = 4 within sensor range of the robot at (1, 8).
910        let mut world = FrontierNavWorld::new(24, 16, (1, 8), (22, 8)).unwrap();
911        world.fill_rect(4, 4, 5, 11);
912        let mut known = vec![vec![Knowledge::Unknown; 16]; 24];
913        sense(&world, &mut known, (1, 8), 6.0);
914        // A near free cell is revealed.
915        assert_eq!(known[3][8], Knowledge::Free);
916        // The near wall face is seen as occupied.
917        assert_eq!(known[4][8], Knowledge::Occupied);
918        // A cell just behind the wall (within range) stays unknown (occluded).
919        assert_eq!(known[6][8], Knowledge::Unknown);
920    }
921
922    #[test]
923    fn rejects_blocked_start() {
924        let mut world = FrontierNavWorld::new(10, 10, (1, 1), (8, 8)).unwrap();
925        world.fill_rect(1, 1, 1, 1);
926        assert!(simulate_frontier_navigation(&world, &FrontierNavConfig::default()).is_err());
927    }
928}