Refactor CBVG for ray-based node creation and angular frontier detection

Major improvements to the Clearance-Based Visibility Graph (CBVG) system:

CBVG Graph:
- Replace grid-based node generation with ray-based placement along lidar scans
- Add angular gap analysis for frontier detection (nodes with uncovered sectors)
- Add clique optimization for merging redundant nodes
- Add path planning with node exclusion for alternative approach angles
- Increase max_nodes to 5000 for large multi-room layouts
- Add configurable max_lidar_range for node creation

Exploration Controller:
- Track CBVG goal nodes that led to collision for each frontier
- Add oscillation detection and frontier suppression
- Implement multi-attempt collision recovery before marking unreachable
- Plan alternative paths by excluding previously failed goal nodes
- Handle collisions during recovery state

Vector Map:
- Add get_path_excluding_nodes() for alternative approach planning
- Integrate CBVG-based frontier detection with configurable parameters

Other:
- Scan module: Include max-range readings for open-space node creation
- Example: Add command-line map file selection
- Temporarily disable release optimizations for debugging

vastu-map/Cargo.toml

@@ -30,6 +30,6 @@ name = "exploration"
 [profile.release]
 opt-level = 3
 lto = true
-codegen-units = 1        # Better optimization, slower compile
-panic = "abort"          # Smaller binary, no unwinding overhead
-strip = true             # Strip debug symbols for smaller binary
+# codegen-units = 1        # Better optimization, slower compile
+# panic = "abort"          # Smaller binary, no unwinding overhead
+# strip = true             # Strip debug symbols for smaller binary

vastu-map/configs/default.yaml

@@ -158,6 +158,12 @@ frontier:
   # Maximum gap width to consider as a single gap (m)
   # Allow gaps across rooms (up to 10m)
   max_gap_width: 10.0
+  # CBVG-based frontier detection parameters
+  # Minimum uncovered angle for a node to be a frontier (radians)
+  # 1.75 rad = 100 degrees
+  min_frontier_angle: 1.75
+  # Maximum distance to check for walls when detecting frontiers (m)
+  max_wall_distance: 0.5
 
 # ============================================================================
 # Occupancy Queries
@@ -180,7 +186,8 @@ cbvg:
   # Sample resolution along Voronoi edges (m)
   voronoi_sample_step: 0.1
   # Maximum edge length between nodes (m)
-  max_edge_length: 0.8
+  # Increased from 0.8 to 1.5 for better graph connectivity during exploration
+  max_edge_length: 1.5
   # Interval for placing coverage nodes in open areas (m)
   # Ensures nodes exist even in areas without medial axis
   coverage_node_interval: 0.35
@@ -192,6 +199,16 @@ cbvg:
   edge_clearance_step: 0.05
   # Wall sampling step for Voronoi approximation (m)
   wall_sample_step: 0.1
+  # Minimum clique size for merging during optimization
+  # Cliques (mutually visible node groups) smaller than this are kept separate
+  min_clique_size_for_merge: 3
+  # Number of lidar scans between clique optimization runs
+  # Set to 1 to optimize after every scan for best graph quality
+  optimize_interval: 1
+  # Maximum range for CBVG node creation (m)
+  # Nodes are only placed up to this distance from robot
+  # (wall detection still uses full lidar range)
+  max_lidar_range: 1.0
 
 # ============================================================================
 # Loop Closure Detection

vastu-map/examples/exploration/main.rs

@@ -10,6 +10,10 @@
 //! # Running
 //!
 //! ```bash
+//! # With a specific map file:
+//! cargo run --example exploration --release -- path/to/map.yaml
+//!
+//! # With random map selection:
 //! cargo run --example exploration --release
 //! ```
 //!
@@ -31,8 +35,8 @@ use vastu_map::exploration::{
 use vastu_map::integration::CoplanarMergeConfig;
 use vastu_map::query::cbvg::ClearanceVisibilityGraph;
 
-/// Maximum simulation time in seconds.
-const MAX_TIME: f32 = 200.0; // 3.3 minutes
+/// Maximum simulation time in seconds (no limit).
+const MAX_TIME: f32 = f32::MAX;
 
 /// Directory containing available maps.
 const MAPS_DIR: &str = concat!(env!("CARGO_MANIFEST_DIR"), "/data/maps");
@@ -173,8 +177,21 @@ fn main() {
     // Ensure results directory exists
     std::fs::create_dir_all("results").ok();
 
-    // Select a random map from available maps
-    let map_file = select_random_map();
+    // Check for command-line argument for map file
+    let args: Vec<String> = std::env::args().collect();
+    let map_file = if args.len() > 1 {
+        let provided_map = &args[1];
+        // Check if file exists
+        if !std::path::Path::new(provided_map).exists() {
+            eprintln!("Error: Map file not found: {}", provided_map);
+            std::process::exit(1);
+        }
+        println!("Using provided map: {}", provided_map);
+        provided_map.clone()
+    } else {
+        // Select a random map from available maps
+        select_random_map()
+    };
 
     // Get default robot radius for clearance checking
     let default_config = HarnessConfig::default();
@@ -261,7 +278,7 @@ fn run_exploration_loop(harness: &mut TestHarness, config: ExplorationConfig) ->
     let mut last_target: Option<Point2D> = None;
     let mut last_cycle_print = 0;
     let mut pending_collision: Option<CollisionEvent> = None;
-    const MAX_STEPS: usize = 20000;
+    const MAX_STEPS: usize = usize::MAX;
     const LIDAR_INTERVAL: usize = 5; // Process lidar every N steps
 
     while step_count < MAX_STEPS && harness.sim_time() < MAX_TIME {
@@ -387,7 +404,42 @@ fn run_exploration_loop(harness: &mut TestHarness, config: ExplorationConfig) ->
             ExplorationState::RecoveringFromCollision { .. } => {
                 // Execute backoff velocity (or stop if none)
                 let vel = step.velocity.unwrap_or(VelocityCommand::new(0.0, 0.0));
-                harness.step(vel.linear, vel.angular);
+                let step_result = harness.step(vel.linear, vel.angular);
+
+                // Check for collision during recovery (defensive - should be rare)
+                if step_result.any_bumper() {
+                    collisions += 1;
+
+                    let collision_type = match (step_result.left_bumper, step_result.right_bumper) {
+                        (true, true) => CollisionType::BumperBoth,
+                        (true, false) => CollisionType::BumperLeft,
+                        (false, true) => CollisionType::BumperRight,
+                        _ => CollisionType::BumperBoth,
+                    };
+
+                    let theta = step_result.physics_pose.theta;
+                    let radius = harness.config().robot.robot_radius;
+                    let point = Point2D::new(
+                        step_result.physics_pose.x + (radius + 0.02) * theta.cos(),
+                        step_result.physics_pose.y + (radius + 0.02) * theta.sin(),
+                    );
+
+                    // Create collision event for controller
+                    pending_collision = Some(CollisionEvent::new(collision_type, point, theta));
+
+                    // Also add virtual wall directly to map
+                    let wall = pending_collision
+                        .as_ref()
+                        .unwrap()
+                        .to_virtual_wall(config.virtual_wall_length);
+                    harness.slam_mut().add_line(wall.line);
+                    virtual_walls += 1;
+
+                    println!(
+                        "    Recovery collision at ({:.2}, {:.2}) - added virtual wall",
+                        point.x, point.y
+                    );
+                }
             }
         }
     }

vastu-map/src/core/scan.rs

@@ -69,7 +69,9 @@ impl PolarScan {
         let mut cloud = PointCloud2D::with_capacity(self.points.len());
 
         for &(angle, dist, quality) in &self.points {
-            if quality >= min_quality && dist >= min_range && dist <= max_range {
+            // Use 1.5% tolerance to include max-range readings (no wall hit)
+            // These are needed for CBVG to create nodes in open-space directions
+            if quality >= min_quality && dist >= min_range && dist <= max_range * 1.015 {
                 let (sin, cos) = angle.sin_cos();
                 cloud.xs.push(dist * cos);
                 cloud.ys.push(dist * sin);
@@ -112,7 +114,9 @@ impl PolarScan {
         target.ys.reserve(self.points.len());
 
         for &(angle, dist, quality) in &self.points {
-            if quality >= min_quality && dist >= min_range && dist <= max_range {
+            // Use 1.5% tolerance to include max-range readings (no wall hit)
+            // These are needed for CBVG to create nodes in open-space directions
+            if quality >= min_quality && dist >= min_range && dist <= max_range * 1.015 {
                 let (sin, cos) = angle.sin_cos();
                 target.xs.push(dist * cos);
                 target.ys.push(dist * sin);

vastu-map/src/exploration/controller.rs

@@ -13,6 +13,8 @@
 //!
 //! This approach eliminates ping-pong behavior and improves coverage efficiency.
 
+use std::collections::{HashMap, VecDeque};
+
 use crate::core::{Point2D, Pose2D};
 use crate::{Frontier, Map, Path};
 
@@ -39,6 +41,8 @@ pub enum ExplorationState {
         waypoint_idx: usize,
         /// Target frontier point.
         target_frontier: Point2D,
+        /// CBVG goal node index used for this path (for collision tracking).
+        goal_node_idx: Option<usize>,
     },
 
     /// Recovering from collision (backing off).
@@ -49,6 +53,12 @@ pub enum ExplorationState {
         backoff_heading: f32,
         /// Position when backoff started.
         start_position: Point2D,
+        /// The frontier we were trying to reach (for alternative path planning).
+        target_frontier: Point2D,
+        /// The CBVG goal node that led to collision (to exclude in retry).
+        failed_goal_node: Option<usize>,
+        /// Number of recovery collision attempts.
+        recovery_attempts: u32,
     },
 
     /// Exploration complete - no frontiers remain.
@@ -179,12 +189,15 @@ pub struct ExplorationController {
     /// Configuration.
     config: ExplorationConfig,
 
-    /// Number of consecutive failed path planning attempts.
+    /// Number of consecutive failed path planning attempts for current frontier.
     failed_planning_attempts: usize,
 
     /// Maximum failed attempts before giving up on a frontier.
     max_failed_attempts: usize,
 
+    /// The frontier viewpoint we're currently attempting (to track per-frontier failures).
+    current_attempt_frontier: Option<Point2D>,
+
     /// Frontiers that have been marked as unreachable.
     unreachable_frontiers: Vec<Point2D>,
 
@@ -203,6 +216,19 @@ pub struct ExplorationController {
     /// Robot position when unreachable frontiers were last cleared.
     /// Used to re-evaluate unreachable frontiers after robot moves significantly.
     position_at_last_unreachable_clear: Option<Point2D>,
+
+    /// CBVG nodes that led to collision for each frontier viewpoint.
+    /// Key is grid-quantized frontier position, value is list of failed node indices.
+    /// Used to find alternative approach angles after collision.
+    collision_nodes: HashMap<(i32, i32), Vec<usize>>,
+
+    /// Recent frontier targets for oscillation detection.
+    /// Stores (grid_x, grid_y) of last N selected frontiers.
+    recent_targets: VecDeque<(i32, i32)>,
+
+    /// Frontiers temporarily suppressed due to oscillation detection.
+    /// Key is grid position, value is suppression count (decrements each cycle).
+    suppressed_frontiers: HashMap<(i32, i32), u32>,
 }
 
 impl ExplorationController {
@@ -218,12 +244,16 @@ impl ExplorationController {
             config,
             failed_planning_attempts: 0,
             max_failed_attempts: 3,
+            current_attempt_frontier: None,
             unreachable_frontiers: Vec::new(),
             frontier_match_distance: 0.5,
             planner,
             history,
             last_update_time: None,
             position_at_last_unreachable_clear: None,
+            collision_nodes: HashMap::new(),
+            recent_targets: VecDeque::with_capacity(8),
+            suppressed_frontiers: HashMap::new(),
         }
     }
 
@@ -232,10 +262,13 @@ impl ExplorationController {
         self.state = ExplorationState::SelectingFrontier;
         self.failed_planning_attempts = 0;
         self.unreachable_frontiers.clear();
+        self.collision_nodes.clear();
         self.planner.reset();
         self.history.clear();
         self.last_update_time = Some(std::time::Instant::now());
         self.position_at_last_unreachable_clear = None;
+        self.recent_targets.clear();
+        self.suppressed_frontiers.clear();
     }
 
     /// Reset to idle state.
@@ -243,10 +276,13 @@ impl ExplorationController {
         self.state = ExplorationState::Idle;
         self.failed_planning_attempts = 0;
         self.unreachable_frontiers.clear();
+        self.collision_nodes.clear();
         self.planner.reset();
         self.history.clear();
         self.last_update_time = None;
         self.position_at_last_unreachable_clear = None;
+        self.recent_targets.clear();
+        self.suppressed_frontiers.clear();
     }
 
     /// Get current state.
@@ -302,17 +338,31 @@ impl ExplorationController {
                 path,
                 waypoint_idx,
                 target_frontier,
-            } => self.follow_path(map, current_pose, path, waypoint_idx, target_frontier),
+                goal_node_idx,
+            } => self.follow_path(
+                map,
+                current_pose,
+                path,
+                waypoint_idx,
+                target_frontier,
+                goal_node_idx,
+            ),
 
             ExplorationState::RecoveringFromCollision {
                 backoff_remaining,
                 backoff_heading,
                 start_position,
+                target_frontier,
+                failed_goal_node,
+                recovery_attempts,
             } => self.execute_backoff(
                 current_pose,
                 backoff_remaining,
                 backoff_heading,
                 start_position,
+                target_frontier,
+                failed_goal_node,
+                recovery_attempts,
             ),
 
             ExplorationState::Complete => ExplorationStep::idle(self.state.clone()),
@@ -322,6 +372,15 @@ impl ExplorationController {
     }
 
     /// Handle a collision event.
+    ///
+    /// When collision occurs during path following:
+    /// 1. Record the failed CBVG goal node (if any)
+    /// 2. Enter recovery state with the target frontier preserved
+    /// 3. After backoff completes, path planning will try alternative nodes
+    ///
+    /// When collision occurs during recovery:
+    /// 1. Increment recovery attempts counter
+    /// 2. If too many attempts, mark frontier as unreachable
     fn handle_collision(&mut self, event: CollisionEvent, current_pose: Pose2D) -> ExplorationStep {
         log::info!(
             "Collision detected: {:?} at ({:.2}, {:.2})",
@@ -333,11 +392,107 @@ impl ExplorationController {
         // Create virtual wall at collision point
         let virtual_wall = event.to_virtual_wall(self.config.virtual_wall_length);
 
-        // Transition to recovery state
+        // Handle collision during recovery - increment attempts and possibly give up
+        if let ExplorationState::RecoveringFromCollision {
+            target_frontier,
+            failed_goal_node,
+            recovery_attempts,
+            backoff_heading,
+            ..
+        } = &self.state
+        {
+            let attempts = *recovery_attempts + 1;
+            let target = *target_frontier;
+            let failed_node = *failed_goal_node;
+
+            log::warn!(
+                "Recovery collision at ({:.2}, {:.2}) - attempt {} of 3",
+                event.point.x,
+                event.point.y,
+                attempts
+            );
+
+            if attempts >= 3 {
+                // Too many recovery collisions - mark frontier as unreachable
+                log::warn!(
+                    "Marking frontier at ({:.2}, {:.2}) as unreachable after {} recovery collisions",
+                    target.x,
+                    target.y,
+                    attempts
+                );
+
+                // Track position for unreachable frontier reset logic
+                if self.unreachable_frontiers.is_empty() {
+                    self.position_at_last_unreachable_clear = Some(current_pose.position());
+                }
+                self.unreachable_frontiers.push(target);
+
+                // Go back to selecting a different frontier
+                self.state = ExplorationState::SelectingFrontier;
+                let mut step = ExplorationStep::idle(self.state.clone());
+                step.virtual_wall = Some(virtual_wall);
+                return step;
+            }
+
+            // Continue recovery with incremented counter
+            // Keep original start_position so backoff distance calculation is correct
+            let original_start =
+                if let ExplorationState::RecoveringFromCollision { start_position, .. } =
+                    &self.state
+                {
+                    *start_position
+                } else {
+                    current_pose.position()
+                };
+
+            self.state = ExplorationState::RecoveringFromCollision {
+                backoff_remaining: self.config.backoff_distance,
+                backoff_heading: *backoff_heading,
+                start_position: original_start, // Keep original start for correct distance calculation
+                target_frontier: target,
+                failed_goal_node: failed_node,
+                recovery_attempts: attempts,
+            };
+
+            let mut step =
+                ExplorationStep::with_velocity(VelocityCommand::stop(), self.state.clone());
+            step.virtual_wall = Some(virtual_wall);
+            return step;
+        }
+
+        // Collision during FollowingPath - extract values then record the failed node
+        let (target_frontier, failed_goal_node) = if let ExplorationState::FollowingPath {
+            target_frontier,
+            goal_node_idx,
+            ..
+        } = &self.state
+        {
+            (*target_frontier, *goal_node_idx)
+        } else {
+            // Collision in some other state (shouldn't happen, but handle gracefully)
+            (current_pose.position(), None)
+        };
+
+        // Record this node as failed for this frontier (after extracting values to avoid borrow conflict)
+        if let Some(node_idx) = failed_goal_node {
+            self.add_collision_node_for_frontier(target_frontier, node_idx);
+        }
+
+        log::info!(
+            "Collision while heading to frontier ({:.2}, {:.2}), failed node: {:?}",
+            target_frontier.x,
+            target_frontier.y,
+            failed_goal_node
+        );
+
+        // Transition to recovery state - preserve target frontier for alternative path planning
         self.state = ExplorationState::RecoveringFromCollision {
             backoff_remaining: self.config.backoff_distance,
             backoff_heading: event.backoff_heading(),
             start_position: current_pose.position(),
+            target_frontier,
+            failed_goal_node,
+            recovery_attempts: 0,
         };
 
         // Return stop command with virtual wall
@@ -364,6 +519,9 @@ impl ExplorationController {
             .unwrap_or(0.1);
         self.last_update_time = Some(std::time::Instant::now());
 
+        // Decay suppression counters from oscillation detection
+        self.decay_suppressed_frontiers();
+
         // Update history with current position
         self.history.mark_visited(robot_pos);
 
@@ -383,19 +541,22 @@ impl ExplorationController {
 
             if should_clear {
                 log::debug!(
-                    "Clearing {} unreachable frontiers after robot moved {:.1}m",
+                    "Clearing {} unreachable frontiers and {} collision node entries after robot moved {:.1}m",
                     self.unreachable_frontiers.len(),
+                    self.collision_nodes.len(),
                     UNREACHABLE_RESET_DISTANCE
                 );
                 self.unreachable_frontiers.clear();
+                self.collision_nodes.clear(); // Also reset collision history
                 self.position_at_last_unreachable_clear = Some(robot_pos);
             }
         }
 
-        // Filter out unreachable frontiers (using viewpoint for distance checks)
+        // Filter out unreachable and suppressed frontiers (using viewpoint for distance checks)
         let reachable_frontiers: Vec<Frontier> = all_frontiers
             .iter()
             .filter(|f| !self.is_frontier_unreachable(f.viewpoint))
+            .filter(|f| !self.is_frontier_suppressed(f.viewpoint))
             .cloned()
             .collect();
 
@@ -423,12 +584,50 @@ impl ExplorationController {
             .update(&reachable_frontiers, robot_pos, &self.history, dt);
 
         // Try to find a frontier with a valid path
+        // Safety limit to prevent infinite loops - max iterations = 2 * number of frontiers + regions
+        const MAX_LOOP_ITERATIONS: usize = 100;
+        let mut loop_iterations = 0;
+
         loop {
+            loop_iterations += 1;
+            if loop_iterations > MAX_LOOP_ITERATIONS {
+                log::error!(
+                    "Exploration stuck: exceeded {} iterations in select_frontier loop",
+                    MAX_LOOP_ITERATIONS
+                );
+                self.state = ExplorationState::Failed {
+                    reason: format!(
+                        "frontier selection loop exceeded {} iterations",
+                        MAX_LOOP_ITERATIONS
+                    ),
+                };
+                return ExplorationStep::idle(self.state.clone());
+            }
+
             // Check if we've exhausted all regions
             if self.planner.plan().is_complete() {
-                // Force replan to pick up any new frontiers
+                // Re-filter frontiers to exclude those marked unreachable during this loop
+                let still_reachable: Vec<Frontier> = reachable_frontiers
+                    .iter()
+                    .filter(|f| !self.is_frontier_unreachable(f.viewpoint))
+                    .cloned()
+                    .collect();
+
+                if still_reachable.is_empty() {
+                    // All frontiers are now unreachable
+                    log::warn!(
+                        "Exploration stuck: all {} frontiers are unreachable",
+                        reachable_frontiers.len()
+                    );
+                    self.state = ExplorationState::Failed {
+                        reason: format!("{} frontiers unreachable", reachable_frontiers.len()),
+                    };
+                    return ExplorationStep::idle(self.state.clone());
+                }
+
+                // Force replan with updated reachable frontiers
                 self.planner
-                    .force_replan(&reachable_frontiers, robot_pos, &self.history);
+                    .force_replan(&still_reachable, robot_pos, &self.history);
 
                 if self.planner.plan().is_complete() || self.planner.plan().is_empty() {
                     // Still no regions - exploration complete
@@ -454,13 +653,14 @@ impl ExplorationController {
 
             // Find nearest CBVG node to the frontier - guaranteed reachable
             // CBVG nodes are in visible areas with proper wall clearance
-            let target = match selected_frontier {
+            let (target, frontier_viewpoint) = match selected_frontier {
                 Some(f) => {
                     // Try to find nearest CBVG node (within 2m of frontier viewpoint)
-                    match map.nearest_cbvg_node(f.viewpoint, 2.0) {
+                    let target_pos = match map.nearest_cbvg_node(f.viewpoint, 2.0) {
                         Some(node_pos) => node_pos,
                         None => f.viewpoint, // Fallback to viewpoint if no node nearby
-                    }
+                    };
+                    (target_pos, f.viewpoint)
                 }
                 None => {
                     // Current region exhausted, advance to next
@@ -483,28 +683,79 @@ impl ExplorationController {
                 );
                 // Mark as visited so we don't keep selecting it
                 self.history.mark_visited(target);
-                self.planner.plan_mut().advance_region();
+
+                // Remove this frontier from the current region so it won't be selected again
+                if let Some(region) = self.planner.plan_mut().current_region_mut() {
+                    region
+                        .frontiers
+                        .retain(|f| f.viewpoint.distance(frontier_viewpoint) > 0.1);
+                }
+
+                // If region is now empty, advance to next region
+                if self
+                    .planner
+                    .plan()
+                    .current_region()
+                    .is_none_or(|r| r.is_empty())
+                {
+                    self.planner.plan_mut().advance_region();
+                }
                 continue;
             }
 
+            // Get excluded nodes for this frontier (nodes that led to collision before)
+            let excluded_nodes = self.get_collision_nodes_for_frontier(frontier_viewpoint);
+
             log::debug!(
-                "Selected frontier viewpoint at ({:.2}, {:.2}) from region {}, distance: {:.2}m",
+                "Selected frontier viewpoint at ({:.2}, {:.2}) from region {}, distance: {:.2}m, excluded nodes: {:?}",
                 target.x,
                 target.y,
                 region.id,
-                distance_to_target
+                distance_to_target,
+                excluded_nodes
             );
 
-            // Plan path directly to viewpoint (no offset needed - viewpoint is already safe)
-            match map.get_path(robot_pos, target) {
-                Some(path) => {
+            // Plan path directly to viewpoint, excluding nodes that led to collision
+            match map.get_path_excluding_nodes(robot_pos, target, &excluded_nodes) {
+                Some((path, goal_node_idx)) => {
                     log::debug!(
-                        "Path planned: {} waypoints, {:.2}m total",
+                        "Path planned: {} waypoints, {:.2}m total, via node {}",
                         path.points.len(),
-                        path.length
+                        path.length,
+                        goal_node_idx
                     );
 
+                    // Check for oscillation before committing to this frontier
+                    // If oscillation is detected, suppress the "other" frontier and continue with this one
+                    if let Some(suppress_key) =
+                        self.record_frontier_and_check_oscillation(frontier_viewpoint)
+                    {
+                        log::info!(
+                            "Oscillation detected - suppressing other frontier at grid {:?}, continuing with ({:.2}, {:.2})",
+                            suppress_key,
+                            frontier_viewpoint.x,
+                            frontier_viewpoint.y
+                        );
+                        // Suppress the other frontier for 10 cycles
+                        self.suppressed_frontiers.insert(suppress_key, 10);
+
+                        // Also remove the suppressed frontier from planner's current region
+                        // so it won't be selected again in subsequent loop iterations
+                        if let Some(region) = self.planner.plan_mut().current_region_mut() {
+                            let grid_cell_size = 0.5; // Same as frontier_grid_key
+                            region.frontiers.retain(|f| {
+                                let key = (
+                                    (f.viewpoint.x / grid_cell_size) as i32,
+                                    (f.viewpoint.y / grid_cell_size) as i32,
+                                );
+                                key != suppress_key
+                            });
+                        }
+                        // Continue with the current frontier (don't skip it)
+                    }
+
                     self.failed_planning_attempts = 0;
+                    self.current_attempt_frontier = None;
 
                     // Immediately start following the path
                     let result = self.path_follower.follow(
@@ -517,7 +768,9 @@ impl ExplorationController {
                     self.state = ExplorationState::FollowingPath {
                         path: path.clone(),
                         waypoint_idx: result.waypoint_idx,
-                        target_frontier: target, // Viewpoint is already a safe target
+                        // Use frontier_viewpoint (not target/CBVG node) so collision tracking matches
+                        target_frontier: frontier_viewpoint,
+                        goal_node_idx: Some(goal_node_idx), // Track which node we're using
                     };
 
                     let mut step =
@@ -534,7 +787,19 @@ impl ExplorationController {
                         target.y
                     );
 
-                    self.failed_planning_attempts += 1;
+                    // Track per-frontier failures - reset counter if we switched frontiers
+                    let is_same_frontier = self
+                        .current_attempt_frontier
+                        .map(|prev| prev.distance(frontier_viewpoint) < 0.1)
+                        .unwrap_or(false);
+
+                    if is_same_frontier {
+                        self.failed_planning_attempts += 1;
+                    } else {
+                        // New frontier - reset counter
+                        self.current_attempt_frontier = Some(frontier_viewpoint);
+                        self.failed_planning_attempts = 1;
+                    }
 
                     if self.failed_planning_attempts >= self.max_failed_attempts {
                         // Track position when we first mark any frontier as unreachable
@@ -543,19 +808,20 @@ impl ExplorationController {
                             self.position_at_last_unreachable_clear = Some(robot_pos);
                         }
 
-                        // Mark this frontier as unreachable
-                        self.unreachable_frontiers.push(target);
+                        // Mark this frontier as unreachable (use viewpoint, not CBVG node)
+                        self.unreachable_frontiers.push(frontier_viewpoint);
                         self.failed_planning_attempts = 0;
+                        self.current_attempt_frontier = None;
                         log::warn!(
                             "Frontier marked as unreachable after {} failed attempts",
                             self.max_failed_attempts
                         );
 
-                        // Remove from current region and try again
+                        // Remove from current region and try again (use viewpoint for matching)
                         if let Some(region) = self.planner.plan_mut().current_region_mut() {
                             region
                                 .frontiers
-                                .retain(|f| f.viewpoint.distance(target) > 0.1);
+                                .retain(|f| f.viewpoint.distance(frontier_viewpoint) > 0.1);
                         }
                         continue;
                     }
@@ -575,6 +841,7 @@ impl ExplorationController {
         path: Path,
         waypoint_idx: usize,
         target_frontier: Point2D,
+        goal_node_idx: Option<usize>,
     ) -> ExplorationStep {
         let result = self.path_follower.follow(
             current_pose,
@@ -590,6 +857,8 @@ impl ExplorationController {
                 target_frontier.x,
                 target_frontier.y
             );
+            // Clear collision nodes for this frontier since we reached it successfully
+            self.clear_collision_nodes_for_frontier(target_frontier);
             self.state = ExplorationState::SelectingFrontier;
 
             return ExplorationStep::idle(self.state.clone());
@@ -604,6 +873,7 @@ impl ExplorationController {
             path,
             waypoint_idx: result.waypoint_idx,
             target_frontier,
+            goal_node_idx,
         };
 
         let mut step = ExplorationStep::with_velocity(result.velocity, self.state.clone());
@@ -619,6 +889,9 @@ impl ExplorationController {
         backoff_remaining: f32,
         backoff_heading: f32,
         start_position: Point2D,
+        target_frontier: Point2D,
+        failed_goal_node: Option<usize>,
+        recovery_attempts: u32,
     ) -> ExplorationStep {
         // Calculate how far we've backed up
         let backed_up = current_pose.position().distance(start_position);
@@ -626,7 +899,12 @@ impl ExplorationController {
 
         if remaining <= 0.0 {
             // Backoff complete - go back to selecting frontier
-            log::debug!("Backoff complete, selecting new frontier");
+            // The collision node has already been recorded in handle_collision
+            log::debug!(
+                "Backoff complete, selecting new frontier (failed node: {:?}, attempts: {})",
+                failed_goal_node,
+                recovery_attempts
+            );
             self.state = ExplorationState::SelectingFrontier;
             return ExplorationStep::idle(self.state.clone());
         }
@@ -636,6 +914,9 @@ impl ExplorationController {
             backoff_remaining: remaining,
             backoff_heading,
             start_position,
+            target_frontier,
+            failed_goal_node,
+            recovery_attempts,
         };
 
         // Command slow reverse motion
@@ -665,6 +946,141 @@ impl ExplorationController {
     pub fn config(&self) -> &ExplorationConfig {
         &self.config
     }
+
+    // ─────────────────────────────────────────────────────────────────────────
+    // Oscillation Detection (prevents ping-pong between nearby frontiers)
+    // ─────────────────────────────────────────────────────────────────────────
+
+    /// Record a frontier selection and check for oscillation or repeated visits.
+    /// Returns the frontier to suppress if a problematic pattern is detected.
+    fn record_frontier_and_check_oscillation(&mut self, frontier: Point2D) -> Option<(i32, i32)> {
+        let key = self.frontier_grid_key(frontier);
+
+        // Add to recent targets (keep last 8)
+        self.recent_targets.push_back(key);
+        if self.recent_targets.len() > 8 {
+            self.recent_targets.pop_front();
+        }
+
+        let targets: Vec<_> = self.recent_targets.iter().collect();
+        let len = targets.len();
+
+        // Check 1: A-B-A-B pattern (oscillation between two frontiers)
+        if len >= 4 {
+            let a = targets[len - 4];
+            let b = targets[len - 3];
+            let c = targets[len - 2];
+            let d = targets[len - 1]; // This is the current selection (key)
+
+            // Pattern: A, B, A, B (same A and B repeating)
+            if a == c && b == d && a != b {
+                log::warn!(
+                    "Oscillation detected between frontiers at grid {:?} and {:?}",
+                    a,
+                    b
+                );
+
+                // Copy the key to suppress before clearing
+                let suppress_key = *c;
+                self.recent_targets.clear();
+                return Some(suppress_key);
+            }
+        }
+
+        // Check 2: Same frontier visited 3+ times in last 6 selections (stuck on one frontier)
+        if len >= 6 {
+            let current = targets[len - 1];
+            let count = targets[len - 6..].iter().filter(|&&t| t == current).count();
+
+            if count >= 3 {
+                log::warn!(
+                    "Repeated visits detected: frontier at grid {:?} visited {} times in last 6",
+                    current,
+                    count
+                );
+
+                // Suppress this frontier since we keep returning to it
+                let suppress_key = *current;
+                self.recent_targets.clear();
+                return Some(suppress_key);
+            }
+        }
+
+        // Check 3: Three frontiers in rotation (A-B-C-A-B-C pattern)
+        if len >= 6 {
+            let a = targets[len - 6];
+            let b = targets[len - 5];
+            let c = targets[len - 4];
+            let d = targets[len - 3];
+            let e = targets[len - 2];
+            let f = targets[len - 1];
+
+            // Pattern: A, B, C, A, B, C
+            if a == d && b == e && c == f && a != b && b != c && a != c {
+                log::warn!("Three-way rotation detected: {:?}, {:?}, {:?}", a, b, c);
+
+                // Suppress the oldest frontier in the pattern
+                let suppress_key = *a;
+                self.recent_targets.clear();
+                return Some(suppress_key);
+            }
+        }
+
+        None
+    }
+
+    /// Check if a frontier is currently suppressed due to oscillation.
+    fn is_frontier_suppressed(&self, frontier: Point2D) -> bool {
+        let key = self.frontier_grid_key(frontier);
+        self.suppressed_frontiers.get(&key).copied().unwrap_or(0) > 0
+    }
+
+    /// Decrement suppression counters (call once per frontier selection cycle).
+    fn decay_suppressed_frontiers(&mut self) {
+        self.suppressed_frontiers.retain(|_, count| {
+            *count = count.saturating_sub(1);
+            *count > 0
+        });
+    }
+
+    // ─────────────────────────────────────────────────────────────────────────
+    // Collision Node Tracking (for alternative approach angles)
+    // ─────────────────────────────────────────────────────────────────────────
+
+    /// Convert a frontier point to a grid key for collision tracking.
+    /// Uses 0.5m grid cells to group nearby frontiers.
+    fn frontier_grid_key(&self, p: Point2D) -> (i32, i32) {
+        ((p.x / 0.5) as i32, (p.y / 0.5) as i32)
+    }
+
+    /// Record a failed CBVG goal node for a frontier.
+    /// This node led to a collision when trying to reach the frontier.
+    fn add_collision_node_for_frontier(&mut self, frontier: Point2D, node_idx: usize) {
+        let key = self.frontier_grid_key(frontier);
+        let nodes = self.collision_nodes.entry(key).or_default();
+        if !nodes.contains(&node_idx) {
+            nodes.push(node_idx);
+            log::debug!(
+                "Recorded failed node {} for frontier at ({:.2}, {:.2})",
+                node_idx,
+                frontier.x,
+                frontier.y
+            );
+        }
+    }
+
+    /// Get failed CBVG goal nodes for a frontier.
+    /// Returns nodes that should be excluded when planning paths to this frontier.
+    fn get_collision_nodes_for_frontier(&self, frontier: Point2D) -> Vec<usize> {
+        let key = self.frontier_grid_key(frontier);
+        self.collision_nodes.get(&key).cloned().unwrap_or_default()
+    }
+
+    /// Clear collision nodes for a frontier (called when frontier is successfully reached).
+    fn clear_collision_nodes_for_frontier(&mut self, frontier: Point2D) {
+        let key = self.frontier_grid_key(frontier);
+        self.collision_nodes.remove(&key);
+    }
 }
 
 #[cfg(test)]
@@ -731,6 +1147,22 @@ mod tests {
             })
         }
 
+        fn get_path_excluding_nodes(
+            &self,
+            from: Point2D,
+            to: Point2D,
+            _excluded_goal_nodes: &[usize],
+        ) -> Option<(Path, usize)> {
+            // Simple direct path, return node index 0
+            Some((
+                Path {
+                    points: vec![from, to],
+                    length: from.distance(to),
+                },
+                0,
+            ))
+        }
+
         fn nearest_cbvg_node(&self, target: Point2D, _max_distance: f32) -> Option<Point2D> {
             // For testing, just return the target point
             Some(target)

vastu-map/src/exploration/planner.rs

@@ -469,12 +469,15 @@ impl ExplorationPlanner {
 }
 
 /// Normalize an angle to [-PI, PI].
+/// Uses modular arithmetic to avoid potential infinite loops with extreme values.
 fn normalize_angle(angle: f32) -> f32 {
-    let mut result = angle;
-    while result > PI {
-        result -= 2.0 * PI;
+    if !angle.is_finite() {
+        return 0.0;
     }
-    while result < -PI {
+    let mut result = angle % (2.0 * PI);
+    if result > PI {
+        result -= 2.0 * PI;
+    } else if result < -PI {
         result += 2.0 * PI;
     }
     result

vastu-map/src/lib.rs

@@ -388,6 +388,27 @@ pub trait Map: Send + Sync {
     /// Path if one exists, None otherwise.
     fn get_path(&self, from: Point2D, to: Point2D) -> Option<Path>;
 
+    /// Plan path between two points, excluding specific CBVG goal nodes.
+    ///
+    /// This is used for finding alternative approach angles after a collision.
+    /// When navigation to a frontier causes a collision, the goal node that
+    /// led to the collision can be excluded, forcing the planner to find a
+    /// different approach.
+    ///
+    /// # Arguments
+    /// * `from` - Start position
+    /// * `to` - Goal position
+    /// * `excluded_goal_nodes` - CBVG node indices to exclude when selecting the goal node
+    ///
+    /// # Returns
+    /// Tuple of (Path, goal_node_index) if path exists, None otherwise.
+    fn get_path_excluding_nodes(
+        &self,
+        from: Point2D,
+        to: Point2D,
+        excluded_goal_nodes: &[usize],
+    ) -> Option<(Path, usize)>;
+
     /// Find the nearest CBVG node to a target point.
     ///
     /// This is useful for exploration to find reachable navigation targets

vastu-map/src/query/cbvg/config.rs

@@ -52,20 +52,41 @@ pub struct CBVGConfig {
     /// Points are sampled along walls for Voronoi computation.
     /// Default: 0.1
     pub wall_sample_step: f32,
+
+    /// Minimum clique size for merging during optimization.
+    /// Cliques (mutually visible node groups) smaller than this are kept separate.
+    /// Set to 3 to merge triangles and larger clusters.
+    /// Default: 3
+    pub min_clique_size_for_merge: usize,
+
+    /// Number of lidar scans between clique optimization runs.
+    /// Higher values reduce CPU but may leave redundant nodes longer.
+    /// Set to 1 to optimize after every scan.
+    /// Default: 1
+    pub optimize_interval: usize,
+
+    /// Maximum lidar range (meters).
+    /// When a scan point is at or near this distance, it indicates no wall was hit
+    /// (open space). Nodes are still added along the ray up to this distance.
+    /// Default: 8.0
+    pub max_lidar_range: f32,
 }
 
 impl Default for CBVGConfig {
     fn default() -> Self {
         Self {
-            min_clearance: 0.3,
+            min_clearance: 0.25, // Reduced since robot radius is accounted separately
             min_clearance_narrow: 0.18,
             voronoi_sample_step: 0.1,
-            max_edge_length: 0.8,
-            coverage_node_interval: 0.35,
-            node_merge_distance: 0.15,
-            max_nodes: 500,
+            max_edge_length: 0.8,        // Reduced for tighter connectivity
+            coverage_node_interval: 0.4, // Reasonable node spacing
+            node_merge_distance: 0.4,    // Minimum distance between nodes
+            max_nodes: 5000,             // Support large multi-room layouts
             edge_clearance_step: 0.05,
             wall_sample_step: 0.1,
+            min_clique_size_for_merge: 3,
+            optimize_interval: 1,
+            max_lidar_range: 8.0,
         }
     }
 }
@@ -130,6 +151,24 @@ impl CBVGConfig {
         self
     }
 
+    /// Builder-style setter for minimum clique size for merging.
+    pub fn with_min_clique_size_for_merge(mut self, size: usize) -> Self {
+        self.min_clique_size_for_merge = size;
+        self
+    }
+
+    /// Builder-style setter for optimization interval.
+    pub fn with_optimize_interval(mut self, interval: usize) -> Self {
+        self.optimize_interval = interval;
+        self
+    }
+
+    /// Builder-style setter for maximum lidar range.
+    pub fn with_max_lidar_range(mut self, range: f32) -> Self {
+        self.max_lidar_range = range;
+        self
+    }
+
     /// Get the effective clearance for a given measured clearance.
     ///
     /// Returns `min_clearance` for open areas, or `min_clearance_narrow`
@@ -151,15 +190,18 @@ mod tests {
     #[test]
     fn test_default_config() {
         let config = CBVGConfig::default();
-        assert_eq!(config.min_clearance, 0.3);
+        assert_eq!(config.min_clearance, 0.25);
         assert_eq!(config.min_clearance_narrow, 0.18);
         assert_eq!(config.voronoi_sample_step, 0.1);
         assert_eq!(config.max_edge_length, 0.8);
-        assert_eq!(config.coverage_node_interval, 0.35);
-        assert_eq!(config.node_merge_distance, 0.15);
-        assert_eq!(config.max_nodes, 500);
+        assert_eq!(config.coverage_node_interval, 0.4);
+        assert_eq!(config.node_merge_distance, 0.4);
+        assert_eq!(config.max_nodes, 5000);
         assert_eq!(config.edge_clearance_step, 0.05);
         assert_eq!(config.wall_sample_step, 0.1);
+        assert_eq!(config.min_clique_size_for_merge, 3);
+        assert_eq!(config.optimize_interval, 1);
+        assert_eq!(config.max_lidar_range, 8.0);
     }
 
     #[test]
@@ -177,7 +219,7 @@ mod tests {
         let config = CBVGConfig::default();
 
         // Open area - use full clearance
-        assert_eq!(config.effective_clearance(0.5), 0.3);
+        assert_eq!(config.effective_clearance(0.5), 0.25);
 
         // Narrow passage - use reduced clearance
         assert_eq!(config.effective_clearance(0.2), 0.18);

vastu-map/src/query/cbvg/mod.rs

@@ -78,6 +78,6 @@ pub use graph::ClearanceVisibilityGraph;
 pub use node::{CBVGNode, NodeType};
 pub use visibility::VisibilityRegion;
 pub use voronoi::{
-    VoronoiEdge, compute_voronoi_edges, generate_medial_axis_nodes, generate_nodes_from_visibility,
-    generate_nodes_hybrid, sample_medial_axis_nodes, sample_wall_points,
+    VoronoiEdge, compute_voronoi_edges, generate_grid_nodes, sample_medial_axis_nodes,
+    sample_wall_points,
 };

vastu-map/src/query/cbvg/visibility.rs

@@ -25,6 +25,10 @@ pub struct VisibilityRegion {
     /// Indexed as grid[y_cell][x_cell].
     grid: Vec<Vec<bool>>,
 
+    /// 2D grid of processed cells: true = already processed for node generation.
+    /// Used to track which visible cells have been returned by get_newly_visible_cells().
+    processed: Vec<Vec<bool>>,
+
     /// Resolution of the visibility grid (meters per cell).
     resolution: f32,
 
@@ -47,6 +51,7 @@ impl VisibilityRegion {
     pub fn new(resolution: f32) -> Self {
         Self {
             grid: Vec::new(),
+            processed: Vec::new(),
             resolution,
             origin: Point2D::zero(),
             width: 0,
@@ -97,7 +102,9 @@ impl VisibilityRegion {
 
     /// Update visibility from a lidar scan.
     ///
-    /// Marks all cells along rays from robot to scan points as visible.
+    /// Marks all cells in the scanned area as visible by filling triangular
+    /// sectors between adjacent lidar rays. This ensures the entire interior
+    /// of the scanned region is marked visible, not just the rays themselves.
     ///
     /// # Arguments
     /// * `robot_pose` - Current robot pose (position and heading)
@@ -112,9 +119,38 @@ impl VisibilityRegion {
         // First, ensure grid covers all scan points plus robot position
         self.ensure_coverage(robot_pos, scan_points);
 
-        // Mark rays from robot to each scan point
-        for &scan_point in scan_points {
-            self.mark_ray_visible(robot_pos, scan_point);
+        // Sort scan points by angle from robot for proper sector filling
+        let mut sorted_points: Vec<Point2D> = scan_points.to_vec();
+        sorted_points.sort_by(|a, b| {
+            let angle_a = (a.y - robot_pos.y).atan2(a.x - robot_pos.x);
+            let angle_b = (b.y - robot_pos.y).atan2(b.x - robot_pos.x);
+            angle_a
+                .partial_cmp(&angle_b)
+                .unwrap_or(std::cmp::Ordering::Equal)
+        });
+
+        // Fill triangular sectors between adjacent scan points
+        // This marks the entire interior as visible, not just the rays
+        for i in 0..sorted_points.len() {
+            let p1 = sorted_points[i];
+            let p2 = sorted_points[(i + 1) % sorted_points.len()];
+
+            // Check if the angle between rays is small enough to fill
+            // (avoid filling across large gaps like behind the robot)
+            let angle1 = (p1.y - robot_pos.y).atan2(p1.x - robot_pos.x);
+            let angle2 = (p2.y - robot_pos.y).atan2(p2.x - robot_pos.x);
+            let mut angle_diff = (angle2 - angle1).abs();
+            if angle_diff > std::f32::consts::PI {
+                angle_diff = 2.0 * std::f32::consts::PI - angle_diff;
+            }
+
+            // Only fill sectors with small angular gaps (< 15 degrees)
+            if angle_diff < 0.26 {
+                self.fill_triangle(robot_pos, p1, p2);
+            } else {
+                // For larger gaps, just mark the rays
+                self.mark_ray_visible(robot_pos, p1);
+            }
         }
     }
 
@@ -180,8 +216,9 @@ impl VisibilityRegion {
         self.width = ((bounds.width() / self.resolution).ceil() as usize).max(1);
         self.height = ((bounds.height() / self.resolution).ceil() as usize).max(1);
 
-        // Create grid filled with false (not visible)
+        // Create grids filled with false (not visible, not processed)
         self.grid = vec![vec![false; self.width]; self.height];
+        self.processed = vec![vec![false; self.width]; self.height];
     }
 
     /// Expand the grid to cover new bounds while preserving existing data.
@@ -198,20 +235,28 @@ impl VisibilityRegion {
         let new_width = ((expanded.width() / self.resolution).ceil() as usize).max(1);
         let new_height = ((expanded.height() / self.resolution).ceil() as usize).max(1);
 
-        // Create new grid
+        // Create new grids
         let mut new_grid = vec![vec![false; new_width]; new_height];
+        let mut new_processed = vec![vec![false; new_width]; new_height];
 
         // Copy existing data
         let offset_x = ((self.origin.x - new_origin.x) / self.resolution).round() as isize;
         let offset_y = ((self.origin.y - new_origin.y) / self.resolution).round() as isize;
 
-        for (old_y, row) in self.grid.iter().enumerate() {
+        for old_y in 0..self.height {
             let new_y = old_y as isize + offset_y;
             if new_y >= 0 && (new_y as usize) < new_height {
-                for (old_x, &visible) in row.iter().enumerate() {
+                for old_x in 0..self.width {
                     let new_x = old_x as isize + offset_x;
-                    if new_x >= 0 && (new_x as usize) < new_width && visible {
-                        new_grid[new_y as usize][new_x as usize] = true;
+                    if new_x >= 0 && (new_x as usize) < new_width {
+                        let nx = new_x as usize;
+                        let ny = new_y as usize;
+                        if self.grid[old_y][old_x] {
+                            new_grid[ny][nx] = true;
+                        }
+                        if self.processed[old_y][old_x] {
+                            new_processed[ny][nx] = true;
+                        }
                     }
                 }
             }
@@ -222,6 +267,7 @@ impl VisibilityRegion {
         self.width = new_width;
         self.height = new_height;
         self.grid = new_grid;
+        self.processed = new_processed;
     }
 
     /// Mark all cells along a ray from start to end as visible.
@@ -266,6 +312,54 @@ impl VisibilityRegion {
         }
     }
 
+    /// Fill a triangular region as visible.
+    ///
+    /// Uses a scanline algorithm to fill the triangle formed by three points.
+    fn fill_triangle(&mut self, p0: Point2D, p1: Point2D, p2: Point2D) {
+        // Convert to grid coordinates
+        let (x0, y0) = self.world_to_grid(p0);
+        let (x1, y1) = self.world_to_grid(p1);
+        let (x2, y2) = self.world_to_grid(p2);
+
+        // Find bounding box
+        let min_y = y0.min(y1).min(y2).max(0);
+        let max_y = y0.max(y1).max(y2).min(self.height as i32 - 1);
+        let min_x = x0.min(x1).min(x2).max(0);
+        let max_x = x0.max(x1).max(x2).min(self.width as i32 - 1);
+
+        // Convert to f32 for edge calculations
+        let (x0, y0) = (x0 as f32, y0 as f32);
+        let (x1, y1) = (x1 as f32, y1 as f32);
+        let (x2, y2) = (x2 as f32, y2 as f32);
+
+        // Use barycentric coordinates to check if points are inside triangle
+        let area = 0.5 * (-y1 * x2 + y0 * (-x1 + x2) + x0 * (y1 - y2) + x1 * y2);
+        if area.abs() < 0.001 {
+            // Degenerate triangle, just mark the edges
+            self.mark_ray_visible(p0, p1);
+            self.mark_ray_visible(p0, p2);
+            return;
+        }
+
+        let sign = if area < 0.0 { -1.0 } else { 1.0 };
+
+        // Scanline fill
+        for py in min_y..=max_y {
+            for px in min_x..=max_x {
+                let px_f = px as f32;
+                let py_f = py as f32;
+
+                // Calculate barycentric coordinates
+                let s = sign * (y0 * x2 - x0 * y2 + (y2 - y0) * px_f + (x0 - x2) * py_f);
+                let t = sign * (x0 * y1 - y0 * x1 + (y0 - y1) * px_f + (x1 - x0) * py_f);
+
+                if s >= 0.0 && t >= 0.0 && (s + t) <= 2.0 * area.abs() * sign {
+                    self.mark_cell_visible(px, py);
+                }
+            }
+        }
+    }
+
     /// Convert world coordinates to grid cell indices.
     fn world_to_grid(&self, point: Point2D) -> (i32, i32) {
         let x = ((point.x - self.origin.x) / self.resolution).floor() as i32;
@@ -304,6 +398,40 @@ impl VisibilityRegion {
         self.visible_cell_count() as f32 / self.total_cell_count() as f32
     }
 
+    /// Get newly visible cells that haven't been processed yet.
+    ///
+    /// Returns the center points of cells that are visible but not yet processed,
+    /// and marks them as processed. This is used for incremental node generation
+    /// in the CBVG graph.
+    ///
+    /// # Returns
+    /// Vector of cell center points in world coordinates.
+    pub fn get_newly_visible_cells(&mut self) -> Vec<Point2D> {
+        let mut new_cells = Vec::new();
+
+        for y in 0..self.height {
+            for x in 0..self.width {
+                if self.grid[y][x] && !self.processed[y][x] {
+                    new_cells.push(self.grid_to_world_center(x, y));
+                    self.processed[y][x] = true;
+                }
+            }
+        }
+
+        new_cells
+    }
+
+    /// Reset the processed state for all cells.
+    ///
+    /// This allows regenerating nodes in all visible areas.
+    pub fn reset_processed(&mut self) {
+        for row in &mut self.processed {
+            for cell in row {
+                *cell = false;
+            }
+        }
+    }
+
     /// Iterator over all visible cell center points.
     pub fn visible_points(&self) -> impl Iterator<Item = Point2D> + '_ {
         self.grid.iter().enumerate().flat_map(move |(y, row)| {

vastu-map/src/query/cbvg/voronoi.rs

@@ -8,8 +8,7 @@ use crate::features::Line2D;
 
 use super::clearance::{min_distance_to_walls, sample_line};
 use super::config::CBVGConfig;
-use super::node::{CBVGNode, NodeType};
-use super::visibility::VisibilityRegion;
+use super::node::CBVGNode;
 
 /// A Voronoi edge connecting two points in free space.
 #[derive(Clone, Debug)]
@@ -124,15 +123,11 @@ pub fn compute_voronoi_edges(
 }
 
 /// Check if a point is near the medial axis (equidistant from multiple walls).
-///
-/// A point is on the medial axis if it's approximately equidistant from
-/// its two nearest walls.
 fn is_near_medial_axis(point: Point2D, lines: &[Line2D], config: &CBVGConfig) -> bool {
     if lines.len() < 2 {
         return false;
     }
 
-    // Find distances to all walls
     let mut distances: Vec<(f32, usize)> = lines
         .iter()
         .enumerate()
@@ -148,16 +143,12 @@ fn is_near_medial_axis(point: Point2D, lines: &[Line2D], config: &CBVGConfig) ->
     let d1 = distances[0].0;
     let d2 = distances[1].0;
 
-    // Point must have minimum clearance
     if d1 < config.min_clearance_narrow {
         return false;
     }
 
-    // Check if approximately equidistant (within tolerance)
     let tolerance = config.voronoi_sample_step * 1.5;
     let diff = (d1 - d2).abs();
-
-    // On medial axis if distances are similar
     diff < tolerance
 }
 
@@ -247,9 +238,15 @@ pub fn sample_medial_axis_nodes(
     nodes
 }
 
-/// Generate nodes directly from distance field without explicit Voronoi computation.
+/// Generate nodes on a uniform grid with clearance checks.
 ///
-/// This is a simpler alternative that samples the medial axis directly.
+/// This replaces medial axis generation with simple grid sampling:
+/// 1. Sample points on a uniform grid (coverage_node_interval spacing)
+/// 2. Keep only points with sufficient clearance from walls
+/// 3. Apply deterministic jitter for better distribution
+///
+/// The graph's optimize_nodes() should be called after to reduce density
+/// by merging visibility cliques into centroids.
 ///
 /// # Arguments
 /// * `lines` - Wall segments
@@ -257,8 +254,8 @@ pub fn sample_medial_axis_nodes(
 /// * `config` - CBVG configuration
 ///
 /// # Returns
-/// Vector of CBVG nodes on the medial axis.
-pub fn generate_medial_axis_nodes(
+/// Vector of CBVG nodes uniformly distributed in free space.
+pub fn generate_grid_nodes(
     lines: &[Line2D],
     bounds: Option<&Bounds>,
     config: &CBVGConfig,
@@ -272,14 +269,15 @@ pub fn generate_medial_axis_nodes(
         None => compute_bounds_from_lines(lines),
     };
 
-    // Expand bounds
+    // Expand bounds slightly to ensure edge coverage
     let margin = config.min_clearance;
     let expanded_bounds = Bounds {
         min: Point2D::new(bounds.min.x - margin, bounds.min.y - margin),
         max: Point2D::new(bounds.max.x + margin, bounds.max.y + margin),
     };
 
-    let step = config.voronoi_sample_step;
+    let step = config.coverage_node_interval;
+    let jitter_range = step * 0.3; // ±30% jitter for better distribution
     let mut nodes: Vec<CBVGNode> = Vec::new();
 
     let x_steps = ((expanded_bounds.max.x - expanded_bounds.min.x) / step).ceil() as usize;
@@ -287,86 +285,39 @@ pub fn generate_medial_axis_nodes(
 
     for ix in 0..=x_steps {
         for iy in 0..=y_steps {
-            let x = expanded_bounds.min.x + ix as f32 * step;
-            let y = expanded_bounds.min.y + iy as f32 * step;
-            let point = Point2D::new(x, y);
+            // Base grid position with deterministic jitter
+            let base_x = expanded_bounds.min.x + ix as f32 * step;
+            let base_y = expanded_bounds.min.y + iy as f32 * step;
+            let (jitter_x, jitter_y) = deterministic_jitter(ix, iy, jitter_range);
+            let point = Point2D::new(base_x + jitter_x, base_y + jitter_y);
 
-            // Check if on medial axis with sufficient clearance
-            if is_near_medial_axis(point, lines, config) {
-                let clearance = min_distance_to_walls(point, lines);
+            // Check clearance from walls
+            let clearance = min_distance_to_walls(point, lines);
+            if clearance < config.min_clearance_narrow {
+                continue;
+            }
 
-                if clearance >= config.min_clearance_narrow {
-                    let is_narrow = clearance < config.min_clearance;
+            // Skip duplicates
+            let is_duplicate = nodes
+                .iter()
+                .any(|n| n.position.distance(point) < config.node_merge_distance);
 
-                    // Check for duplicates
-                    let is_duplicate = nodes
-                        .iter()
-                        .any(|n| n.position.distance(point) < config.node_merge_distance);
+            if is_duplicate {
+                continue;
+            }
 
-                    if !is_duplicate {
-                        nodes.push(CBVGNode::medial_axis(point, clearance, is_narrow));
+            let is_narrow = clearance < config.min_clearance;
+            nodes.push(CBVGNode::medial_axis(point, clearance, is_narrow));
 
-                        if nodes.len() >= config.max_nodes {
-                            return nodes;
-                        }
-                    }
-                }
+            if nodes.len() >= config.max_nodes {
+                return nodes;
             }
         }
     }
 
-    // Add junction nodes at corridor intersections
-    add_junction_nodes(&mut nodes, lines, config);
-
     nodes
 }
 
-/// Add junction nodes where multiple corridors meet.
-fn add_junction_nodes(nodes: &mut [CBVGNode], lines: &[Line2D], config: &CBVGConfig) {
-    // Find points where 3+ walls are approximately equidistant
-    // These are corridor junctions
-
-    if lines.len() < 3 {
-        return;
-    }
-
-    // For now, detect junctions as points where medial axis changes direction significantly
-    // This is a simplified approach; a full implementation would use Voronoi vertices
-
-    let mut junction_candidates: Vec<Point2D> = Vec::new();
-
-    for node in nodes.iter() {
-        // Count nearby walls at similar distances
-        let clearance = node.clearance;
-        let tolerance = config.voronoi_sample_step;
-
-        let nearby_walls: usize = lines
-            .iter()
-            .filter(|line| {
-                let dist = line.distance_to_point_segment(node.position);
-                (dist - clearance).abs() < tolerance
-            })
-            .count();
-
-        // Junction if equidistant from 3+ walls
-        if nearby_walls >= 3 {
-            junction_candidates.push(node.position);
-        }
-    }
-
-    // Mark existing nodes as junctions or add new junction nodes
-    for candidate in junction_candidates {
-        // Find if there's an existing node nearby
-        let existing = nodes
-            .iter_mut()
-            .find(|n| n.position.distance(candidate) < config.node_merge_distance);
-
-        if let Some(node) = existing {
-            node.node_type = NodeType::Junction;
-        }
-    }
-}
-
 /// Compute bounds from line segments.
 fn compute_bounds_from_lines(lines: &[Line2D]) -> Bounds {
     if lines.is_empty() {
@@ -384,98 +335,6 @@ fn compute_bounds_from_lines(lines: &[Line2D]) -> Bounds {
     Bounds { min, max }
 }
 
-/// Generate nodes from lidar visibility region.
-///
-/// Only creates nodes in areas the lidar has actually observed, with proper
-/// wall clearance. Uses a grid-based approach with the coverage_node_interval
-/// to ensure uniform coverage in open areas.
-///
-/// # Arguments
-/// * `visibility` - Visibility region tracking scanned areas
-/// * `lines` - Wall segments for clearance computation
-/// * `config` - CBVG configuration
-///
-/// # Returns
-/// Vector of CBVG nodes in visible areas with safe clearance.
-pub fn generate_nodes_from_visibility(
-    visibility: &VisibilityRegion,
-    lines: &[Line2D],
-    config: &CBVGConfig,
-) -> Vec<CBVGNode> {
-    if visibility.is_empty() || lines.is_empty() {
-        return Vec::new();
-    }
-
-    let bounds = visibility.visible_bounds();
-    if bounds.is_empty() {
-        return Vec::new();
-    }
-
-    let step = config.coverage_node_interval;
-    let mut nodes: Vec<CBVGNode> = Vec::new();
-
-    // Jitter parameters: nodes are offset by ±30% of step size
-    // This prevents systematic gaps near walls due to grid alignment
-    let jitter_range = step * 0.3;
-
-    // Grid sample within visible bounds
-    let x_start = bounds.min.x;
-    let x_end = bounds.max.x;
-    let y_start = bounds.min.y;
-    let y_end = bounds.max.y;
-
-    let x_steps = ((x_end - x_start) / step).ceil() as usize;
-    let y_steps = ((y_end - y_start) / step).ceil() as usize;
-
-    for ix in 0..=x_steps {
-        for iy in 0..=y_steps {
-            // Base grid position
-            let base_x = x_start + ix as f32 * step;
-            let base_y = y_start + iy as f32 * step;
-
-            // Apply deterministic jitter based on grid cell
-            // Uses a simple hash to create pseudo-random but reproducible offsets
-            let (jitter_x, jitter_y) = deterministic_jitter(ix, iy, jitter_range);
-            let x = base_x + jitter_x;
-            let y = base_y + jitter_y;
-            let point = Point2D::new(x, y);
-
-            // Must be in visible area (scanned by lidar)
-            if !visibility.is_visible(point) {
-                continue;
-            }
-
-            // Must have clearance from walls
-            let clearance = min_distance_to_walls(point, lines);
-            if clearance < config.min_clearance_narrow {
-                continue;
-            }
-
-            // Check for duplicates
-            let is_duplicate = nodes
-                .iter()
-                .any(|n| n.position.distance(point) < config.node_merge_distance);
-
-            if is_duplicate {
-                continue;
-            }
-
-            // Determine node type based on clearance
-            let is_narrow = clearance < config.min_clearance;
-            nodes.push(CBVGNode::medial_axis(point, clearance, is_narrow));
-
-            if nodes.len() >= config.max_nodes {
-                return nodes;
-            }
-        }
-    }
-
-    // Add junction nodes at corridor intersections
-    add_junction_nodes(&mut nodes, lines, config);
-
-    nodes
-}
-
 /// Generate deterministic jitter for a grid cell.
 ///
 /// Uses a simple hash function to create pseudo-random but reproducible
@@ -489,7 +348,7 @@ pub fn generate_nodes_from_visibility(
 ///
 /// # Returns
 /// Tuple of (x_offset, y_offset) in range [-range, +range]
-fn deterministic_jitter(ix: usize, iy: usize, range: f32) -> (f32, f32) {
+pub fn deterministic_jitter(ix: usize, iy: usize, range: f32) -> (f32, f32) {
     // Simple hash combining grid indices
     // Using prime multipliers for better distribution
     let hash1 = ((ix.wrapping_mul(73856093)) ^ (iy.wrapping_mul(19349663))) as u32;
@@ -502,82 +361,6 @@ fn deterministic_jitter(ix: usize, iy: usize, range: f32) -> (f32, f32) {
     (norm1 * range, norm2 * range)
 }
 
-/// Generate nodes combining visibility-aware and medial axis approaches.
-///
-/// This hybrid approach:
-/// 1. Places coverage nodes uniformly in visible areas
-/// 2. Adds medial axis nodes where they don't already exist
-/// 3. Ensures good connectivity in narrow passages
-///
-/// # Arguments
-/// * `visibility` - Visibility region tracking scanned areas
-/// * `lines` - Wall segments
-/// * `config` - CBVG configuration
-///
-/// # Returns
-/// Vector of CBVG nodes optimized for path planning.
-pub fn generate_nodes_hybrid(
-    visibility: &VisibilityRegion,
-    lines: &[Line2D],
-    config: &CBVGConfig,
-) -> Vec<CBVGNode> {
-    // Start with visibility-based coverage nodes
-    let mut nodes = generate_nodes_from_visibility(visibility, lines, config);
-
-    if nodes.len() >= config.max_nodes {
-        return nodes;
-    }
-
-    // Add medial axis nodes for better coverage in corridors
-    // Use smaller step for more precision in narrow areas
-    let corridor_config = CBVGConfig {
-        voronoi_sample_step: config.voronoi_sample_step,
-        ..config.clone()
-    };
-
-    let bounds = visibility.visible_bounds();
-    for ix in 0..((bounds.width() / corridor_config.voronoi_sample_step).ceil() as usize) {
-        for iy in 0..((bounds.height() / corridor_config.voronoi_sample_step).ceil() as usize) {
-            let x = bounds.min.x + ix as f32 * corridor_config.voronoi_sample_step;
-            let y = bounds.min.y + iy as f32 * corridor_config.voronoi_sample_step;
-            let point = Point2D::new(x, y);
-
-            // Only add in visible areas
-            if !visibility.is_visible(point) {
-                continue;
-            }
-
-            // Only add if on medial axis
-            if !is_near_medial_axis(point, lines, &corridor_config) {
-                continue;
-            }
-
-            let clearance = min_distance_to_walls(point, lines);
-            if clearance < config.min_clearance_narrow {
-                continue;
-            }
-
-            // Skip if already covered
-            let is_duplicate = nodes
-                .iter()
-                .any(|n| n.position.distance(point) < config.node_merge_distance);
-
-            if is_duplicate {
-                continue;
-            }
-
-            let is_narrow = clearance < config.min_clearance;
-            nodes.push(CBVGNode::medial_axis(point, clearance, is_narrow));
-
-            if nodes.len() >= config.max_nodes {
-                return nodes;
-            }
-        }
-    }
-
-    nodes
-}
-
 #[cfg(test)]
 mod tests {
     use super::*;
@@ -638,28 +421,14 @@ mod tests {
     }
 
     #[test]
-    fn test_is_near_medial_axis() {
-        let walls = make_corridor();
-        let config = CBVGConfig::default().with_voronoi_sample_step(0.1);
-
-        // Point in the middle of corridor should be on medial axis
-        let center = Point2D::new(2.5, 0.5);
-        assert!(is_near_medial_axis(center, &walls, &config));
-
-        // Point close to a wall should not be on medial axis
-        let near_wall = Point2D::new(2.5, 0.1);
-        assert!(!is_near_medial_axis(near_wall, &walls, &config));
-    }
-
-    #[test]
-    fn test_generate_medial_axis_nodes() {
+    fn test_generate_grid_nodes() {
         let walls = make_room();
         let config = CBVGConfig::default()
-            .with_voronoi_sample_step(0.2)
+            .with_coverage_node_interval(0.3)
             .with_min_clearance(0.3)
             .with_min_clearance_narrow(0.18);
 
-        let nodes = generate_medial_axis_nodes(&walls, None, &config);
+        let nodes = generate_grid_nodes(&walls, None, &config);
 
         // Should have nodes in the room
         assert!(!nodes.is_empty());
@@ -669,7 +438,7 @@ mod tests {
             assert!(node.clearance >= config.min_clearance_narrow);
         }
 
-        // Most nodes should be near the center of the room
+        // Should have nodes near the center of the room
         let center = Point2D::new(2.0, 1.5);
         let near_center: usize = nodes
             .iter()
@@ -692,90 +461,7 @@ mod tests {
     #[test]
     fn test_empty_walls() {
         let config = CBVGConfig::default();
-        let nodes = generate_medial_axis_nodes(&[], None, &config);
+        let nodes = generate_grid_nodes(&[], None, &config);
         assert!(nodes.is_empty());
     }
-
-    #[test]
-    fn test_generate_nodes_from_visibility() {
-        use crate::core::Pose2D;
-
-        let walls = make_room();
-        let config = CBVGConfig::default()
-            .with_coverage_node_interval(0.5)
-            .with_min_clearance(0.3)
-            .with_min_clearance_narrow(0.18);
-
-        // Create visibility region with scan from center of room
-        let mut visibility = VisibilityRegion::new(0.1);
-        let robot_pose = Pose2D::new(2.0, 1.5, 0.0);
-
-        // Simulate a circular scan around the robot
-        let scan_points: Vec<Point2D> = (0..36)
-            .map(|i| {
-                let angle = i as f32 * std::f32::consts::PI / 18.0;
-                let dist = 1.5; // 1.5m range
-                robot_pose.transform_point(Point2D::from_polar(angle, dist))
-            })
-            .collect();
-
-        visibility.update_from_scan(robot_pose, &scan_points);
-
-        let nodes = generate_nodes_from_visibility(&visibility, &walls, &config);
-
-        // Should have nodes
-        assert!(!nodes.is_empty());
-
-        // All nodes should be in visible area
-        for node in &nodes {
-            assert!(visibility.is_visible(node.position));
-        }
-
-        // All nodes should have sufficient clearance
-        for node in &nodes {
-            assert!(node.clearance >= config.min_clearance_narrow);
-        }
-    }
-
-    #[test]
-    fn test_visibility_nodes_only_in_scanned_area() {
-        use crate::core::Pose2D;
-
-        // Create a large room
-        let walls = vec![
-            Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0)),
-            Line2D::new(Point2D::new(10.0, 0.0), Point2D::new(10.0, 10.0)),
-            Line2D::new(Point2D::new(10.0, 10.0), Point2D::new(0.0, 10.0)),
-            Line2D::new(Point2D::new(0.0, 10.0), Point2D::new(0.0, 0.0)),
-        ];
-
-        let config = CBVGConfig::default()
-            .with_coverage_node_interval(0.5)
-            .with_min_clearance(0.3);
-
-        // Scan only the bottom-left corner
-        let mut visibility = VisibilityRegion::new(0.1);
-        let robot_pose = Pose2D::new(2.0, 2.0, 0.0);
-
-        let scan_points: Vec<Point2D> = (0..36)
-            .map(|i| {
-                let angle = i as f32 * std::f32::consts::PI / 18.0;
-                let dist = 1.5;
-                robot_pose.transform_point(Point2D::from_polar(angle, dist))
-            })
-            .collect();
-
-        visibility.update_from_scan(robot_pose, &scan_points);
-
-        let nodes = generate_nodes_from_visibility(&visibility, &walls, &config);
-
-        // All nodes should be near the robot (not in unexplored far corner)
-        for node in &nodes {
-            assert!(
-                node.position.distance(robot_pose.position()) < 3.0,
-                "Node at {:?} is too far from scanned area",
-                node.position
-            );
-        }
-    }
 }

vastu-map/src/query/frontier.rs

@@ -467,6 +467,19 @@ pub struct FrontierConfig {
     /// Larger distances are treated as separate areas, not a gap.
     /// Default: 3.0m
     pub max_gap_width: f32,
+
+    // ─────────────────────────────────────────────────────────────────────────
+    // CBVG-based frontier detection parameters
+    // ─────────────────────────────────────────────────────────────────────────
+    /// Minimum uncovered angle for a CBVG node to be a frontier (radians).
+    /// Nodes with a gap of at least this size (no neighbors or walls) are frontiers.
+    /// Default: 1.75 rad (100 degrees)
+    pub min_frontier_angle: f32,
+
+    /// Maximum distance to check for walls when detecting frontiers (meters).
+    /// Walls closer than this block the direction from being a frontier.
+    /// Default: 0.5m
+    pub max_wall_distance: f32,
 }
 
 impl Default for FrontierConfig {
@@ -481,6 +494,9 @@ impl Default for FrontierConfig {
             viewpoint_step: 0.1,
             min_gap_width: 0.4,  // robot diameter (0.3m) + margin
             max_gap_width: 10.0, // allow gaps across rooms
+            // CBVG-based parameters
+            min_frontier_angle: 1.75, // 100 degrees in radians
+            max_wall_distance: 0.5,
         }
     }
 }

vastu-map/src/vector_map/map.rs

@@ -14,7 +14,7 @@ use crate::integration::{
 use crate::loop_closure::{LoopClosure, LoopClosureDetector};
 use crate::matching::PointToLineIcp;
 use crate::query::cbvg::{ClearanceVisibilityGraph, VisibilityRegion};
-use crate::query::{detect_frontiers, is_straight_path_clear, query_occupancy, raycast};
+use crate::query::{is_straight_path_clear, query_occupancy, raycast};
 use crate::{Frontier, Map, ObserveResult, Occupancy, Path, TimingBreakdown};
 
 use super::config::VectorMapConfig;
@@ -269,17 +269,10 @@ impl VectorMap {
 
     /// Build and return a CBVG for visualization.
     ///
-    /// Uses visibility-based node generation if visibility data is available.
+    /// Returns a clone of the internal CBVG which is incrementally updated
+    /// with each lidar scan via `add_nodes_from_scan`.
     pub fn build_cbvg(&self) -> ClearanceVisibilityGraph {
-        let mut cbvg = ClearanceVisibilityGraph::new(self.config.cbvg.clone());
-
-        if !self.visibility.is_empty() {
-            cbvg.build_from_visibility(&self.visibility, self.line_store.lines());
-        } else {
-            cbvg.build(self.line_store.lines(), self.line_store.bounds());
-        }
-
-        cbvg
+        self.cbvg.clone()
     }
 
     /// Find the nearest CBVG node to a target point.
@@ -485,7 +478,15 @@ impl VectorMap {
             .collect();
         self.visibility
             .update_from_scan(final_pose, &world_scan_points);
-        self.cbvg_dirty = true;
+        // Incrementally add nodes from this scan
+        self.cbvg
+            .add_nodes_from_scan(final_pose, &world_scan_points, self.line_store.lines());
+        // Clean up any existing nodes that are now too close to scan points
+        self.cbvg
+            .cleanup_nodes_near_scan_points(&world_scan_points, self.line_store.lines());
+        // Clean up any edges that pass too close to scan points
+        self.cbvg.cleanup_edges_near_scan_points(&world_scan_points);
+        self.cbvg_dirty = false;
 
         // ─── Scan Storage (Exploration Mode) ───
         if let Some(ref mut store) = self.scan_store {
@@ -579,7 +580,15 @@ impl VectorMap {
             .collect();
         self.visibility
             .update_from_scan(odometry, &world_scan_points);
-        self.cbvg_dirty = true;
+        // Incrementally add nodes from this scan
+        self.cbvg
+            .add_nodes_from_scan(odometry, &world_scan_points, self.line_store.lines());
+        // Clean up any existing nodes that are now too close to scan points
+        self.cbvg
+            .cleanup_nodes_near_scan_points(&world_scan_points, self.line_store.lines());
+        // Clean up any edges that pass too close to scan points
+        self.cbvg.cleanup_edges_near_scan_points(&world_scan_points);
+        self.cbvg_dirty = false;
 
         // ─── Scan Storage (Exploration Mode) ───
         if let Some(ref mut store) = self.scan_store {
@@ -896,32 +905,35 @@ impl Map for VectorMap {
     }
 
     fn frontiers(&self) -> Vec<Frontier> {
-        // Use current robot pose for distance-based filtering
-        detect_frontiers(
+        // Use CBVG-based frontier detection
+        // A node is a frontier if it has a large uncovered angular gap
+        // (no neighbors or walls in that direction)
+        let (frontiers, _indices) = self.cbvg.detect_frontiers(
             self.line_store.lines(),
-            self.current_pose.position(),
-            &self.config.frontier,
-        )
+            self.config.frontier.min_frontier_angle,
+            self.config.frontier.max_wall_distance,
+        );
+        frontiers
     }
 
     fn get_path(&self, from: Point2D, to: Point2D) -> Option<Path> {
-        // Use CBVG for path planning with visibility-aware node generation
-        // This ensures nodes are only placed in lidar-scanned areas
-        let mut cbvg = ClearanceVisibilityGraph::new(self.config.cbvg.clone());
+        // Use the internal CBVG which is incrementally updated with each scan
+        self.cbvg.find_path(from, to, self.line_store.lines())
+    }
 
-        // Use visibility-based build if we have visibility data
-        if !self.visibility.is_empty() {
-            cbvg.build_from_visibility(&self.visibility, self.line_store.lines());
-        } else {
-            // Fallback to wall-based build for legacy/testing
-            cbvg.build(self.line_store.lines(), self.line_store.bounds());
-        }
-
-        cbvg.find_path(from, to, self.line_store.lines())
+    fn get_path_excluding_nodes(
+        &self,
+        from: Point2D,
+        to: Point2D,
+        excluded_goal_nodes: &[usize],
+    ) -> Option<(Path, usize)> {
+        // Use CBVG with node exclusions for alternative approach angles
+        self.cbvg
+            .find_path_excluding_nodes(from, to, self.line_store.lines(), excluded_goal_nodes)
     }
 
     fn nearest_cbvg_node(&self, target: Point2D, max_distance: f32) -> Option<Point2D> {
-        self.nearest_cbvg_node(target, max_distance)
+        self.cbvg.nearest_node(target, max_distance)
     }
 
     fn is_path_clear(&self, from: Point2D, to: Point2D) -> bool {
@@ -1086,18 +1098,57 @@ mod tests {
         assert!(frontiers.is_empty());
     }
 
+    /// Create a point cloud for a partial room (3 walls, top missing).
+    /// Simulates a lidar scan from origin seeing walls at x=-2, x=2, y=-2.
+    fn make_partial_room_scan() -> PointCloud2D {
+        let mut xs = Vec::new();
+        let mut ys = Vec::new();
+
+        // Left wall (x = -2)
+        for i in -10..=10 {
+            let y = i as f32 * 0.2;
+            xs.push(-2.0);
+            ys.push(y);
+        }
+        // Right wall (x = 2)
+        for i in -10..=10 {
+            let y = i as f32 * 0.2;
+            xs.push(2.0);
+            ys.push(y);
+        }
+        // Bottom wall (y = -2)
+        for i in -10..=10 {
+            let x = i as f32 * 0.2;
+            xs.push(x);
+            ys.push(-2.0);
+        }
+        // Top wall is MISSING - this is where frontier should be detected
+
+        PointCloud2D { xs, ys }
+    }
+
     #[test]
     fn test_frontiers_partial_room() {
         let mut map = VectorMap::default();
 
-        map.add_lines(&[
-            Line2D::new(Point2D::new(-2.0, -2.0), Point2D::new(2.0, -2.0)),
-            Line2D::new(Point2D::new(2.0, -2.0), Point2D::new(2.0, 2.0)),
-            Line2D::new(Point2D::new(-2.0, 2.0), Point2D::new(-2.0, -2.0)),
-        ]);
+        // Use observe() to properly build lines AND CBVG nodes
+        let scan = make_partial_room_scan();
+        map.observe(&scan, Pose2D::identity());
 
+        // With 3 walls visible and top open, we should have frontiers
         let frontiers = map.frontiers();
-        assert!(!frontiers.is_empty());
+
+        // CBVG nodes should exist after observation
+        assert!(
+            !map.cbvg.nodes().is_empty(),
+            "CBVG should have nodes after observation"
+        );
+
+        // Frontiers should be detected in the open direction (toward +y)
+        assert!(
+            !frontiers.is_empty(),
+            "Partial room with open top should have frontiers"
+        );
     }
 
     #[test]

vastu-map/src/vector_map/yaml.rs

@@ -257,6 +257,8 @@ frontier:
   viewpoint_step: 0.1
   min_gap_width: 0.4
   max_gap_width: 10.0
+  min_frontier_angle: 1.75
+  max_wall_distance: 0.5
 occupancy:
   obstacle_distance: 0.1
   known_distance: 2.0
@@ -376,6 +378,8 @@ frontier:
   viewpoint_step: 0.1
   min_gap_width: 0.4
   max_gap_width: 10.0
+  min_frontier_angle: 1.75
+  max_wall_distance: 0.5
 occupancy:
   obstacle_distance: 0.1
   known_distance: 2.0