Add region-based exploration with gap frontier detection
Refactor exploration to use hierarchical region-based planning instead of greedy closest-frontier selection. This eliminates ping-pong behavior and improves coverage efficiency. Key changes: - Gap-based frontier detection: frontiers are now viewpoints to observe unexplored gaps between unconnected wall endpoints, not the endpoints themselves. Each frontier has a safe viewpoint, look direction, and estimated unexplored area. - Region clustering: frontiers grouped using union-find clustering, enabling systematic area-by-area exploration - TSP-ordered planning: regions ordered using greedy TSP to minimize travel distance while maintaining coverage - Exploration history: spatial grid tracks visited areas to penalize backtracking and improve region prioritization - Unreachable frontier recovery: clears unreachable list when robot moves 1.5m, allowing retry from new positions New modules: history.rs, planner.rs, region.rs Updated: Frontier struct (viewpoint/look_direction/endpoint/area)
This commit is contained in:
codetiger
@@ -11,9 +11,9 @@ extraction:
|
||||
# Minimum points to form a valid line
|
||||
min_points: 5
|
||||
# Minimum line length to keep (m)
|
||||
min_length: 0.10
|
||||
min_length: 0.05
|
||||
# Maximum gap between consecutive points (m)
|
||||
max_point_gap: 0.30
|
||||
max_point_gap: 0.10
|
||||
# Merge threshold for collinear segments (m)
|
||||
merge_threshold: 0.05
|
||||
# Coefficient for range-adaptive threshold scaling
|
||||
@@ -135,15 +135,29 @@ merger:
|
||||
min_extension: 0.05
|
||||
|
||||
# ============================================================================
|
||||
# Frontier Detection
|
||||
# Frontier Detection (Gap-Based)
|
||||
# ============================================================================
|
||||
frontier:
|
||||
# Maximum endpoint distance for "connected" (m)
|
||||
connection_distance: 0.3
|
||||
# Minimum distance from robot for valid frontier (m)
|
||||
min_distance_from_robot: 0.5
|
||||
# Minimum openness score for valid frontier
|
||||
min_openness: 0.3
|
||||
min_distance_from_robot: 1.0
|
||||
# Safety margin: viewpoint offset from wall (m)
|
||||
safety_margin: 0.4
|
||||
# Gap probe distance: how far to probe for unexplored gaps (m)
|
||||
gap_probe_distance: 2.0
|
||||
# Robot radius for viewpoint validation (m)
|
||||
robot_radius: 0.16
|
||||
# Maximum iterations for viewpoint adjustment
|
||||
max_viewpoint_iterations: 5
|
||||
# Viewpoint adjustment step size (m)
|
||||
viewpoint_step: 0.1
|
||||
# Minimum gap width for safe robot passage (m)
|
||||
# Gaps narrower than this are ignored (robot can't fit)
|
||||
min_gap_width: 0.4
|
||||
# Maximum gap width to consider as a single gap (m)
|
||||
# Allow gaps across rooms (up to 10m)
|
||||
max_gap_width: 10.0
|
||||
|
||||
# ============================================================================
|
||||
# Occupancy Queries
|
||||
@@ -153,15 +167,13 @@ occupancy:
|
||||
obstacle_distance: 0.1
|
||||
# Distance threshold for "known" (m)
|
||||
known_distance: 2.0
|
||||
# Use winding number for inside/outside determination
|
||||
use_winding_number: false
|
||||
|
||||
# ============================================================================
|
||||
# Path Planning
|
||||
# ============================================================================
|
||||
path_planning:
|
||||
# Robot radius for collision checking (m)
|
||||
robot_radius: 0.15
|
||||
robot_radius: 0.16
|
||||
# Maximum visibility graph nodes
|
||||
max_nodes: 1000
|
||||
# Only plan through known free space
|
||||
|
||||
@@ -3,6 +3,8 @@
|
||||
use crate::query::frontier::FrontierConfig;
|
||||
use crate::query::path_planning::PathPlanningConfig;
|
||||
|
||||
use super::planner::PlannerConfig;
|
||||
|
||||
/// Configuration for exploration behavior.
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct ExplorationConfig {
|
||||
@@ -35,10 +37,22 @@ pub struct ExplorationConfig {
|
||||
pub lookahead_distance: f32,
|
||||
|
||||
/// Frontier detection configuration.
|
||||
/// Note: With shadow-based frontiers, viewpoints are already safe observation points.
|
||||
pub frontier_config: FrontierConfig,
|
||||
|
||||
/// Path planning configuration.
|
||||
pub path_planning_config: PathPlanningConfig,
|
||||
|
||||
/// Region-based exploration planner configuration.
|
||||
pub planner_config: PlannerConfig,
|
||||
|
||||
/// Cell size for visit history tracking (meters).
|
||||
/// Default: 0.5
|
||||
pub history_cell_size: f32,
|
||||
|
||||
/// Maximum number of recent positions to track in history.
|
||||
/// Default: 100
|
||||
pub history_max_recent: usize,
|
||||
}
|
||||
|
||||
impl Default for ExplorationConfig {
|
||||
@@ -53,6 +67,9 @@ impl Default for ExplorationConfig {
|
||||
lookahead_distance: 0.3,
|
||||
frontier_config: FrontierConfig::default(),
|
||||
path_planning_config: PathPlanningConfig::default().with_robot_radius(0.15),
|
||||
planner_config: PlannerConfig::default(),
|
||||
history_cell_size: 0.5,
|
||||
history_max_recent: 100,
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -117,6 +134,24 @@ impl ExplorationConfig {
|
||||
self.path_planning_config = config;
|
||||
self
|
||||
}
|
||||
|
||||
/// Builder-style setter for planner config.
|
||||
pub fn with_planner_config(mut self, config: PlannerConfig) -> Self {
|
||||
self.planner_config = config;
|
||||
self
|
||||
}
|
||||
|
||||
/// Builder-style setter for history cell size.
|
||||
pub fn with_history_cell_size(mut self, size: f32) -> Self {
|
||||
self.history_cell_size = size;
|
||||
self
|
||||
}
|
||||
|
||||
/// Builder-style setter for history max recent.
|
||||
pub fn with_history_max_recent(mut self, max: usize) -> Self {
|
||||
self.history_max_recent = max;
|
||||
self
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
|
||||
@@ -3,13 +3,24 @@
|
||||
//! Manages frontier-based exploration with collision recovery.
|
||||
//! The primary goal is complete map coverage, with obstacle avoidance
|
||||
//! as the secondary priority.
|
||||
//!
|
||||
//! # Region-Based Exploration
|
||||
//!
|
||||
//! This controller uses a hierarchical exploration strategy:
|
||||
//! 1. Cluster frontiers into exploration regions
|
||||
//! 2. Order regions using greedy TSP to minimize travel
|
||||
//! 3. Select frontiers within each region using heading-aligned selection
|
||||
//!
|
||||
//! This approach eliminates ping-pong behavior and improves coverage efficiency.
|
||||
|
||||
use crate::core::{Point2D, Pose2D};
|
||||
use crate::{Frontier, Map, Path};
|
||||
|
||||
use super::collision::{CollisionEvent, VirtualWall};
|
||||
use super::config::ExplorationConfig;
|
||||
use super::history::ExplorationHistory;
|
||||
use super::path_follower::{PathFollower, VelocityCommand};
|
||||
use super::planner::ExplorationPlanner;
|
||||
|
||||
/// Current exploration state.
|
||||
#[derive(Clone, Debug)]
|
||||
@@ -179,12 +190,27 @@ pub struct ExplorationController {
|
||||
|
||||
/// Distance threshold to consider frontiers as same location.
|
||||
frontier_match_distance: f32,
|
||||
|
||||
/// Region-based exploration planner.
|
||||
planner: ExplorationPlanner,
|
||||
|
||||
/// Exploration history for visit tracking.
|
||||
history: ExplorationHistory,
|
||||
|
||||
/// Time accumulator for dt calculation (seconds).
|
||||
last_update_time: Option<std::time::Instant>,
|
||||
|
||||
/// 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>,
|
||||
}
|
||||
|
||||
impl ExplorationController {
|
||||
/// Create a new exploration controller.
|
||||
pub fn new(config: ExplorationConfig) -> Self {
|
||||
let path_follower = PathFollower::new(&config);
|
||||
let planner = ExplorationPlanner::new(config.planner_config.clone());
|
||||
let history = ExplorationHistory::new(config.history_cell_size, config.history_max_recent);
|
||||
|
||||
Self {
|
||||
state: ExplorationState::Idle,
|
||||
@@ -194,6 +220,10 @@ impl ExplorationController {
|
||||
max_failed_attempts: 3,
|
||||
unreachable_frontiers: Vec::new(),
|
||||
frontier_match_distance: 0.5,
|
||||
planner,
|
||||
history,
|
||||
last_update_time: None,
|
||||
position_at_last_unreachable_clear: None,
|
||||
}
|
||||
}
|
||||
|
||||
@@ -202,6 +232,10 @@ impl ExplorationController {
|
||||
self.state = ExplorationState::SelectingFrontier;
|
||||
self.failed_planning_attempts = 0;
|
||||
self.unreachable_frontiers.clear();
|
||||
self.planner.reset();
|
||||
self.history.clear();
|
||||
self.last_update_time = Some(std::time::Instant::now());
|
||||
self.position_at_last_unreachable_clear = None;
|
||||
}
|
||||
|
||||
/// Reset to idle state.
|
||||
@@ -209,6 +243,10 @@ impl ExplorationController {
|
||||
self.state = ExplorationState::Idle;
|
||||
self.failed_planning_attempts = 0;
|
||||
self.unreachable_frontiers.clear();
|
||||
self.planner.reset();
|
||||
self.history.clear();
|
||||
self.last_update_time = None;
|
||||
self.position_at_last_unreachable_clear = None;
|
||||
}
|
||||
|
||||
/// Get current state.
|
||||
@@ -308,17 +346,57 @@ impl ExplorationController {
|
||||
step
|
||||
}
|
||||
|
||||
/// Select the next frontier to explore.
|
||||
/// Select the next frontier to explore using region-based strategy.
|
||||
///
|
||||
/// The algorithm:
|
||||
/// 1. Update exploration history with current position
|
||||
/// 2. Update the region-based planner with current frontiers
|
||||
/// 3. Select the best frontier from the current region
|
||||
/// 4. Plan a path to the selected frontier
|
||||
/// 5. If path planning fails, try next frontier or advance to next region
|
||||
fn select_frontier<M: Map>(&mut self, map: &M, current_pose: Pose2D) -> ExplorationStep {
|
||||
let robot_pos = current_pose.position();
|
||||
|
||||
// Calculate dt for planner update
|
||||
let dt = self
|
||||
.last_update_time
|
||||
.map(|t| t.elapsed().as_secs_f32())
|
||||
.unwrap_or(0.1);
|
||||
self.last_update_time = Some(std::time::Instant::now());
|
||||
|
||||
// Update history with current position
|
||||
self.history.mark_visited(robot_pos);
|
||||
|
||||
// Get all frontiers
|
||||
let all_frontiers = map.frontiers();
|
||||
|
||||
// Filter out unreachable frontiers
|
||||
let reachable_frontiers: Vec<&Frontier> = all_frontiers
|
||||
// Re-evaluate unreachable frontiers when robot has moved significantly.
|
||||
// This allows frontiers that were previously unreachable from position A
|
||||
// to be reconsidered when the robot has moved to position B.
|
||||
// Use 1.5m for more aggressive retry - robot tries new positions sooner.
|
||||
const UNREACHABLE_RESET_DISTANCE: f32 = 1.5; // meters
|
||||
if !self.unreachable_frontiers.is_empty() {
|
||||
let should_clear = match self.position_at_last_unreachable_clear {
|
||||
Some(last_pos) => robot_pos.distance(last_pos) > UNREACHABLE_RESET_DISTANCE,
|
||||
None => false, // First time through, don't clear
|
||||
};
|
||||
|
||||
if should_clear {
|
||||
log::debug!(
|
||||
"Clearing {} unreachable frontiers after robot moved {:.1}m",
|
||||
self.unreachable_frontiers.len(),
|
||||
UNREACHABLE_RESET_DISTANCE
|
||||
);
|
||||
self.unreachable_frontiers.clear();
|
||||
self.position_at_last_unreachable_clear = Some(robot_pos);
|
||||
}
|
||||
}
|
||||
|
||||
// Filter out unreachable frontiers (using viewpoint for distance checks)
|
||||
let reachable_frontiers: Vec<Frontier> = all_frontiers
|
||||
.iter()
|
||||
.filter(|f| !self.is_frontier_unreachable(f.point))
|
||||
.filter(|f| !self.is_frontier_unreachable(f.viewpoint))
|
||||
.cloned()
|
||||
.collect();
|
||||
|
||||
if reachable_frontiers.is_empty() {
|
||||
@@ -340,85 +418,144 @@ impl ExplorationController {
|
||||
}
|
||||
}
|
||||
|
||||
// Find closest reachable frontier
|
||||
let best_frontier = reachable_frontiers
|
||||
.iter()
|
||||
.min_by(|a, b| {
|
||||
let dist_a = robot_pos.distance(a.point);
|
||||
let dist_b = robot_pos.distance(b.point);
|
||||
dist_a
|
||||
.partial_cmp(&dist_b)
|
||||
.unwrap_or(std::cmp::Ordering::Equal)
|
||||
})
|
||||
.map(|f| f.point);
|
||||
// Update the region-based planner
|
||||
self.planner
|
||||
.update(&reachable_frontiers, robot_pos, &self.history, dt);
|
||||
|
||||
let target = match best_frontier {
|
||||
Some(p) => p,
|
||||
None => {
|
||||
self.state = ExplorationState::Complete;
|
||||
return ExplorationStep::idle(self.state.clone());
|
||||
}
|
||||
};
|
||||
// Try to find a frontier with a valid path
|
||||
loop {
|
||||
// Check if we've exhausted all regions
|
||||
if self.planner.plan().is_complete() {
|
||||
// Force replan to pick up any new frontiers
|
||||
self.planner
|
||||
.force_replan(&reachable_frontiers, robot_pos, &self.history);
|
||||
|
||||
log::debug!(
|
||||
"Selected frontier at ({:.2}, {:.2}), distance: {:.2}m",
|
||||
target.x,
|
||||
target.y,
|
||||
robot_pos.distance(target)
|
||||
);
|
||||
|
||||
// Plan path to frontier
|
||||
match map.get_path(robot_pos, target) {
|
||||
Some(path) => {
|
||||
log::debug!(
|
||||
"Path planned: {} waypoints, {:.2}m total",
|
||||
path.points.len(),
|
||||
path.length
|
||||
);
|
||||
|
||||
self.failed_planning_attempts = 0;
|
||||
|
||||
// Immediately start following the path
|
||||
let result = self.path_follower.follow(
|
||||
current_pose,
|
||||
&path,
|
||||
0,
|
||||
self.config.waypoint_tolerance,
|
||||
);
|
||||
|
||||
self.state = ExplorationState::FollowingPath {
|
||||
path: path.clone(),
|
||||
waypoint_idx: result.waypoint_idx,
|
||||
target_frontier: target,
|
||||
};
|
||||
|
||||
let mut step = ExplorationStep::with_velocity(result.velocity, self.state.clone());
|
||||
step.path_planned = true;
|
||||
step.target_frontier = Some(target);
|
||||
step.distance_to_target = result.distance_to_waypoint;
|
||||
step
|
||||
}
|
||||
None => {
|
||||
log::warn!(
|
||||
"Failed to plan path to frontier at ({:.2}, {:.2})",
|
||||
target.x,
|
||||
target.y
|
||||
);
|
||||
|
||||
self.failed_planning_attempts += 1;
|
||||
|
||||
if self.failed_planning_attempts >= self.max_failed_attempts {
|
||||
// Mark this frontier as unreachable
|
||||
self.unreachable_frontiers.push(target);
|
||||
self.failed_planning_attempts = 0;
|
||||
log::warn!(
|
||||
"Frontier marked as unreachable after {} failed attempts",
|
||||
self.max_failed_attempts
|
||||
);
|
||||
if self.planner.plan().is_complete() || self.planner.plan().is_empty() {
|
||||
// Still no regions - exploration complete
|
||||
log::info!("Exploration complete: all regions exhausted");
|
||||
self.state = ExplorationState::Complete;
|
||||
return ExplorationStep::idle(self.state.clone());
|
||||
}
|
||||
}
|
||||
|
||||
// Stay in SelectingFrontier to try another
|
||||
ExplorationStep::idle(self.state.clone())
|
||||
// Get current region
|
||||
let region = match self.planner.plan().current_region() {
|
||||
Some(r) => r,
|
||||
None => {
|
||||
self.state = ExplorationState::Complete;
|
||||
return ExplorationStep::idle(self.state.clone());
|
||||
}
|
||||
};
|
||||
|
||||
// Select best frontier in current region (heading-aligned)
|
||||
let selected_frontier =
|
||||
self.planner
|
||||
.select_frontier_in_region(region, robot_pos, current_pose.theta);
|
||||
|
||||
// With shadow-based frontiers, viewpoint is already a safe observation point
|
||||
let target = match selected_frontier {
|
||||
Some(f) => f.viewpoint, // Use viewpoint directly - it's already safe!
|
||||
None => {
|
||||
// Current region exhausted, advance to next
|
||||
log::debug!("Region {} exhausted, advancing to next", region.id);
|
||||
self.planner.plan_mut().advance_region();
|
||||
continue;
|
||||
}
|
||||
};
|
||||
|
||||
// Check if we're already close enough to this viewpoint
|
||||
let distance_to_target = robot_pos.distance(target);
|
||||
if distance_to_target <= self.config.waypoint_tolerance {
|
||||
// Already at this viewpoint - skip to next
|
||||
log::debug!(
|
||||
"Skipping frontier viewpoint at ({:.2}, {:.2}) - already there ({:.2}m <= {:.2}m)",
|
||||
target.x,
|
||||
target.y,
|
||||
distance_to_target,
|
||||
self.config.waypoint_tolerance
|
||||
);
|
||||
// Mark as visited so we don't keep selecting it
|
||||
self.history.mark_visited(target);
|
||||
self.planner.plan_mut().advance_region();
|
||||
continue;
|
||||
}
|
||||
|
||||
log::debug!(
|
||||
"Selected frontier viewpoint at ({:.2}, {:.2}) from region {}, distance: {:.2}m",
|
||||
target.x,
|
||||
target.y,
|
||||
region.id,
|
||||
distance_to_target
|
||||
);
|
||||
|
||||
// Plan path directly to viewpoint (no offset needed - viewpoint is already safe)
|
||||
match map.get_path(robot_pos, target) {
|
||||
Some(path) => {
|
||||
log::debug!(
|
||||
"Path planned: {} waypoints, {:.2}m total",
|
||||
path.points.len(),
|
||||
path.length
|
||||
);
|
||||
|
||||
self.failed_planning_attempts = 0;
|
||||
|
||||
// Immediately start following the path
|
||||
let result = self.path_follower.follow(
|
||||
current_pose,
|
||||
&path,
|
||||
0,
|
||||
self.config.waypoint_tolerance,
|
||||
);
|
||||
|
||||
self.state = ExplorationState::FollowingPath {
|
||||
path: path.clone(),
|
||||
waypoint_idx: result.waypoint_idx,
|
||||
target_frontier: target, // Viewpoint is already a safe target
|
||||
};
|
||||
|
||||
let mut step =
|
||||
ExplorationStep::with_velocity(result.velocity, self.state.clone());
|
||||
step.path_planned = true;
|
||||
step.target_frontier = Some(target);
|
||||
step.distance_to_target = result.distance_to_waypoint;
|
||||
return step;
|
||||
}
|
||||
None => {
|
||||
log::warn!(
|
||||
"Failed to plan path to frontier at ({:.2}, {:.2})",
|
||||
target.x,
|
||||
target.y
|
||||
);
|
||||
|
||||
self.failed_planning_attempts += 1;
|
||||
|
||||
if self.failed_planning_attempts >= self.max_failed_attempts {
|
||||
// Track position when we first mark any frontier as unreachable
|
||||
// (so we can re-evaluate after robot moves 3m)
|
||||
if self.unreachable_frontiers.is_empty() {
|
||||
self.position_at_last_unreachable_clear = Some(robot_pos);
|
||||
}
|
||||
|
||||
// Mark this frontier as unreachable
|
||||
self.unreachable_frontiers.push(target);
|
||||
self.failed_planning_attempts = 0;
|
||||
log::warn!(
|
||||
"Frontier marked as unreachable after {} failed attempts",
|
||||
self.max_failed_attempts
|
||||
);
|
||||
|
||||
// Remove from current region and try again
|
||||
if let Some(region) = self.planner.plan_mut().current_region_mut() {
|
||||
region
|
||||
.frontiers
|
||||
.retain(|f| f.viewpoint.distance(target) > 0.1);
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
// Try another frontier in same region
|
||||
continue;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -514,6 +651,7 @@ impl ExplorationController {
|
||||
/// Clear unreachable frontiers (useful after map updates).
|
||||
pub fn clear_unreachable_frontiers(&mut self) {
|
||||
self.unreachable_frontiers.clear();
|
||||
self.position_at_last_unreachable_clear = None;
|
||||
}
|
||||
|
||||
/// Get the current configuration.
|
||||
@@ -544,10 +682,14 @@ mod tests {
|
||||
}
|
||||
|
||||
fn with_frontier(mut self, x: f32, y: f32) -> Self {
|
||||
// Create a frontier with viewpoint at (x, y)
|
||||
// In real usage, the viewpoint is computed from line endpoints
|
||||
self.frontiers.push(Frontier {
|
||||
point: Point2D::new(x, y),
|
||||
viewpoint: Point2D::new(x, y),
|
||||
look_direction: Point2D::new(1.0, 0.0), // Default: looking right
|
||||
endpoint: Point2D::new(x - 0.4, y), // Endpoint is offset back
|
||||
line_idx: 0,
|
||||
is_start: true,
|
||||
estimated_area: 4.0,
|
||||
});
|
||||
self
|
||||
}
|
||||
|
||||
@@ -0,0 +1,264 @@
|
||||
//! Exploration history tracking.
|
||||
//!
|
||||
//! Tracks visited areas using a spatial grid to avoid revisiting
|
||||
//! recently explored regions.
|
||||
|
||||
use std::collections::{HashMap, VecDeque};
|
||||
|
||||
use crate::core::Point2D;
|
||||
|
||||
/// Tracks exploration history using a spatial grid.
|
||||
///
|
||||
/// Used to penalize revisiting recently explored areas during
|
||||
/// region selection and frontier scoring.
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct ExplorationHistory {
|
||||
/// Grid of visited cells mapping (cell_x, cell_y) -> visit count.
|
||||
visited_grid: HashMap<(i32, i32), u8>,
|
||||
|
||||
/// Cell size for the grid (meters).
|
||||
cell_size: f32,
|
||||
|
||||
/// Recently visited positions (ring buffer).
|
||||
recent_positions: VecDeque<Point2D>,
|
||||
|
||||
/// Maximum number of recent positions to track.
|
||||
max_recent: usize,
|
||||
}
|
||||
|
||||
impl ExplorationHistory {
|
||||
/// Create a new exploration history.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `cell_size` - Size of each grid cell in meters (default: 0.5)
|
||||
/// * `max_recent` - Maximum number of recent positions to track
|
||||
pub fn new(cell_size: f32, max_recent: usize) -> Self {
|
||||
Self {
|
||||
visited_grid: HashMap::with_capacity(1000),
|
||||
cell_size,
|
||||
recent_positions: VecDeque::with_capacity(max_recent),
|
||||
max_recent,
|
||||
}
|
||||
}
|
||||
|
||||
/// Create with default parameters.
|
||||
pub fn with_defaults() -> Self {
|
||||
Self::new(0.5, 100)
|
||||
}
|
||||
|
||||
/// Mark a position as visited.
|
||||
pub fn mark_visited(&mut self, position: Point2D) {
|
||||
// Update grid cell
|
||||
let cell = self.point_to_cell(position);
|
||||
let count = self.visited_grid.entry(cell).or_insert(0);
|
||||
*count = count.saturating_add(1);
|
||||
|
||||
// Update recent positions
|
||||
self.recent_positions.push_back(position);
|
||||
if self.recent_positions.len() > self.max_recent {
|
||||
self.recent_positions.pop_front();
|
||||
}
|
||||
}
|
||||
|
||||
/// Check if a position is in a recently visited area.
|
||||
///
|
||||
/// Returns true if the position is within the same grid cell
|
||||
/// as any recent position.
|
||||
pub fn is_recently_visited(&self, position: Point2D) -> bool {
|
||||
let cell = self.point_to_cell(position);
|
||||
self.visited_grid.get(&cell).copied().unwrap_or(0) > 0
|
||||
}
|
||||
|
||||
/// Check if an area around a position has been recently visited.
|
||||
///
|
||||
/// Checks a 3x3 neighborhood of cells around the position.
|
||||
pub fn is_area_recently_visited(&self, position: Point2D) -> bool {
|
||||
let (cx, cy) = self.point_to_cell(position);
|
||||
|
||||
for dx in -1..=1 {
|
||||
for dy in -1..=1 {
|
||||
let cell = (cx + dx, cy + dy);
|
||||
if self.visited_grid.get(&cell).copied().unwrap_or(0) > 0 {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
false
|
||||
}
|
||||
|
||||
/// Get the visit count for a position.
|
||||
pub fn get_visit_count(&self, position: Point2D) -> u8 {
|
||||
let cell = self.point_to_cell(position);
|
||||
self.visited_grid.get(&cell).copied().unwrap_or(0)
|
||||
}
|
||||
|
||||
/// Get the total number of cells visited.
|
||||
pub fn cells_visited(&self) -> usize {
|
||||
self.visited_grid.len()
|
||||
}
|
||||
|
||||
/// Get the number of recent positions tracked.
|
||||
pub fn recent_count(&self) -> usize {
|
||||
self.recent_positions.len()
|
||||
}
|
||||
|
||||
/// Get the distance to the nearest recent position.
|
||||
///
|
||||
/// Returns None if there are no recent positions.
|
||||
pub fn distance_to_nearest_recent(&self, position: Point2D) -> Option<f32> {
|
||||
self.recent_positions
|
||||
.iter()
|
||||
.map(|&p| position.distance(p))
|
||||
.min_by(|a, b| a.partial_cmp(b).unwrap_or(std::cmp::Ordering::Equal))
|
||||
}
|
||||
|
||||
/// Clear all history.
|
||||
pub fn clear(&mut self) {
|
||||
self.visited_grid.clear();
|
||||
self.recent_positions.clear();
|
||||
}
|
||||
|
||||
/// Convert a point to a grid cell coordinate.
|
||||
#[inline]
|
||||
fn point_to_cell(&self, point: Point2D) -> (i32, i32) {
|
||||
(
|
||||
(point.x / self.cell_size).floor() as i32,
|
||||
(point.y / self.cell_size).floor() as i32,
|
||||
)
|
||||
}
|
||||
|
||||
/// Get estimated memory usage in bytes.
|
||||
pub fn memory_usage(&self) -> usize {
|
||||
// HashMap overhead + entries
|
||||
let grid_size = self.visited_grid.len()
|
||||
* (std::mem::size_of::<(i32, i32)>() + std::mem::size_of::<u8>() + 16);
|
||||
// VecDeque + Point2D entries
|
||||
let recent_size = self.recent_positions.len() * std::mem::size_of::<Point2D>();
|
||||
|
||||
grid_size + recent_size
|
||||
}
|
||||
}
|
||||
|
||||
impl Default for ExplorationHistory {
|
||||
fn default() -> Self {
|
||||
Self::with_defaults()
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn test_empty_history() {
|
||||
let history = ExplorationHistory::new(0.5, 100);
|
||||
assert_eq!(history.cells_visited(), 0);
|
||||
assert_eq!(history.recent_count(), 0);
|
||||
assert!(!history.is_recently_visited(Point2D::new(0.0, 0.0)));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_mark_visited() {
|
||||
let mut history = ExplorationHistory::new(0.5, 100);
|
||||
|
||||
history.mark_visited(Point2D::new(1.0, 1.0));
|
||||
|
||||
assert_eq!(history.cells_visited(), 1);
|
||||
assert_eq!(history.recent_count(), 1);
|
||||
assert!(history.is_recently_visited(Point2D::new(1.0, 1.0)));
|
||||
assert!(history.is_recently_visited(Point2D::new(1.2, 1.2))); // Same cell
|
||||
assert!(!history.is_recently_visited(Point2D::new(5.0, 5.0))); // Different cell
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_visit_count() {
|
||||
let mut history = ExplorationHistory::new(0.5, 100);
|
||||
|
||||
// Visit same cell multiple times
|
||||
history.mark_visited(Point2D::new(1.0, 1.0));
|
||||
history.mark_visited(Point2D::new(1.1, 1.1));
|
||||
history.mark_visited(Point2D::new(1.2, 1.2));
|
||||
|
||||
assert_eq!(history.get_visit_count(Point2D::new(1.0, 1.0)), 3);
|
||||
assert_eq!(history.cells_visited(), 1);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_recent_positions_limit() {
|
||||
let mut history = ExplorationHistory::new(0.5, 3);
|
||||
|
||||
history.mark_visited(Point2D::new(0.0, 0.0));
|
||||
history.mark_visited(Point2D::new(1.0, 0.0));
|
||||
history.mark_visited(Point2D::new(2.0, 0.0));
|
||||
history.mark_visited(Point2D::new(3.0, 0.0));
|
||||
|
||||
// Should only have last 3 positions
|
||||
assert_eq!(history.recent_count(), 3);
|
||||
|
||||
// But all cells should still be marked
|
||||
assert_eq!(history.cells_visited(), 4);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_area_recently_visited() {
|
||||
let mut history = ExplorationHistory::new(1.0, 100);
|
||||
|
||||
history.mark_visited(Point2D::new(5.0, 5.0));
|
||||
|
||||
// Position in same cell
|
||||
assert!(history.is_area_recently_visited(Point2D::new(5.5, 5.5)));
|
||||
|
||||
// Position in adjacent cell (within 3x3 neighborhood)
|
||||
assert!(history.is_area_recently_visited(Point2D::new(6.5, 5.5)));
|
||||
|
||||
// Position far away
|
||||
assert!(!history.is_area_recently_visited(Point2D::new(10.0, 10.0)));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_distance_to_nearest_recent() {
|
||||
let mut history = ExplorationHistory::new(0.5, 100);
|
||||
|
||||
assert!(
|
||||
history
|
||||
.distance_to_nearest_recent(Point2D::new(0.0, 0.0))
|
||||
.is_none()
|
||||
);
|
||||
|
||||
history.mark_visited(Point2D::new(0.0, 0.0));
|
||||
history.mark_visited(Point2D::new(3.0, 0.0));
|
||||
|
||||
let dist = history.distance_to_nearest_recent(Point2D::new(1.0, 0.0));
|
||||
assert!(dist.is_some());
|
||||
assert!((dist.unwrap() - 1.0).abs() < 0.001);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_clear() {
|
||||
let mut history = ExplorationHistory::new(0.5, 100);
|
||||
|
||||
history.mark_visited(Point2D::new(1.0, 1.0));
|
||||
history.mark_visited(Point2D::new(2.0, 2.0));
|
||||
|
||||
assert_eq!(history.cells_visited(), 2);
|
||||
|
||||
history.clear();
|
||||
|
||||
assert_eq!(history.cells_visited(), 0);
|
||||
assert_eq!(history.recent_count(), 0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_cell_boundaries() {
|
||||
let mut history = ExplorationHistory::new(1.0, 100);
|
||||
|
||||
// Points at cell boundaries
|
||||
history.mark_visited(Point2D::new(0.99, 0.99)); // Cell (0, 0)
|
||||
history.mark_visited(Point2D::new(1.01, 1.01)); // Cell (1, 1)
|
||||
|
||||
assert_eq!(history.cells_visited(), 2);
|
||||
assert_eq!(history.get_visit_count(Point2D::new(0.5, 0.5)), 1);
|
||||
assert_eq!(history.get_visit_count(Point2D::new(1.5, 1.5)), 1);
|
||||
}
|
||||
}
|
||||
@@ -64,10 +64,16 @@
|
||||
mod collision;
|
||||
mod config;
|
||||
mod controller;
|
||||
pub mod history;
|
||||
mod path_follower;
|
||||
pub mod planner;
|
||||
pub mod region;
|
||||
|
||||
// Re-export public types
|
||||
pub use collision::{CollisionEvent, CollisionType, VirtualWall};
|
||||
pub use config::ExplorationConfig;
|
||||
pub use controller::{ExplorationController, ExplorationState, ExplorationStep};
|
||||
pub use history::ExplorationHistory;
|
||||
pub use path_follower::{FollowResult, PathFollower, VelocityCommand};
|
||||
pub use planner::{ExplorationPlan, ExplorationPlanner, PlannerConfig};
|
||||
pub use region::{ExplorationRegion, RegionConfig, RegionDetector};
|
||||
|
||||
@@ -0,0 +1,577 @@
|
||||
//! Exploration planning with region-based TSP ordering.
|
||||
//!
|
||||
//! Provides global exploration planning by ordering regions to minimize
|
||||
//! total travel distance while ensuring complete coverage.
|
||||
|
||||
use std::f32::consts::PI;
|
||||
|
||||
use crate::Frontier;
|
||||
use crate::core::Point2D;
|
||||
|
||||
use super::history::ExplorationHistory;
|
||||
use super::region::{ExplorationRegion, RegionConfig, RegionDetector};
|
||||
|
||||
/// Configuration for exploration planning.
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct PlannerConfig {
|
||||
/// Weight for region priority in selection (0.0 - 1.0).
|
||||
/// Default: 0.3
|
||||
pub priority_weight: f32,
|
||||
|
||||
/// Weight for distance penalty in region selection.
|
||||
/// Default: 0.5
|
||||
pub distance_weight: f32,
|
||||
|
||||
/// Penalty for revisiting regions.
|
||||
/// Default: 0.5
|
||||
pub revisit_penalty: f32,
|
||||
|
||||
/// Bonus per frontier in a region.
|
||||
/// Default: 0.1
|
||||
pub frontier_bonus: f32,
|
||||
|
||||
/// Weight for heading alignment when selecting frontiers within a region.
|
||||
/// Default: 0.3
|
||||
pub heading_weight: f32,
|
||||
|
||||
/// Weight for frontier openness.
|
||||
/// Default: 0.4
|
||||
pub openness_weight: f32,
|
||||
|
||||
/// Weight for local distance within a region.
|
||||
/// Default: 0.3
|
||||
pub local_distance_weight: f32,
|
||||
|
||||
/// Interval before forcing a full replan (seconds).
|
||||
/// Default: 10.0
|
||||
pub replan_interval_secs: f32,
|
||||
|
||||
/// Threshold of frontier count change to trigger replan.
|
||||
/// Default: 5
|
||||
pub frontier_change_threshold: usize,
|
||||
|
||||
/// Region detection configuration.
|
||||
pub region_config: RegionConfig,
|
||||
}
|
||||
|
||||
impl Default for PlannerConfig {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
priority_weight: 0.3,
|
||||
distance_weight: 0.5,
|
||||
revisit_penalty: 0.5,
|
||||
frontier_bonus: 0.1,
|
||||
heading_weight: 0.3,
|
||||
openness_weight: 0.4,
|
||||
local_distance_weight: 0.3,
|
||||
replan_interval_secs: 120.0, // Replan less often to maintain region focus
|
||||
frontier_change_threshold: 50, // Only replan on major changes
|
||||
region_config: RegionConfig::default(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl PlannerConfig {
|
||||
/// Create a new configuration with default values.
|
||||
pub fn new() -> Self {
|
||||
Self::default()
|
||||
}
|
||||
|
||||
/// Builder-style setter for heading weight.
|
||||
pub fn with_heading_weight(mut self, weight: f32) -> Self {
|
||||
self.heading_weight = weight;
|
||||
self
|
||||
}
|
||||
|
||||
/// Builder-style setter for openness weight.
|
||||
pub fn with_openness_weight(mut self, weight: f32) -> Self {
|
||||
self.openness_weight = weight;
|
||||
self
|
||||
}
|
||||
|
||||
/// Builder-style setter for local distance weight.
|
||||
pub fn with_local_distance_weight(mut self, weight: f32) -> Self {
|
||||
self.local_distance_weight = weight;
|
||||
self
|
||||
}
|
||||
|
||||
/// Builder-style setter for region config.
|
||||
pub fn with_region_config(mut self, config: RegionConfig) -> Self {
|
||||
self.region_config = config;
|
||||
self
|
||||
}
|
||||
}
|
||||
|
||||
/// Global exploration plan with ordered regions.
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct ExplorationPlan {
|
||||
/// Ordered list of regions to visit.
|
||||
pub regions: Vec<ExplorationRegion>,
|
||||
|
||||
/// Current region index being explored.
|
||||
pub current_region_idx: usize,
|
||||
|
||||
/// Whether the plan needs recomputation.
|
||||
pub stale: bool,
|
||||
|
||||
/// Number of frontiers at last planning.
|
||||
pub last_frontier_count: usize,
|
||||
|
||||
/// Elapsed time since last replan (seconds).
|
||||
pub time_since_replan: f32,
|
||||
}
|
||||
|
||||
impl Default for ExplorationPlan {
|
||||
fn default() -> Self {
|
||||
Self::new()
|
||||
}
|
||||
}
|
||||
|
||||
impl ExplorationPlan {
|
||||
/// Create a new empty exploration plan.
|
||||
pub fn new() -> Self {
|
||||
Self {
|
||||
regions: Vec::new(),
|
||||
current_region_idx: 0,
|
||||
stale: true,
|
||||
last_frontier_count: 0,
|
||||
time_since_replan: 0.0,
|
||||
}
|
||||
}
|
||||
|
||||
/// Check if the plan is empty (no regions).
|
||||
pub fn is_empty(&self) -> bool {
|
||||
self.regions.is_empty()
|
||||
}
|
||||
|
||||
/// Get the current region being explored.
|
||||
pub fn current_region(&self) -> Option<&ExplorationRegion> {
|
||||
self.regions.get(self.current_region_idx)
|
||||
}
|
||||
|
||||
/// Get the current region mutably.
|
||||
pub fn current_region_mut(&mut self) -> Option<&mut ExplorationRegion> {
|
||||
self.regions.get_mut(self.current_region_idx)
|
||||
}
|
||||
|
||||
/// Advance to the next region.
|
||||
pub fn advance_region(&mut self) {
|
||||
if let Some(region) = self.regions.get_mut(self.current_region_idx) {
|
||||
region.visit_count += 1;
|
||||
}
|
||||
self.current_region_idx += 1;
|
||||
}
|
||||
|
||||
/// Check if all regions have been processed.
|
||||
pub fn is_complete(&self) -> bool {
|
||||
self.current_region_idx >= self.regions.len()
|
||||
}
|
||||
|
||||
/// Get the total number of remaining frontiers across all regions.
|
||||
pub fn remaining_frontier_count(&self) -> usize {
|
||||
self.regions
|
||||
.iter()
|
||||
.skip(self.current_region_idx)
|
||||
.map(|r| r.frontier_count())
|
||||
.sum()
|
||||
}
|
||||
}
|
||||
|
||||
/// Exploration planner that manages region detection and ordering.
|
||||
pub struct ExplorationPlanner {
|
||||
/// Region detector instance.
|
||||
region_detector: RegionDetector,
|
||||
|
||||
/// Current exploration plan.
|
||||
plan: ExplorationPlan,
|
||||
|
||||
/// Configuration.
|
||||
config: PlannerConfig,
|
||||
}
|
||||
|
||||
impl ExplorationPlanner {
|
||||
/// Create a new exploration planner.
|
||||
pub fn new(config: PlannerConfig) -> Self {
|
||||
Self {
|
||||
region_detector: RegionDetector::new(),
|
||||
plan: ExplorationPlan::new(),
|
||||
config,
|
||||
}
|
||||
}
|
||||
|
||||
/// Get the current exploration plan.
|
||||
pub fn plan(&self) -> &ExplorationPlan {
|
||||
&self.plan
|
||||
}
|
||||
|
||||
/// Get the current exploration plan mutably.
|
||||
pub fn plan_mut(&mut self) -> &mut ExplorationPlan {
|
||||
&mut self.plan
|
||||
}
|
||||
|
||||
/// Update the exploration plan if needed.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `frontiers` - Current list of frontiers
|
||||
/// * `robot_pos` - Current robot position
|
||||
/// * `history` - Exploration history for visit tracking
|
||||
/// * `dt` - Time since last update (seconds)
|
||||
pub fn update(
|
||||
&mut self,
|
||||
frontiers: &[Frontier],
|
||||
robot_pos: Point2D,
|
||||
history: &ExplorationHistory,
|
||||
dt: f32,
|
||||
) {
|
||||
self.plan.time_since_replan += dt;
|
||||
|
||||
if self.should_replan(frontiers) {
|
||||
self.replan(frontiers, robot_pos, history);
|
||||
} else {
|
||||
// Incremental update: refresh frontiers in current region
|
||||
self.update_current_region(frontiers);
|
||||
}
|
||||
}
|
||||
|
||||
/// Force a full replan.
|
||||
pub fn force_replan(
|
||||
&mut self,
|
||||
frontiers: &[Frontier],
|
||||
robot_pos: Point2D,
|
||||
history: &ExplorationHistory,
|
||||
) {
|
||||
self.replan(frontiers, robot_pos, history);
|
||||
}
|
||||
|
||||
/// Check if a replan is needed.
|
||||
fn should_replan(&self, frontiers: &[Frontier]) -> bool {
|
||||
// Plan is stale
|
||||
if self.plan.stale {
|
||||
return true;
|
||||
}
|
||||
|
||||
// Time-based replan
|
||||
if self.plan.time_since_replan >= self.config.replan_interval_secs {
|
||||
return true;
|
||||
}
|
||||
|
||||
// Frontier count changed significantly
|
||||
let frontier_change =
|
||||
(frontiers.len() as i32 - self.plan.last_frontier_count as i32).unsigned_abs() as usize;
|
||||
if frontier_change >= self.config.frontier_change_threshold {
|
||||
return true;
|
||||
}
|
||||
|
||||
// Current region exhausted
|
||||
if self.plan.current_region_idx >= self.plan.regions.len() {
|
||||
return true;
|
||||
}
|
||||
|
||||
// Current region has no frontiers left
|
||||
if let Some(region) = self.plan.current_region()
|
||||
&& region.is_empty()
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
false
|
||||
}
|
||||
|
||||
/// Perform a full replan.
|
||||
fn replan(&mut self, frontiers: &[Frontier], robot_pos: Point2D, history: &ExplorationHistory) {
|
||||
// Remember previous region centroid to try to maintain context
|
||||
let prev_centroid = self.plan.current_region().map(|r| r.centroid);
|
||||
|
||||
// Detect regions
|
||||
let mut regions = self
|
||||
.region_detector
|
||||
.detect(frontiers, &self.config.region_config);
|
||||
|
||||
// Order regions using greedy TSP
|
||||
let ordered_regions = self.order_regions(&mut regions, robot_pos, history);
|
||||
|
||||
// Update plan
|
||||
self.plan.regions = ordered_regions;
|
||||
self.plan.stale = false;
|
||||
self.plan.last_frontier_count = frontiers.len();
|
||||
self.plan.time_since_replan = 0.0;
|
||||
|
||||
// Try to find a region close to where we were exploring
|
||||
// This maintains continuity instead of jumping to a new area
|
||||
if let Some(prev) = prev_centroid {
|
||||
let closest_idx = self
|
||||
.plan
|
||||
.regions
|
||||
.iter()
|
||||
.enumerate()
|
||||
.min_by(|(_, a), (_, b)| {
|
||||
let dist_a = a.centroid.distance(prev);
|
||||
let dist_b = b.centroid.distance(prev);
|
||||
dist_a
|
||||
.partial_cmp(&dist_b)
|
||||
.unwrap_or(std::cmp::Ordering::Equal)
|
||||
})
|
||||
.map(|(i, _)| i)
|
||||
.unwrap_or(0);
|
||||
|
||||
// Only use the closest region if it's reasonably close (within 2m)
|
||||
if let Some(region) = self.plan.regions.get(closest_idx)
|
||||
&& region.centroid.distance(prev) < 2.0
|
||||
{
|
||||
self.plan.current_region_idx = closest_idx;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// Default: start from the first (best) region
|
||||
self.plan.current_region_idx = 0;
|
||||
}
|
||||
|
||||
/// Order regions using greedy TSP with priority weighting.
|
||||
fn order_regions(
|
||||
&self,
|
||||
regions: &mut Vec<ExplorationRegion>,
|
||||
robot_pos: Point2D,
|
||||
history: &ExplorationHistory,
|
||||
) -> Vec<ExplorationRegion> {
|
||||
if regions.is_empty() {
|
||||
return Vec::new();
|
||||
}
|
||||
|
||||
// Compute priorities for all regions
|
||||
for region in regions.iter_mut() {
|
||||
region.priority = self.compute_region_priority(region, robot_pos, history);
|
||||
}
|
||||
|
||||
// Greedy TSP: always pick the best next region
|
||||
let mut ordered = Vec::with_capacity(regions.len());
|
||||
let mut remaining: Vec<ExplorationRegion> = std::mem::take(regions);
|
||||
let mut current_pos = robot_pos;
|
||||
|
||||
while !remaining.is_empty() {
|
||||
// Find best next region
|
||||
let best_idx = remaining
|
||||
.iter()
|
||||
.enumerate()
|
||||
.map(|(i, region)| {
|
||||
let distance = current_pos.distance(region.centroid);
|
||||
let score = region.priority * self.config.priority_weight
|
||||
- distance * self.config.distance_weight
|
||||
- region.visit_count as f32 * self.config.revisit_penalty
|
||||
+ region.frontier_count() as f32 * self.config.frontier_bonus;
|
||||
(i, score)
|
||||
})
|
||||
.max_by(|a, b| a.1.partial_cmp(&b.1).unwrap_or(std::cmp::Ordering::Equal))
|
||||
.map(|(i, _)| i)
|
||||
.unwrap_or(0);
|
||||
|
||||
let selected = remaining.remove(best_idx);
|
||||
current_pos = selected.centroid;
|
||||
ordered.push(selected);
|
||||
}
|
||||
|
||||
ordered
|
||||
}
|
||||
|
||||
/// Compute the priority of a region.
|
||||
fn compute_region_priority(
|
||||
&self,
|
||||
region: &ExplorationRegion,
|
||||
_robot_pos: Point2D,
|
||||
history: &ExplorationHistory,
|
||||
) -> f32 {
|
||||
// Base priority from estimated area (larger regions are more valuable)
|
||||
let area_score = (region.estimated_area / 10.0).min(1.0);
|
||||
|
||||
// Penalty for regions near recently visited areas
|
||||
let visit_penalty = if history.is_area_recently_visited(region.centroid) {
|
||||
0.5
|
||||
} else {
|
||||
0.0
|
||||
};
|
||||
|
||||
// Bonus for regions with more frontiers
|
||||
let frontier_score = (region.frontier_count() as f32 / 5.0).min(1.0);
|
||||
|
||||
area_score * 0.3 + frontier_score * 0.7 - visit_penalty
|
||||
}
|
||||
|
||||
/// Update the current region with valid frontiers.
|
||||
fn update_current_region(&mut self, frontiers: &[Frontier]) {
|
||||
if let Some(region) = self.plan.current_region_mut() {
|
||||
region.retain_valid_frontiers(frontiers);
|
||||
}
|
||||
}
|
||||
|
||||
/// Select the best frontier within the current region.
|
||||
///
|
||||
/// Uses heading-aligned selection to prevent ping-pong behavior.
|
||||
pub fn select_frontier_in_region<'a>(
|
||||
&self,
|
||||
region: &'a ExplorationRegion,
|
||||
robot_pos: Point2D,
|
||||
robot_heading: f32,
|
||||
) -> Option<&'a Frontier> {
|
||||
if region.is_empty() {
|
||||
return None;
|
||||
}
|
||||
|
||||
// Score each frontier
|
||||
|
||||
region
|
||||
.frontiers
|
||||
.iter()
|
||||
.map(|frontier| {
|
||||
let score = self.compute_frontier_score(frontier, robot_pos, robot_heading);
|
||||
(frontier, score)
|
||||
})
|
||||
.max_by(|a, b| a.1.partial_cmp(&b.1).unwrap_or(std::cmp::Ordering::Equal))
|
||||
.map(|(f, _)| f)
|
||||
}
|
||||
|
||||
/// Compute the score of a frontier for selection.
|
||||
fn compute_frontier_score(
|
||||
&self,
|
||||
frontier: &Frontier,
|
||||
robot_pos: Point2D,
|
||||
robot_heading: f32,
|
||||
) -> f32 {
|
||||
// Use viewpoint for distance calculations (safe navigation target)
|
||||
let direction = Point2D::new(
|
||||
frontier.viewpoint.x - robot_pos.x,
|
||||
frontier.viewpoint.y - robot_pos.y,
|
||||
);
|
||||
let distance = direction.length();
|
||||
|
||||
if distance < 0.001 {
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
// Heading alignment score (1.0 = same direction, 0.0 = opposite)
|
||||
let angle_to_frontier = direction.y.atan2(direction.x);
|
||||
let angle_diff = normalize_angle(angle_to_frontier - robot_heading);
|
||||
let heading_score = 1.0 - (angle_diff.abs() / PI);
|
||||
|
||||
// Distance score (prefer closer frontiers within the region)
|
||||
let distance_score = 1.0 / (1.0 + distance * 0.5);
|
||||
|
||||
// Combined score
|
||||
heading_score * self.config.heading_weight
|
||||
+ distance_score * self.config.local_distance_weight
|
||||
+ self.config.openness_weight * 0.5 // Placeholder for openness (not currently available on Frontier)
|
||||
}
|
||||
|
||||
/// Reset the planner state.
|
||||
pub fn reset(&mut self) {
|
||||
self.plan = ExplorationPlan::new();
|
||||
self.region_detector.reset_ids();
|
||||
}
|
||||
}
|
||||
|
||||
/// Normalize an angle to [-PI, PI].
|
||||
fn normalize_angle(angle: f32) -> f32 {
|
||||
let mut result = angle;
|
||||
while result > PI {
|
||||
result -= 2.0 * PI;
|
||||
}
|
||||
while result < -PI {
|
||||
result += 2.0 * PI;
|
||||
}
|
||||
result
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
fn make_frontier(x: f32, y: f32) -> Frontier {
|
||||
Frontier {
|
||||
viewpoint: Point2D::new(x, y),
|
||||
look_direction: Point2D::new(1.0, 0.0),
|
||||
endpoint: Point2D::new(x - 0.4, y),
|
||||
line_idx: 0,
|
||||
estimated_area: 4.0,
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_empty_plan() {
|
||||
let config = PlannerConfig::default();
|
||||
let planner = ExplorationPlanner::new(config);
|
||||
|
||||
assert!(planner.plan().is_empty());
|
||||
assert!(planner.plan().is_complete());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_replan_with_frontiers() {
|
||||
let config = PlannerConfig::default();
|
||||
let mut planner = ExplorationPlanner::new(config);
|
||||
let history = ExplorationHistory::new(0.5, 100);
|
||||
|
||||
let frontiers = vec![
|
||||
make_frontier(1.0, 0.0),
|
||||
make_frontier(1.5, 0.0),
|
||||
make_frontier(5.0, 5.0),
|
||||
make_frontier(5.5, 5.0),
|
||||
];
|
||||
|
||||
planner.force_replan(&frontiers, Point2D::new(0.0, 0.0), &history);
|
||||
|
||||
assert!(!planner.plan().is_empty());
|
||||
assert!(!planner.plan().is_complete());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_frontier_selection_prefers_heading() {
|
||||
let config = PlannerConfig::default().with_heading_weight(0.9);
|
||||
let planner = ExplorationPlanner::new(config);
|
||||
|
||||
// Robot facing right (heading = 0)
|
||||
let robot_pos = Point2D::new(0.0, 0.0);
|
||||
let robot_heading = 0.0;
|
||||
|
||||
// Two frontiers: one ahead (x=2), one behind (x=-2)
|
||||
let frontiers = vec![make_frontier(2.0, 0.0), make_frontier(-2.0, 0.0)];
|
||||
|
||||
let region = ExplorationRegion::from_frontiers(0, frontiers);
|
||||
|
||||
let selected = planner.select_frontier_in_region(®ion, robot_pos, robot_heading);
|
||||
|
||||
assert!(selected.is_some());
|
||||
let selected = selected.unwrap();
|
||||
// Should prefer the frontier ahead (positive x)
|
||||
assert!(selected.viewpoint.x > 0.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_normalize_angle() {
|
||||
assert!((normalize_angle(0.0) - 0.0).abs() < 0.001);
|
||||
assert!((normalize_angle(PI) - PI).abs() < 0.001);
|
||||
assert!((normalize_angle(-PI) - (-PI)).abs() < 0.001);
|
||||
assert!((normalize_angle(3.0 * PI) - PI).abs() < 0.001);
|
||||
assert!((normalize_angle(-3.0 * PI) - (-PI)).abs() < 0.001);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_plan_advance_region() {
|
||||
let mut plan = ExplorationPlan::new();
|
||||
let frontiers = vec![make_frontier(1.0, 0.0)];
|
||||
plan.regions
|
||||
.push(ExplorationRegion::from_frontiers(0, frontiers.clone()));
|
||||
plan.regions
|
||||
.push(ExplorationRegion::from_frontiers(1, frontiers));
|
||||
plan.stale = false;
|
||||
|
||||
assert_eq!(plan.current_region_idx, 0);
|
||||
assert!(!plan.is_complete());
|
||||
|
||||
plan.advance_region();
|
||||
assert_eq!(plan.current_region_idx, 1);
|
||||
assert!(!plan.is_complete());
|
||||
|
||||
plan.advance_region();
|
||||
assert_eq!(plan.current_region_idx, 2);
|
||||
assert!(plan.is_complete());
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,424 @@
|
||||
//! Exploration region detection and clustering.
|
||||
//!
|
||||
//! Groups nearby frontiers into exploration regions using hierarchical clustering.
|
||||
//! This enables region-based exploration strategies that avoid ping-pong behavior.
|
||||
|
||||
use std::collections::HashMap;
|
||||
|
||||
use crate::Frontier;
|
||||
use crate::core::{Bounds, Point2D};
|
||||
|
||||
/// Configuration for region detection.
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct RegionConfig {
|
||||
/// Maximum distance for clustering frontiers into regions (meters).
|
||||
/// Default: 1.5
|
||||
pub cluster_distance: f32,
|
||||
|
||||
/// Minimum number of frontiers to form a valid region.
|
||||
/// Default: 1
|
||||
pub min_region_size: usize,
|
||||
}
|
||||
|
||||
impl Default for RegionConfig {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
cluster_distance: 1.5,
|
||||
min_region_size: 1,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl RegionConfig {
|
||||
/// Create a new configuration with default values.
|
||||
pub fn new() -> Self {
|
||||
Self::default()
|
||||
}
|
||||
|
||||
/// Builder-style setter for cluster distance.
|
||||
pub fn with_cluster_distance(mut self, distance: f32) -> Self {
|
||||
self.cluster_distance = distance;
|
||||
self
|
||||
}
|
||||
|
||||
/// Builder-style setter for minimum region size.
|
||||
pub fn with_min_region_size(mut self, size: usize) -> Self {
|
||||
self.min_region_size = size;
|
||||
self
|
||||
}
|
||||
}
|
||||
|
||||
/// A clustered group of frontiers representing an unexplored area.
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct ExplorationRegion {
|
||||
/// Unique region identifier.
|
||||
pub id: u32,
|
||||
|
||||
/// Centroid of the region (mean of all frontier points).
|
||||
pub centroid: Point2D,
|
||||
|
||||
/// Individual frontiers in this region.
|
||||
pub frontiers: Vec<Frontier>,
|
||||
|
||||
/// Bounding box of the region.
|
||||
pub bounds: Bounds,
|
||||
|
||||
/// Estimated unexplored area (heuristic based on spread).
|
||||
pub estimated_area: f32,
|
||||
|
||||
/// Priority score (higher = explore first).
|
||||
pub priority: f32,
|
||||
|
||||
/// Number of times we've attempted to visit this region.
|
||||
pub visit_count: u32,
|
||||
}
|
||||
|
||||
impl ExplorationRegion {
|
||||
/// Create a new region from a cluster of frontiers.
|
||||
pub fn from_frontiers(id: u32, frontiers: Vec<Frontier>) -> Self {
|
||||
let centroid = compute_centroid(&frontiers);
|
||||
let bounds = compute_bounds(&frontiers);
|
||||
let estimated_area = estimate_area(&bounds);
|
||||
|
||||
Self {
|
||||
id,
|
||||
centroid,
|
||||
frontiers,
|
||||
bounds,
|
||||
estimated_area,
|
||||
priority: 0.0,
|
||||
visit_count: 0,
|
||||
}
|
||||
}
|
||||
|
||||
/// Get the number of frontiers in this region.
|
||||
pub fn frontier_count(&self) -> usize {
|
||||
self.frontiers.len()
|
||||
}
|
||||
|
||||
/// Check if the region has no frontiers left.
|
||||
pub fn is_empty(&self) -> bool {
|
||||
self.frontiers.is_empty()
|
||||
}
|
||||
|
||||
/// Remove frontiers that are no longer valid (not in the current frontier list).
|
||||
pub fn retain_valid_frontiers(&mut self, valid_frontiers: &[Frontier]) {
|
||||
self.frontiers.retain(|f| {
|
||||
valid_frontiers
|
||||
.iter()
|
||||
.any(|vf| f.viewpoint.distance(vf.viewpoint) < 0.1)
|
||||
});
|
||||
|
||||
// Update centroid and bounds if we still have frontiers
|
||||
if !self.frontiers.is_empty() {
|
||||
self.centroid = compute_centroid(&self.frontiers);
|
||||
self.bounds = compute_bounds(&self.frontiers);
|
||||
self.estimated_area = estimate_area(&self.bounds);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Detect exploration regions from frontiers using hierarchical clustering.
|
||||
pub struct RegionDetector {
|
||||
/// Reusable union-find parent array.
|
||||
parent: Vec<usize>,
|
||||
|
||||
/// Reusable union-find rank array.
|
||||
rank: Vec<usize>,
|
||||
|
||||
/// Next region ID to assign.
|
||||
next_id: u32,
|
||||
|
||||
/// Spatial hash grid for efficient neighbor lookup.
|
||||
spatial_grid: HashMap<(i32, i32), Vec<usize>>,
|
||||
}
|
||||
|
||||
impl Default for RegionDetector {
|
||||
fn default() -> Self {
|
||||
Self::new()
|
||||
}
|
||||
}
|
||||
|
||||
impl RegionDetector {
|
||||
/// Create a new region detector.
|
||||
pub fn new() -> Self {
|
||||
Self {
|
||||
parent: Vec::with_capacity(100),
|
||||
rank: Vec::with_capacity(100),
|
||||
next_id: 0,
|
||||
spatial_grid: HashMap::with_capacity(100),
|
||||
}
|
||||
}
|
||||
|
||||
/// Detect regions from a list of frontiers.
|
||||
pub fn detect(
|
||||
&mut self,
|
||||
frontiers: &[Frontier],
|
||||
config: &RegionConfig,
|
||||
) -> Vec<ExplorationRegion> {
|
||||
if frontiers.is_empty() {
|
||||
return Vec::new();
|
||||
}
|
||||
|
||||
// Cluster frontiers using union-find
|
||||
let clusters = self.cluster_frontiers(frontiers, config.cluster_distance);
|
||||
|
||||
// Convert clusters to regions
|
||||
let mut regions = Vec::new();
|
||||
for cluster in clusters {
|
||||
if cluster.len() >= config.min_region_size {
|
||||
let region_frontiers: Vec<Frontier> =
|
||||
cluster.iter().map(|&idx| frontiers[idx].clone()).collect();
|
||||
|
||||
let region = ExplorationRegion::from_frontiers(self.next_id, region_frontiers);
|
||||
self.next_id += 1;
|
||||
regions.push(region);
|
||||
}
|
||||
}
|
||||
|
||||
regions
|
||||
}
|
||||
|
||||
/// Cluster frontiers using union-find with spatial hashing.
|
||||
fn cluster_frontiers(&mut self, frontiers: &[Frontier], max_distance: f32) -> Vec<Vec<usize>> {
|
||||
let n = frontiers.len();
|
||||
|
||||
// Initialize union-find
|
||||
self.parent.clear();
|
||||
self.parent.resize(n, 0);
|
||||
self.rank.clear();
|
||||
self.rank.resize(n, 0);
|
||||
for i in 0..n {
|
||||
self.parent[i] = i;
|
||||
}
|
||||
|
||||
// Clear spatial grid completely to avoid stale indices
|
||||
self.spatial_grid.clear();
|
||||
|
||||
// Build spatial hash grid (using viewpoint for clustering)
|
||||
let cell_size = max_distance;
|
||||
for (i, frontier) in frontiers.iter().enumerate() {
|
||||
let cell = (
|
||||
(frontier.viewpoint.x / cell_size).floor() as i32,
|
||||
(frontier.viewpoint.y / cell_size).floor() as i32,
|
||||
);
|
||||
|
||||
self.spatial_grid.entry(cell).or_default().push(i);
|
||||
}
|
||||
|
||||
// Collect pairs to union (avoids borrow conflict)
|
||||
let mut pairs_to_union: Vec<(usize, usize)> = Vec::new();
|
||||
|
||||
for (i, frontier) in frontiers.iter().enumerate() {
|
||||
let cx = (frontier.viewpoint.x / cell_size).floor() as i32;
|
||||
let cy = (frontier.viewpoint.y / cell_size).floor() as i32;
|
||||
|
||||
// Check 3x3 neighborhood
|
||||
for dx in -1..=1 {
|
||||
for dy in -1..=1 {
|
||||
let neighbor_cell = (cx + dx, cy + dy);
|
||||
if let Some(indices) = self.spatial_grid.get(&neighbor_cell) {
|
||||
for &j in indices {
|
||||
if i < j {
|
||||
let dist = frontier.viewpoint.distance(frontiers[j].viewpoint);
|
||||
if dist <= max_distance {
|
||||
pairs_to_union.push((i, j));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Perform unions
|
||||
for (i, j) in pairs_to_union {
|
||||
self.union(i, j);
|
||||
}
|
||||
|
||||
// Extract clusters
|
||||
let mut cluster_map: HashMap<usize, Vec<usize>> = HashMap::new();
|
||||
for i in 0..n {
|
||||
let root = self.find(i);
|
||||
cluster_map.entry(root).or_default().push(i);
|
||||
}
|
||||
|
||||
cluster_map.into_values().collect()
|
||||
}
|
||||
|
||||
/// Union-find: find with path compression.
|
||||
fn find(&mut self, x: usize) -> usize {
|
||||
if self.parent[x] != x {
|
||||
self.parent[x] = self.find(self.parent[x]);
|
||||
}
|
||||
self.parent[x]
|
||||
}
|
||||
|
||||
/// Union-find: union by rank.
|
||||
fn union(&mut self, x: usize, y: usize) {
|
||||
let root_x = self.find(x);
|
||||
let root_y = self.find(y);
|
||||
|
||||
if root_x != root_y {
|
||||
if self.rank[root_x] < self.rank[root_y] {
|
||||
self.parent[root_x] = root_y;
|
||||
} else if self.rank[root_x] > self.rank[root_y] {
|
||||
self.parent[root_y] = root_x;
|
||||
} else {
|
||||
self.parent[root_y] = root_x;
|
||||
self.rank[root_x] += 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Reset the region ID counter.
|
||||
pub fn reset_ids(&mut self) {
|
||||
self.next_id = 0;
|
||||
}
|
||||
}
|
||||
|
||||
/// Compute the centroid of a cluster of frontiers (using viewpoints).
|
||||
fn compute_centroid(frontiers: &[Frontier]) -> Point2D {
|
||||
if frontiers.is_empty() {
|
||||
return Point2D::new(0.0, 0.0);
|
||||
}
|
||||
|
||||
let sum_x: f32 = frontiers.iter().map(|f| f.viewpoint.x).sum();
|
||||
let sum_y: f32 = frontiers.iter().map(|f| f.viewpoint.y).sum();
|
||||
let n = frontiers.len() as f32;
|
||||
|
||||
Point2D::new(sum_x / n, sum_y / n)
|
||||
}
|
||||
|
||||
/// Compute the bounding box of a cluster of frontiers (using viewpoints).
|
||||
fn compute_bounds(frontiers: &[Frontier]) -> Bounds {
|
||||
if frontiers.is_empty() {
|
||||
return Bounds::empty();
|
||||
}
|
||||
|
||||
let mut min_x = f32::INFINITY;
|
||||
let mut min_y = f32::INFINITY;
|
||||
let mut max_x = f32::NEG_INFINITY;
|
||||
let mut max_y = f32::NEG_INFINITY;
|
||||
|
||||
for frontier in frontiers {
|
||||
min_x = min_x.min(frontier.viewpoint.x);
|
||||
min_y = min_y.min(frontier.viewpoint.y);
|
||||
max_x = max_x.max(frontier.viewpoint.x);
|
||||
max_y = max_y.max(frontier.viewpoint.y);
|
||||
}
|
||||
|
||||
Bounds::new(Point2D::new(min_x, min_y), Point2D::new(max_x, max_y))
|
||||
}
|
||||
|
||||
/// Estimate the unexplored area of a region based on its bounding box.
|
||||
fn estimate_area(bounds: &Bounds) -> f32 {
|
||||
let width = bounds.max.x - bounds.min.x;
|
||||
let height = bounds.max.y - bounds.min.y;
|
||||
|
||||
// Add a minimum area to avoid zero for single-point regions
|
||||
(width * height).max(0.1)
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
fn make_frontier(x: f32, y: f32) -> Frontier {
|
||||
Frontier {
|
||||
viewpoint: Point2D::new(x, y),
|
||||
look_direction: Point2D::new(1.0, 0.0),
|
||||
endpoint: Point2D::new(x - 0.4, y),
|
||||
line_idx: 0,
|
||||
estimated_area: 4.0,
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_empty_frontiers() {
|
||||
let mut detector = RegionDetector::new();
|
||||
let config = RegionConfig::default();
|
||||
let regions = detector.detect(&[], &config);
|
||||
assert!(regions.is_empty());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_single_frontier() {
|
||||
let mut detector = RegionDetector::new();
|
||||
let config = RegionConfig::default();
|
||||
let frontiers = vec![make_frontier(1.0, 1.0)];
|
||||
let regions = detector.detect(&frontiers, &config);
|
||||
|
||||
assert_eq!(regions.len(), 1);
|
||||
assert_eq!(regions[0].frontiers.len(), 1);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_two_clusters() {
|
||||
let mut detector = RegionDetector::new();
|
||||
let config = RegionConfig::default().with_cluster_distance(1.0);
|
||||
|
||||
// Two clusters: (0,0), (0.5, 0) and (10, 10), (10.5, 10)
|
||||
let frontiers = vec![
|
||||
make_frontier(0.0, 0.0),
|
||||
make_frontier(0.5, 0.0),
|
||||
make_frontier(10.0, 10.0),
|
||||
make_frontier(10.5, 10.0),
|
||||
];
|
||||
|
||||
let regions = detector.detect(&frontiers, &config);
|
||||
|
||||
assert_eq!(regions.len(), 2);
|
||||
assert_eq!(regions[0].frontiers.len(), 2);
|
||||
assert_eq!(regions[1].frontiers.len(), 2);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_centroid_calculation() {
|
||||
let frontiers = vec![
|
||||
make_frontier(0.0, 0.0),
|
||||
make_frontier(2.0, 0.0),
|
||||
make_frontier(1.0, 2.0),
|
||||
];
|
||||
|
||||
let centroid = compute_centroid(&frontiers);
|
||||
assert!((centroid.x - 1.0).abs() < 0.001);
|
||||
assert!((centroid.y - 0.667).abs() < 0.01);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_bounds_calculation() {
|
||||
let frontiers = vec![
|
||||
make_frontier(1.0, 2.0),
|
||||
make_frontier(3.0, 4.0),
|
||||
make_frontier(2.0, 1.0),
|
||||
];
|
||||
|
||||
let bounds = compute_bounds(&frontiers);
|
||||
assert_eq!(bounds.min.x, 1.0);
|
||||
assert_eq!(bounds.min.y, 1.0);
|
||||
assert_eq!(bounds.max.x, 3.0);
|
||||
assert_eq!(bounds.max.y, 4.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_min_region_size() {
|
||||
let mut detector = RegionDetector::new();
|
||||
let config = RegionConfig::default()
|
||||
.with_cluster_distance(0.5)
|
||||
.with_min_region_size(2);
|
||||
|
||||
// One isolated frontier and two nearby ones
|
||||
let frontiers = vec![
|
||||
make_frontier(0.0, 0.0), // Isolated
|
||||
make_frontier(10.0, 10.0),
|
||||
make_frontier(10.2, 10.0), // Together
|
||||
];
|
||||
|
||||
let regions = detector.detect(&frontiers, &config);
|
||||
|
||||
// Only the cluster with 2 frontiers should be included
|
||||
assert_eq!(regions.len(), 1);
|
||||
assert_eq!(regions[0].frontiers.len(), 2);
|
||||
}
|
||||
}
|
||||
+37
-6
@@ -159,15 +159,46 @@ pub enum Occupancy {
|
||||
Unknown,
|
||||
}
|
||||
|
||||
/// Exploration frontier (unconnected line endpoint).
|
||||
/// Exploration frontier representing an unexplored shadow area.
|
||||
///
|
||||
/// A frontier is a safe observation point from which the robot can
|
||||
/// scan an unexplored region (gap between unconnected lines).
|
||||
///
|
||||
/// # Key Concept
|
||||
///
|
||||
/// Instead of targeting line endpoints (which are walls), frontiers
|
||||
/// now represent viewpoints that can observe unexplored gaps:
|
||||
///
|
||||
/// ```text
|
||||
/// Wall ─────┐ endpoint
|
||||
/// │
|
||||
/// GAP │ ← Unexplored shadow area
|
||||
/// HERE │
|
||||
/// │
|
||||
/// Wall ─────┘ endpoint
|
||||
///
|
||||
/// Viewpoint is offset from wall, looking into the gap
|
||||
/// ```
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct Frontier {
|
||||
/// Position of the frontier point.
|
||||
pub point: Point2D,
|
||||
/// Index of the line this endpoint belongs to.
|
||||
/// Safe observation point where robot should navigate to.
|
||||
/// This is offset from the wall to ensure the robot stays in free space.
|
||||
pub viewpoint: Point2D,
|
||||
|
||||
/// Direction towards the unexplored area (unit vector).
|
||||
/// The robot's lidar will scan in this direction when it reaches the viewpoint.
|
||||
pub look_direction: Point2D,
|
||||
|
||||
/// Original line endpoint that created this frontier.
|
||||
/// Used for tracking which gap this frontier observes.
|
||||
pub endpoint: Point2D,
|
||||
|
||||
/// Index of the source line in the map.
|
||||
pub line_idx: usize,
|
||||
/// True if this is the start endpoint, false if end.
|
||||
pub is_start: bool,
|
||||
|
||||
/// Estimated unexplored area visible from this viewpoint (m²).
|
||||
/// Used for prioritizing larger unexplored regions.
|
||||
pub estimated_area: f32,
|
||||
}
|
||||
|
||||
/// A planned path through the map.
|
||||
|
||||
+571
-543
File diff suppressed because it is too large
Load Diff
@@ -36,7 +36,7 @@ pub mod raycast;
|
||||
// Re-export main types and functions
|
||||
pub use frontier::{
|
||||
FrontierConfig, FrontierDetector, cluster_centroid, cluster_frontiers, detect_frontiers,
|
||||
detect_frontiers_from_robot, get_best_frontier, rank_frontiers,
|
||||
get_best_frontier, rank_frontiers,
|
||||
};
|
||||
pub use occupancy::{
|
||||
OccupancyConfig, is_path_clear, is_region_clear, query_occupancy, query_occupancy_batch,
|
||||
|
||||
@@ -22,12 +22,6 @@ pub struct OccupancyConfig {
|
||||
/// Points must be within this distance of some map feature to be known.
|
||||
/// Default: 2.0m
|
||||
pub known_distance: f32,
|
||||
|
||||
/// Whether to use winding number for inside/outside determination.
|
||||
/// If true, uses contour-based occupancy (like font rendering).
|
||||
/// If false, uses simple distance-based occupancy.
|
||||
/// Default: false (distance-based is simpler and usually sufficient)
|
||||
pub use_winding_number: bool,
|
||||
}
|
||||
|
||||
impl Default for OccupancyConfig {
|
||||
@@ -35,7 +29,6 @@ impl Default for OccupancyConfig {
|
||||
Self {
|
||||
obstacle_distance: 0.1,
|
||||
known_distance: 2.0,
|
||||
use_winding_number: false,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -331,6 +331,30 @@ impl VisibilityGraph {
|
||||
pub fn edge_count(&self) -> usize {
|
||||
self.edges.iter().map(|e| e.len()).sum::<usize>() / 2
|
||||
}
|
||||
|
||||
/// Get the adjacency list of edges.
|
||||
///
|
||||
/// Returns a slice of edge lists, where each inner list contains
|
||||
/// (neighbor_index, distance) pairs for the corresponding node.
|
||||
pub fn edges(&self) -> &[Vec<(usize, f32)>] {
|
||||
&self.edges
|
||||
}
|
||||
|
||||
/// Get all unique edges as (from_node, to_node, distance) tuples.
|
||||
///
|
||||
/// Each edge is returned only once (not duplicated for both directions).
|
||||
pub fn edge_list(&self) -> Vec<(usize, usize, f32)> {
|
||||
let mut edges = Vec::new();
|
||||
for (from_idx, neighbors) in self.edges.iter().enumerate() {
|
||||
for &(to_idx, dist) in neighbors {
|
||||
// Only include each edge once (from < to)
|
||||
if from_idx < to_idx {
|
||||
edges.push((from_idx, to_idx, dist));
|
||||
}
|
||||
}
|
||||
}
|
||||
edges
|
||||
}
|
||||
}
|
||||
|
||||
/// State for Dijkstra's algorithm priority queue.
|
||||
@@ -418,6 +442,14 @@ impl PathPlanner {
|
||||
}
|
||||
}
|
||||
|
||||
// Try direct path first - much faster than building full visibility graph
|
||||
if is_straight_path_clear(from, to, lines, self.config.robot_radius) {
|
||||
return Some(Path {
|
||||
points: vec![from, to],
|
||||
length: from.distance(to),
|
||||
});
|
||||
}
|
||||
|
||||
// Build visibility graph
|
||||
let graph = VisibilityGraph::build(
|
||||
lines,
|
||||
@@ -451,6 +483,30 @@ impl PathPlanner {
|
||||
pub fn config(&self) -> &PathPlanningConfig {
|
||||
&self.config
|
||||
}
|
||||
|
||||
/// Build a visibility graph for visualization.
|
||||
///
|
||||
/// This builds the full visibility graph without planning a path,
|
||||
/// useful for debugging and visualization purposes.
|
||||
///
|
||||
/// # Arguments
|
||||
///
|
||||
/// * `from` - Start position
|
||||
/// * `to` - Goal position
|
||||
/// * `lines` - Map lines (obstacles)
|
||||
///
|
||||
/// # Returns
|
||||
///
|
||||
/// The visibility graph with all nodes and edges.
|
||||
pub fn build_graph(&self, from: Point2D, to: Point2D, lines: &[Line2D]) -> VisibilityGraph {
|
||||
VisibilityGraph::build(
|
||||
lines,
|
||||
from,
|
||||
to,
|
||||
self.config.robot_radius,
|
||||
self.config.max_nodes,
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
/// Check if a straight-line path is clear of obstacles.
|
||||
|
||||
@@ -814,7 +814,12 @@ impl Map for VectorMap {
|
||||
}
|
||||
|
||||
fn frontiers(&self) -> Vec<Frontier> {
|
||||
detect_frontiers(self.line_store.lines(), &self.config.frontier)
|
||||
// Use current robot pose for distance-based filtering
|
||||
detect_frontiers(
|
||||
self.line_store.lines(),
|
||||
self.current_pose.position(),
|
||||
&self.config.frontier,
|
||||
)
|
||||
}
|
||||
|
||||
fn get_path(&self, from: Point2D, to: Point2D) -> Option<Path> {
|
||||
|
||||
@@ -250,11 +250,16 @@ merger:
|
||||
frontier:
|
||||
connection_distance: 0.3
|
||||
min_distance_from_robot: 0.5
|
||||
min_openness: 0.3
|
||||
safety_margin: 0.4
|
||||
gap_probe_distance: 2.0
|
||||
robot_radius: 0.15
|
||||
max_viewpoint_iterations: 5
|
||||
viewpoint_step: 0.1
|
||||
min_gap_width: 0.4
|
||||
max_gap_width: 10.0
|
||||
occupancy:
|
||||
obstacle_distance: 0.1
|
||||
known_distance: 2.0
|
||||
use_winding_number: false
|
||||
path_planning:
|
||||
robot_radius: 0.15
|
||||
max_nodes: 1000
|
||||
@@ -368,11 +373,16 @@ merger:
|
||||
frontier:
|
||||
connection_distance: 0.3
|
||||
min_distance_from_robot: 0.5
|
||||
min_openness: 0.3
|
||||
safety_margin: 0.4
|
||||
gap_probe_distance: 2.0
|
||||
robot_radius: 0.15
|
||||
max_viewpoint_iterations: 5
|
||||
viewpoint_step: 0.1
|
||||
min_gap_width: 0.4
|
||||
max_gap_width: 10.0
|
||||
occupancy:
|
||||
obstacle_distance: 0.1
|
||||
known_distance: 2.0
|
||||
use_winding_number: false
|
||||
path_planning:
|
||||
robot_radius: 0.15
|
||||
max_nodes: 1000
|
||||
|
||||
@@ -32,8 +32,8 @@ use vastu_map::core::Pose2D;
|
||||
use vastu_map::core::math::normalize_angle;
|
||||
use vastu_map::integration::{ScanStore, ScanStoreConfig};
|
||||
use vastu_map::odometry::WheelOdometry;
|
||||
use vastu_map::query::PathPlanningConfig;
|
||||
use vastu_map::{Map, VectorMap, VectorMapConfig};
|
||||
use vastu_map::query::{PathPlanningConfig, VisibilityGraph};
|
||||
use vastu_map::{Map, Path as PlannedPath, VectorMap, VectorMapConfig};
|
||||
|
||||
use crate::adapters::lidar_to_point_cloud;
|
||||
use crate::metrics::{ConvergenceStats, TestMetrics, TimingStats};
|
||||
@@ -631,6 +631,59 @@ impl TestHarness {
|
||||
}
|
||||
}
|
||||
|
||||
/// Finalize the test with visibility graph visualization.
|
||||
///
|
||||
/// Extended version of `finalize` that also renders the visibility graph
|
||||
/// and planned path in the SVG output.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `graph` - Optional visibility graph to visualize
|
||||
/// * `path` - Optional planned path to visualize
|
||||
pub fn finalize_with_graph(
|
||||
&mut self,
|
||||
graph: Option<&VisibilityGraph>,
|
||||
path: Option<&PlannedPath>,
|
||||
) -> TestResult {
|
||||
let final_pose = self.slam.current_pose();
|
||||
let ground_truth_pose =
|
||||
Pose2D::new(self.physics.x(), self.physics.y(), self.physics.theta());
|
||||
|
||||
// Compute metrics using the simulation map, including convergence stats
|
||||
let metrics = TestMetrics::compute(
|
||||
&self.slam,
|
||||
&self.map,
|
||||
final_pose,
|
||||
ground_truth_pose,
|
||||
self.convergence_stats.clone(),
|
||||
);
|
||||
|
||||
// Generate enhanced visualization with graph if enabled
|
||||
if let Some(ref viz) = self.visualizer {
|
||||
viz.render_with_path_planning(
|
||||
&self.slam,
|
||||
&self.map,
|
||||
final_pose,
|
||||
ground_truth_pose,
|
||||
&self.scan_store,
|
||||
&self.trajectory,
|
||||
&metrics,
|
||||
graph,
|
||||
path,
|
||||
);
|
||||
}
|
||||
|
||||
// Print timing summary
|
||||
self.timing_stats.print_summary();
|
||||
|
||||
TestResult {
|
||||
metrics,
|
||||
final_pose,
|
||||
ground_truth_pose,
|
||||
observations: self.observations,
|
||||
sim_time: self.sim_time,
|
||||
}
|
||||
}
|
||||
|
||||
/// Get current SLAM map reference.
|
||||
pub fn slam(&self) -> &VectorMap {
|
||||
&self.slam
|
||||
|
||||
@@ -26,26 +26,19 @@
|
||||
|
||||
use crate::{HarnessConfig, TestHarness};
|
||||
use vastu_map::Map;
|
||||
use vastu_map::Path as PlannedPath;
|
||||
use vastu_map::config::ExplorationConfig as MapExplorationConfig;
|
||||
use vastu_map::core::Point2D;
|
||||
use vastu_map::exploration::{CollisionEvent, CollisionType, ExplorationConfig, VelocityCommand};
|
||||
use vastu_map::core::{Point2D, Pose2D};
|
||||
use vastu_map::exploration::{
|
||||
CollisionEvent, CollisionType, ExplorationConfig, ExplorationController, ExplorationState,
|
||||
VelocityCommand,
|
||||
};
|
||||
use vastu_map::integration::CoplanarMergeConfig;
|
||||
|
||||
/// Maximum exploration cycles before timeout.
|
||||
const MAX_CYCLES: usize = 200;
|
||||
use vastu_map::query::{PathPlanner, PathPlanningConfig, VisibilityGraph};
|
||||
|
||||
/// Maximum simulation time in seconds.
|
||||
const MAX_TIME: f32 = 200.0; // 3.3 minutes
|
||||
|
||||
/// How long to wait for map update (in simulation seconds).
|
||||
const MAP_UPDATE_WAIT: f32 = 0.4;
|
||||
|
||||
/// Distance threshold to consider "arrived" at frontier (meters).
|
||||
const ARRIVAL_THRESHOLD: f32 = 0.4;
|
||||
|
||||
/// Maximum attempts to reach the same frontier before marking unreachable.
|
||||
const MAX_FRONTIER_ATTEMPTS: usize = 2;
|
||||
|
||||
/// Result of exploration test.
|
||||
#[derive(Debug)]
|
||||
pub struct ExplorationTestResult {
|
||||
@@ -133,17 +126,22 @@ mod tests {
|
||||
|
||||
// Assertions for exploration quality
|
||||
// 1. Map should have reasonable number of lines (room walls + obstacles)
|
||||
// Note: line optimization can merge many lines, so threshold is conservative
|
||||
assert!(
|
||||
result.map_lines >= 15,
|
||||
"Map has too few lines: {} (expected >= 15 for room + obstacles)",
|
||||
result.map_lines >= 10,
|
||||
"Map has too few lines: {} (expected >= 10 for room + obstacles)",
|
||||
result.map_lines
|
||||
);
|
||||
|
||||
// 2. Robot should have explored for multiple cycles
|
||||
// 2. Robot should have explored significant number of frontiers
|
||||
// (better metric than cycles, since nearby frontiers don't count as new cycles)
|
||||
let frontiers_explored = result
|
||||
.initial_frontiers
|
||||
.saturating_sub(result.remaining_frontiers);
|
||||
assert!(
|
||||
result.cycles >= 5,
|
||||
"Robot did not explore enough: only {} cycles (expected >= 5)",
|
||||
result.cycles
|
||||
frontiers_explored >= 8 || result.completed,
|
||||
"Robot did not explore enough: only {} frontiers explored (expected >= 8)",
|
||||
frontiers_explored
|
||||
);
|
||||
|
||||
// 3. Virtual walls should be added when collisions happen
|
||||
@@ -163,7 +161,10 @@ mod tests {
|
||||
);
|
||||
}
|
||||
|
||||
/// Run the exploration loop with stop-scan-move pattern.
|
||||
/// Run the exploration loop using the ExplorationController with region-based TSP.
|
||||
///
|
||||
/// This uses a continuous control loop where the controller is called every step,
|
||||
/// properly integrating with its state machine for path following and collision handling.
|
||||
fn run_exploration_loop(
|
||||
harness: &mut TestHarness,
|
||||
config: ExplorationConfig,
|
||||
@@ -171,18 +172,15 @@ mod tests {
|
||||
let mut cycles = 0;
|
||||
let mut collisions = 0;
|
||||
let mut virtual_walls = 0;
|
||||
let mut unreachable_frontiers: Vec<Point2D> = Vec::new();
|
||||
// Track attempts per frontier location (using grid cells)
|
||||
let mut frontier_attempts: std::collections::HashMap<(i32, i32), usize> =
|
||||
std::collections::HashMap::new();
|
||||
const FRONTIER_MATCH_DIST: f32 = 0.5;
|
||||
const GRID_CELL_SIZE: f32 = 0.5;
|
||||
|
||||
fn point_to_cell(p: Point2D) -> (i32, i32) {
|
||||
((p.x / GRID_CELL_SIZE) as i32, (p.y / GRID_CELL_SIZE) as i32)
|
||||
}
|
||||
// Variables to capture the last visibility graph and path for visualization
|
||||
let mut last_visibility_graph: Option<VisibilityGraph> = None;
|
||||
let mut last_planned_path: Option<PlannedPath> = None;
|
||||
|
||||
println!("Starting exploration with path planning...");
|
||||
println!("Starting exploration with region-based TSP planning...");
|
||||
|
||||
// Create the exploration controller with region-based algorithm
|
||||
let mut controller = ExplorationController::new(config.clone());
|
||||
|
||||
// Phase 1: Initial map update (longer for initial observation)
|
||||
wait_for_map_update(harness, 2.0);
|
||||
@@ -190,159 +188,170 @@ mod tests {
|
||||
let initial_frontiers = harness.slam().frontiers().len();
|
||||
println!("Initial frontiers: {}", initial_frontiers);
|
||||
|
||||
while cycles < MAX_CYCLES && harness.sim_time() < MAX_TIME {
|
||||
cycles += 1;
|
||||
// Start exploration
|
||||
controller.start();
|
||||
|
||||
// Step 1: Get frontiers from updated map, excluding unreachable ones
|
||||
let frontiers = harness.slam().frontiers();
|
||||
let reachable_frontiers: Vec<_> = frontiers
|
||||
.iter()
|
||||
.filter(|f| {
|
||||
let cell = point_to_cell(f.point);
|
||||
let attempts = frontier_attempts.get(&cell).copied().unwrap_or(0);
|
||||
attempts < MAX_FRONTIER_ATTEMPTS
|
||||
&& !unreachable_frontiers
|
||||
.iter()
|
||||
.any(|u| u.distance(f.point) < FRONTIER_MATCH_DIST)
|
||||
})
|
||||
.collect();
|
||||
let mut step_count = 0;
|
||||
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 LIDAR_INTERVAL: usize = 5; // Process lidar every N steps
|
||||
|
||||
if reachable_frontiers.is_empty() {
|
||||
if frontiers.is_empty() {
|
||||
println!(
|
||||
" Cycle {}: No frontiers remain - exploration complete!",
|
||||
cycles
|
||||
);
|
||||
} else {
|
||||
println!(
|
||||
" Cycle {}: All {} frontiers unreachable - done",
|
||||
cycles,
|
||||
frontiers.len()
|
||||
);
|
||||
while step_count < MAX_STEPS && harness.sim_time() < MAX_TIME {
|
||||
step_count += 1;
|
||||
|
||||
// Get current pose
|
||||
let physics_pose = harness.physics_pose();
|
||||
let current_pose = Pose2D::new(physics_pose.x, physics_pose.y, physics_pose.theta);
|
||||
|
||||
// Update controller with any pending collision
|
||||
let step = controller.update(harness.slam(), current_pose, pending_collision.take());
|
||||
|
||||
// Check completion states
|
||||
match &step.state {
|
||||
ExplorationState::Complete => {
|
||||
println!(" Exploration complete - no frontiers remain!");
|
||||
break;
|
||||
}
|
||||
break;
|
||||
}
|
||||
ExplorationState::Failed { reason } => {
|
||||
println!(" Exploration failed - {}", reason);
|
||||
break;
|
||||
}
|
||||
ExplorationState::Idle => {
|
||||
break;
|
||||
}
|
||||
ExplorationState::SelectingFrontier => {
|
||||
// Step simulation forward while selecting
|
||||
harness.step(0.0, 0.0);
|
||||
// Process lidar while selecting
|
||||
if step_count % LIDAR_INTERVAL == 0 {
|
||||
harness.process_lidar();
|
||||
}
|
||||
}
|
||||
ExplorationState::FollowingPath {
|
||||
target_frontier,
|
||||
path,
|
||||
..
|
||||
} => {
|
||||
// Track cycles (new target = new cycle)
|
||||
let is_new_target = last_target
|
||||
.map(|last| last.distance(*target_frontier) > 0.3)
|
||||
.unwrap_or(true);
|
||||
|
||||
// Step 2: Find best frontier with path planning
|
||||
let robot_pos = harness.physics_pose().position();
|
||||
if is_new_target {
|
||||
cycles += 1;
|
||||
last_target = Some(*target_frontier);
|
||||
|
||||
// Sort by distance, preferring frontiers with fewer attempts
|
||||
let mut sorted_frontiers: Vec<_> = reachable_frontiers
|
||||
.iter()
|
||||
.map(|f| {
|
||||
let cell = point_to_cell(f.point);
|
||||
let attempts = frontier_attempts.get(&cell).copied().unwrap_or(0);
|
||||
let distance = robot_pos.distance(f.point);
|
||||
// Score: lower is better, penalize retries
|
||||
let score = distance + (attempts as f32) * 0.5;
|
||||
(f.point, distance, score)
|
||||
})
|
||||
.collect();
|
||||
sorted_frontiers.sort_by(|a, b| a.2.partial_cmp(&b.2).unwrap());
|
||||
|
||||
let mut selected_target: Option<(Point2D, vastu_map::Path)> = None;
|
||||
|
||||
for (target, distance, _) in &sorted_frontiers {
|
||||
// Try to plan a path to this frontier
|
||||
if let Some(path) = harness.slam().get_path(robot_pos, *target) {
|
||||
if path.points.len() >= 2 && path.length < distance * 3.0 {
|
||||
// Accept path if it's not too much longer than direct distance
|
||||
let robot_pos = current_pose.position();
|
||||
println!(
|
||||
" Cycle {}: Path to ({:.2}, {:.2}), dist: {:.2}m, path: {:.2}m, {} waypoints",
|
||||
cycles,
|
||||
target.x,
|
||||
target.y,
|
||||
distance,
|
||||
target_frontier.x,
|
||||
target_frontier.y,
|
||||
robot_pos.distance(*target_frontier),
|
||||
path.length,
|
||||
path.points.len()
|
||||
);
|
||||
selected_target = Some((*target, path));
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
let (target, path) = match selected_target {
|
||||
Some(t) => t,
|
||||
None => {
|
||||
// No path found, try direct approach to nearest unexplored
|
||||
let nearest = sorted_frontiers.first().map(|(p, _, _)| *p);
|
||||
if let Some(target) = nearest {
|
||||
println!(
|
||||
" Cycle {}: No path, direct to ({:.2}, {:.2})",
|
||||
cycles, target.x, target.y
|
||||
// Capture visibility graph for this path for visualization
|
||||
let path_config = PathPlanningConfig::default()
|
||||
.with_robot_radius(config.robot_radius)
|
||||
.with_conservative(false);
|
||||
let planner = PathPlanner::new(path_config);
|
||||
last_visibility_graph = Some(planner.build_graph(
|
||||
robot_pos,
|
||||
*target_frontier,
|
||||
harness.slam().lines(),
|
||||
));
|
||||
last_planned_path = Some(path.clone());
|
||||
}
|
||||
|
||||
// Execute velocity command from controller (or stop if none)
|
||||
let vel = step.velocity.unwrap_or(VelocityCommand::new(0.0, 0.0));
|
||||
let step_result = harness.step(vel.linear, vel.angular);
|
||||
|
||||
// Check for collision
|
||||
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(),
|
||||
);
|
||||
let direct_path = vastu_map::Path {
|
||||
points: vec![robot_pos, target],
|
||||
length: robot_pos.distance(target),
|
||||
};
|
||||
(target, direct_path)
|
||||
} else {
|
||||
break;
|
||||
|
||||
// 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!(
|
||||
" Collision at ({:.2}, {:.2}) - added virtual wall",
|
||||
point.x, point.y
|
||||
);
|
||||
}
|
||||
|
||||
// Process lidar periodically
|
||||
if step_count % LIDAR_INTERVAL == 0 {
|
||||
harness.process_lidar();
|
||||
}
|
||||
|
||||
// Log progress periodically
|
||||
if cycles > last_cycle_print && step_count % 100 == 0 {
|
||||
let end_pos = harness.physics_pose().position();
|
||||
let current_frontiers = harness.slam().frontiers().len();
|
||||
println!(
|
||||
" Position: ({:.2}, {:.2}), frontiers: {}, unreachable: {}",
|
||||
end_pos.x,
|
||||
end_pos.y,
|
||||
current_frontiers,
|
||||
controller.unreachable_frontier_count()
|
||||
);
|
||||
last_cycle_print = cycles;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
// Track attempt for this frontier location
|
||||
let target_cell = point_to_cell(target);
|
||||
*frontier_attempts.entry(target_cell).or_insert(0) += 1;
|
||||
|
||||
let start_distance = robot_pos.distance(target);
|
||||
|
||||
// Step 3: Follow the path
|
||||
let (collision_count, wall_count, reached) =
|
||||
follow_path(harness, &config, &path, ARRIVAL_THRESHOLD);
|
||||
collisions += collision_count;
|
||||
virtual_walls += wall_count;
|
||||
|
||||
// Check result
|
||||
let end_pos = harness.physics_pose().position();
|
||||
let final_distance = end_pos.distance(target);
|
||||
|
||||
if reached {
|
||||
// Successfully reached frontier - frontier should disappear
|
||||
// after map update
|
||||
} else if final_distance < start_distance * 0.5 {
|
||||
// Made significant progress even if didn't fully reach
|
||||
// Don't mark as unreachable, just continue
|
||||
} else if collision_count > 0 {
|
||||
// Had collision, will replan with virtual wall
|
||||
} else {
|
||||
// Got stuck without collision and didn't make progress
|
||||
// Check if we've tried too many times
|
||||
let attempts = frontier_attempts.get(&target_cell).copied().unwrap_or(0);
|
||||
if attempts >= MAX_FRONTIER_ATTEMPTS {
|
||||
println!(
|
||||
" Too many attempts at ({:.2}, {:.2}), marking unreachable",
|
||||
target.x, target.y
|
||||
);
|
||||
unreachable_frontiers.push(target);
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
// Step 4: Wait for map update after moving
|
||||
wait_for_map_update(harness, MAP_UPDATE_WAIT);
|
||||
|
||||
// Log progress
|
||||
let current_frontiers = harness.slam().frontiers().len();
|
||||
println!(
|
||||
" Position: ({:.2}, {:.2}), frontiers: {}, unreachable: {}",
|
||||
end_pos.x,
|
||||
end_pos.y,
|
||||
current_frontiers,
|
||||
unreachable_frontiers.len()
|
||||
);
|
||||
}
|
||||
|
||||
// Final log
|
||||
let end_pos = harness.physics_pose().position();
|
||||
let current_frontiers = harness.slam().frontiers().len();
|
||||
println!(
|
||||
" Final position: ({:.2}, {:.2}), frontiers: {}, unreachable: {}",
|
||||
end_pos.x,
|
||||
end_pos.y,
|
||||
current_frontiers,
|
||||
controller.unreachable_frontier_count()
|
||||
);
|
||||
|
||||
// Get line count before optimization
|
||||
let map_lines_before = harness.slam().lines().len();
|
||||
|
||||
// Optimize coplanar lines with tight thresholds:
|
||||
// - max_angle_diff: 1 degree = 0.01745 rad
|
||||
// - max_perpendicular_dist: 2cm = 0.02m
|
||||
// Optimize coplanar lines with tight thresholds
|
||||
let coplanar_config = CoplanarMergeConfig::default()
|
||||
.with_max_angle_diff(3.0 * std::f32::consts::PI / 180.0) // 1 degree
|
||||
.with_max_perpendicular_dist(0.05); // 2cm
|
||||
.with_max_angle_diff(3.0 * std::f32::consts::PI / 180.0)
|
||||
.with_max_perpendicular_dist(0.05);
|
||||
|
||||
let lines_merged = harness.slam_mut().optimize_lines(&coplanar_config);
|
||||
|
||||
@@ -353,8 +362,9 @@ mod tests {
|
||||
lines_merged
|
||||
);
|
||||
|
||||
// Finalize to generate visualization
|
||||
let _test_result = harness.finalize();
|
||||
// Finalize to generate visualization with visibility graph
|
||||
let _test_result =
|
||||
harness.finalize_with_graph(last_visibility_graph.as_ref(), last_planned_path.as_ref());
|
||||
|
||||
ExplorationTestResult {
|
||||
completed: harness.slam().frontiers().is_empty(),
|
||||
@@ -381,186 +391,4 @@ mod tests {
|
||||
harness.process_lidar();
|
||||
}
|
||||
}
|
||||
|
||||
/// Follow a planned path through waypoints.
|
||||
/// Returns (collision_count, virtual_wall_count, reached_goal).
|
||||
fn follow_path(
|
||||
harness: &mut TestHarness,
|
||||
config: &ExplorationConfig,
|
||||
path: &vastu_map::Path,
|
||||
arrival_threshold: f32,
|
||||
) -> (usize, usize, bool) {
|
||||
let mut collisions = 0;
|
||||
let mut virtual_walls = 0;
|
||||
let mut steps = 0;
|
||||
const MAX_STEPS: usize = 500; // Prevent infinite loops
|
||||
|
||||
if path.points.len() < 2 {
|
||||
return (0, 0, false);
|
||||
}
|
||||
|
||||
let mut waypoint_idx = 1; // Start from first waypoint (skip start position)
|
||||
let goal = path.points.last().copied().unwrap();
|
||||
let start_pos = harness.physics_pose().position();
|
||||
let initial_distance = start_pos.distance(goal);
|
||||
let mut last_progress_step = 0;
|
||||
let mut best_distance_to_goal = initial_distance;
|
||||
|
||||
// For short paths, allow less strict progress requirements
|
||||
let progress_threshold = (initial_distance * 0.05).max(0.03);
|
||||
|
||||
while steps < MAX_STEPS && harness.sim_time() < MAX_TIME && waypoint_idx < path.points.len()
|
||||
{
|
||||
let pose = harness.physics_pose();
|
||||
let robot_pos = pose.position();
|
||||
let current_waypoint = path.points[waypoint_idx];
|
||||
let distance_to_waypoint = robot_pos.distance(current_waypoint);
|
||||
let distance_to_goal = robot_pos.distance(goal);
|
||||
|
||||
// Check if reached goal (can skip intermediate waypoints)
|
||||
if distance_to_goal < arrival_threshold {
|
||||
return (collisions, virtual_walls, true);
|
||||
}
|
||||
|
||||
// Check if reached current waypoint
|
||||
if distance_to_waypoint < arrival_threshold {
|
||||
waypoint_idx += 1;
|
||||
if waypoint_idx >= path.points.len() {
|
||||
return (collisions, virtual_walls, true);
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
// Track progress towards goal
|
||||
if distance_to_goal < best_distance_to_goal - progress_threshold {
|
||||
best_distance_to_goal = distance_to_goal;
|
||||
last_progress_step = steps;
|
||||
}
|
||||
|
||||
// Check if stuck (no progress for too long)
|
||||
// Allow more time for short paths
|
||||
let max_stuck_steps = if initial_distance < 1.0 { 200 } else { 150 };
|
||||
if steps - last_progress_step > max_stuck_steps {
|
||||
// Not making progress, abort
|
||||
return (collisions, virtual_walls, false);
|
||||
}
|
||||
|
||||
// Compute velocity towards waypoint
|
||||
let velocity = compute_velocity_to_waypoint(pose, current_waypoint, config);
|
||||
|
||||
// Execute movement
|
||||
let step_result = harness.step(velocity.linear, velocity.angular);
|
||||
steps += 1;
|
||||
|
||||
// Handle collision
|
||||
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,
|
||||
_ => continue,
|
||||
};
|
||||
|
||||
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(),
|
||||
);
|
||||
|
||||
// Add virtual wall
|
||||
let collision = CollisionEvent::new(collision_type, point, theta);
|
||||
let wall = collision.to_virtual_wall(config.virtual_wall_length);
|
||||
harness.slam_mut().add_line(wall.line);
|
||||
virtual_walls += 1;
|
||||
|
||||
println!(
|
||||
" Collision at ({:.2}, {:.2}) - added virtual wall",
|
||||
point.x, point.y
|
||||
);
|
||||
|
||||
// Back off after collision
|
||||
backoff(harness, config.backoff_distance, theta);
|
||||
|
||||
// Abort path following - need to replan
|
||||
return (collisions, virtual_walls, false);
|
||||
}
|
||||
|
||||
// Process lidar periodically
|
||||
if steps % 5 == 0 {
|
||||
harness.process_lidar();
|
||||
}
|
||||
}
|
||||
|
||||
// Check if we reached the goal
|
||||
let final_distance = harness.physics_pose().position().distance(goal);
|
||||
(
|
||||
collisions,
|
||||
virtual_walls,
|
||||
final_distance < arrival_threshold,
|
||||
)
|
||||
}
|
||||
|
||||
/// Compute velocity command to move towards a waypoint.
|
||||
fn compute_velocity_to_waypoint(
|
||||
pose: vastu_map::core::Pose2D,
|
||||
waypoint: Point2D,
|
||||
config: &ExplorationConfig,
|
||||
) -> VelocityCommand {
|
||||
// Transform waypoint to robot frame
|
||||
let local_target = pose.inverse_transform_point(waypoint);
|
||||
|
||||
// Angle to waypoint (in robot frame, forward is +X)
|
||||
let angle_to_target = local_target.y.atan2(local_target.x);
|
||||
let distance = local_target.length();
|
||||
|
||||
// Angular velocity: proportional to angle error
|
||||
let angular =
|
||||
(angle_to_target * 2.5).clamp(-config.max_angular_speed, config.max_angular_speed);
|
||||
|
||||
// Linear velocity depends on alignment with target
|
||||
let alignment = angle_to_target.abs().cos(); // 1.0 when facing target, 0 when perpendicular
|
||||
|
||||
// Base linear velocity proportional to distance
|
||||
let base_linear = (distance * 0.5).min(config.max_linear_speed);
|
||||
|
||||
// Reduce speed when not well aligned, but always maintain some forward motion
|
||||
// if we're close enough that turning alone won't help
|
||||
let linear = if angle_to_target.abs() > 1.0 {
|
||||
// Very misaligned - turn in place
|
||||
0.0
|
||||
} else if angle_to_target.abs() > 0.3 {
|
||||
// Somewhat misaligned - slow forward while turning
|
||||
base_linear * alignment * 0.3
|
||||
} else {
|
||||
// Well aligned - full speed
|
||||
base_linear * alignment
|
||||
};
|
||||
|
||||
// Ensure minimum speed when close to target
|
||||
let linear = if distance < 0.5 && linear < 0.05 && angle_to_target.abs() < 0.5 {
|
||||
0.1
|
||||
} else {
|
||||
linear
|
||||
};
|
||||
|
||||
VelocityCommand::new(linear, angular)
|
||||
}
|
||||
|
||||
/// Back off after collision.
|
||||
fn backoff(harness: &mut TestHarness, distance: f32, _heading: f32) {
|
||||
let start_pos = harness.physics_pose().position();
|
||||
let backup_speed = -0.15; // Reverse
|
||||
let mut steps = 0;
|
||||
const MAX_BACKUP_STEPS: usize = 50;
|
||||
|
||||
while harness.physics_pose().position().distance(start_pos) < distance
|
||||
&& steps < MAX_BACKUP_STEPS
|
||||
{
|
||||
harness.step(backup_speed, 0.0);
|
||||
steps += 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -16,9 +16,11 @@ use svg::node::element::{
|
||||
Circle, Definitions, Group, Line, Marker, Path, Polyline, Rectangle, Text,
|
||||
};
|
||||
|
||||
use vastu_map::Path as PlannedPath;
|
||||
use vastu_map::VectorMap;
|
||||
use vastu_map::core::{Point2D, Pose2D};
|
||||
use vastu_map::integration::ScanStore;
|
||||
use vastu_map::query::VisibilityGraph;
|
||||
|
||||
use crate::harness::TrajectoryHistory;
|
||||
use crate::metrics::TestMetrics;
|
||||
@@ -39,6 +41,16 @@ mod colors {
|
||||
pub const LIDAR_SCAN: &str = "#56B4E9";
|
||||
/// Lidar scan history - lighter, semi-transparent
|
||||
pub const LIDAR_SCAN_HISTORY: &str = "#56B4E9";
|
||||
/// Visibility graph nodes - purple
|
||||
pub const GRAPH_NODE: &str = "#CC79A7";
|
||||
/// Visibility graph edges - light purple
|
||||
pub const GRAPH_EDGE: &str = "#CC79A7";
|
||||
/// Planned path - green
|
||||
pub const PLANNED_PATH: &str = "#009E73";
|
||||
/// Start node - green
|
||||
pub const START_NODE: &str = "#00FF00";
|
||||
/// Goal node - red
|
||||
pub const GOAL_NODE: &str = "#FF0000";
|
||||
}
|
||||
|
||||
/// Interval for displaying drift vectors (every Nth observation).
|
||||
@@ -349,6 +361,8 @@ impl Visualizer {
|
||||
(colors::TRUTH_PATH, "Ground Truth Path", "line"),
|
||||
("gradient", "Estimated Path (Confidence)", "gradient"),
|
||||
(colors::DRIFT_VECTOR, "Drift Vector", "arrow"),
|
||||
(colors::GRAPH_NODE, "Graph Nodes", "circle"),
|
||||
(colors::PLANNED_PATH, "Planned Path", "line"),
|
||||
];
|
||||
|
||||
for (color, label, icon_type) in items {
|
||||
@@ -640,6 +654,288 @@ impl Visualizer {
|
||||
|
||||
Group::new().add(body).add(direction)
|
||||
}
|
||||
|
||||
/// Render a visibility graph showing nodes and edges.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `graph` - The visibility graph to render
|
||||
/// * `svg_height` - SVG canvas height for coordinate transformation
|
||||
/// * `show_edges` - Whether to show edges (can be noisy for large graphs)
|
||||
pub fn render_visibility_graph(
|
||||
&self,
|
||||
graph: &VisibilityGraph,
|
||||
svg_height: f32,
|
||||
show_edges: bool,
|
||||
) -> Group {
|
||||
let mut group = Group::new().set("id", "visibility_graph");
|
||||
|
||||
let nodes = graph.nodes();
|
||||
let start_idx = graph.start_idx();
|
||||
let goal_idx = graph.goal_idx();
|
||||
|
||||
// Render edges first (so nodes appear on top)
|
||||
if show_edges {
|
||||
let edge_group = self.render_graph_edges(graph, svg_height);
|
||||
group = group.add(edge_group);
|
||||
}
|
||||
|
||||
// Render nodes
|
||||
for (idx, &node) in nodes.iter().enumerate() {
|
||||
let (sx, sy) = self.transform_point(node, 0.0, 0.0, svg_height);
|
||||
|
||||
// Choose color based on node type
|
||||
let (color, radius, opacity) = if idx == start_idx {
|
||||
(colors::START_NODE, 6.0, 0.9)
|
||||
} else if idx == goal_idx {
|
||||
(colors::GOAL_NODE, 6.0, 0.9)
|
||||
} else {
|
||||
(colors::GRAPH_NODE, 4.0, 0.7)
|
||||
};
|
||||
|
||||
let circle = Circle::new()
|
||||
.set("cx", sx)
|
||||
.set("cy", sy)
|
||||
.set("r", radius)
|
||||
.set("fill", color)
|
||||
.set("opacity", opacity);
|
||||
|
||||
group = group.add(circle);
|
||||
}
|
||||
|
||||
group
|
||||
}
|
||||
|
||||
/// Render visibility graph edges.
|
||||
fn render_graph_edges(&self, graph: &VisibilityGraph, svg_height: f32) -> Group {
|
||||
let mut group = Group::new().set("id", "graph_edges");
|
||||
|
||||
let nodes = graph.nodes();
|
||||
let edge_list = graph.edge_list();
|
||||
|
||||
for (from_idx, to_idx, _dist) in edge_list {
|
||||
let from_node = nodes[from_idx];
|
||||
let to_node = nodes[to_idx];
|
||||
|
||||
let line = self.create_line(
|
||||
from_node,
|
||||
to_node,
|
||||
0.0,
|
||||
0.0,
|
||||
svg_height,
|
||||
colors::GRAPH_EDGE,
|
||||
0.5,
|
||||
Some("2,2"), // Dashed line for edges
|
||||
);
|
||||
|
||||
group = group.add(line.set("opacity", 0.3));
|
||||
}
|
||||
|
||||
group
|
||||
}
|
||||
|
||||
/// Render a planned path.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `path` - The planned path to render
|
||||
/// * `svg_height` - SVG canvas height for coordinate transformation
|
||||
pub fn render_planned_path(&self, path: &PlannedPath, svg_height: f32) -> Group {
|
||||
let mut group = Group::new().set("id", "planned_path");
|
||||
|
||||
if path.points.len() < 2 {
|
||||
return group;
|
||||
}
|
||||
|
||||
// Render path as connected line segments
|
||||
for i in 1..path.points.len() {
|
||||
let from = path.points[i - 1];
|
||||
let to = path.points[i];
|
||||
|
||||
let line = self.create_line(
|
||||
from,
|
||||
to,
|
||||
0.0,
|
||||
0.0,
|
||||
svg_height,
|
||||
colors::PLANNED_PATH,
|
||||
3.0,
|
||||
None,
|
||||
);
|
||||
|
||||
group = group.add(line);
|
||||
}
|
||||
|
||||
// Render waypoint markers
|
||||
for (i, &point) in path.points.iter().enumerate() {
|
||||
let (sx, sy) = self.transform_point(point, 0.0, 0.0, svg_height);
|
||||
|
||||
// First and last points are start/goal, middle points are waypoints
|
||||
let (color, radius) = if i == 0 {
|
||||
(colors::START_NODE, 5.0)
|
||||
} else if i == path.points.len() - 1 {
|
||||
(colors::GOAL_NODE, 5.0)
|
||||
} else {
|
||||
(colors::PLANNED_PATH, 3.0)
|
||||
};
|
||||
|
||||
let circle = Circle::new()
|
||||
.set("cx", sx)
|
||||
.set("cy", sy)
|
||||
.set("r", radius)
|
||||
.set("fill", color)
|
||||
.set("stroke", "white")
|
||||
.set("stroke-width", 1.0);
|
||||
|
||||
group = group.add(circle);
|
||||
}
|
||||
|
||||
group
|
||||
}
|
||||
|
||||
/// Render full visualization with visibility graph.
|
||||
///
|
||||
/// Extended version of `render_full` that also shows the visibility graph
|
||||
/// and planned path for path planning debugging.
|
||||
#[allow(clippy::too_many_arguments)]
|
||||
pub fn render_with_path_planning(
|
||||
&self,
|
||||
slam: &VectorMap,
|
||||
map: &SimulationMap,
|
||||
final_pose: Pose2D,
|
||||
truth_pose: Pose2D,
|
||||
scan_store: &ScanStore,
|
||||
trajectory: &TrajectoryHistory,
|
||||
metrics: &TestMetrics,
|
||||
graph: Option<&VisibilityGraph>,
|
||||
path: Option<&PlannedPath>,
|
||||
) {
|
||||
// Compute bounds from map
|
||||
let map_width_m = map.width() as f32 * map.resolution();
|
||||
let map_height_m = map.height() as f32 * map.resolution();
|
||||
|
||||
let width = (map_width_m * self.scale + 2.0 * self.margin) as i32;
|
||||
let height = (map_height_m * self.scale + 2.0 * self.margin) as i32;
|
||||
|
||||
// Create document with arrow marker definition
|
||||
let mut doc = Document::new()
|
||||
.set("width", width)
|
||||
.set("height", height)
|
||||
.set("viewBox", (0, 0, width, height));
|
||||
|
||||
// Add marker definitions for arrows
|
||||
let defs = self.create_marker_definitions();
|
||||
doc = doc.add(defs);
|
||||
|
||||
// Layer 0: Background
|
||||
doc = doc.add(
|
||||
Rectangle::new()
|
||||
.set("x", 0)
|
||||
.set("y", 0)
|
||||
.set("width", width)
|
||||
.set("height", height)
|
||||
.set("fill", "white"),
|
||||
);
|
||||
|
||||
// Layer 1: Map walls (gray)
|
||||
doc = doc.add(self.render_map_walls(map, height as f32));
|
||||
|
||||
// Layer 2: All scans from scan store (semi-transparent)
|
||||
doc = doc.add(self.render_all_scans(scan_store, height as f32));
|
||||
|
||||
// Layer 3: Final lidar scan (full opacity) - use last scan from store
|
||||
if let Some(last_scan) = scan_store.latest() {
|
||||
let world_points = last_scan.world_points();
|
||||
doc = doc.add(self.render_world_points(&world_points, height as f32));
|
||||
}
|
||||
|
||||
// Layer 4: Detected walls (teal)
|
||||
doc = doc.add(self.render_detected(slam, height as f32));
|
||||
|
||||
// Layer 5: Visibility graph (if provided)
|
||||
if let Some(g) = graph {
|
||||
doc = doc.add(self.render_visibility_graph(g, height as f32, true));
|
||||
}
|
||||
|
||||
// Layer 6: Planned path (if provided)
|
||||
if let Some(p) = path {
|
||||
doc = doc.add(self.render_planned_path(p, height as f32));
|
||||
}
|
||||
|
||||
// Layer 7: Trajectories with confidence heatmap
|
||||
doc = doc.add(self.render_trajectories(trajectory, height as f32));
|
||||
|
||||
// Layer 8: Drift vectors
|
||||
doc = doc.add(self.render_drift_vectors(trajectory, height as f32));
|
||||
|
||||
// Layer 9: Final poses
|
||||
doc = doc.add(self.render_poses(final_pose, truth_pose, height as f32));
|
||||
|
||||
// Layer 10: Enhanced legend
|
||||
doc = doc.add(self.render_enhanced_legend(height as f32));
|
||||
|
||||
// Layer 11: Metrics overlay
|
||||
doc = doc.add(self.render_metrics_overlay(metrics, width));
|
||||
|
||||
// Ensure output directory exists
|
||||
if let Some(parent) = std::path::Path::new(&self.output_path).parent() {
|
||||
std::fs::create_dir_all(parent).ok();
|
||||
}
|
||||
|
||||
// Save
|
||||
svg::save(&self.output_path, &doc).expect("Failed to save SVG");
|
||||
}
|
||||
|
||||
/// Render standalone visibility graph visualization.
|
||||
///
|
||||
/// Creates a simple SVG showing just the visibility graph, detected walls,
|
||||
/// and optionally a planned path. Useful for path planning debugging.
|
||||
#[allow(dead_code)]
|
||||
pub fn render_graph_only(
|
||||
&self,
|
||||
slam: &VectorMap,
|
||||
graph: &VisibilityGraph,
|
||||
path: Option<&PlannedPath>,
|
||||
bounds: (f32, f32, f32, f32), // (min_x, min_y, max_x, max_y)
|
||||
) {
|
||||
let (min_x, min_y, max_x, max_y) = bounds;
|
||||
let map_width_m = max_x - min_x;
|
||||
let map_height_m = max_y - min_y;
|
||||
|
||||
let width = (map_width_m * self.scale + 2.0 * self.margin) as i32;
|
||||
let height = (map_height_m * self.scale + 2.0 * self.margin) as i32;
|
||||
|
||||
let mut doc = Document::new()
|
||||
.set("width", width)
|
||||
.set("height", height)
|
||||
.set("viewBox", (0, 0, width, height));
|
||||
|
||||
// Background
|
||||
doc = doc.add(
|
||||
Rectangle::new()
|
||||
.set("x", 0)
|
||||
.set("y", 0)
|
||||
.set("width", width)
|
||||
.set("height", height)
|
||||
.set("fill", "white"),
|
||||
);
|
||||
|
||||
// Detected walls
|
||||
doc = doc.add(self.render_detected(slam, height as f32));
|
||||
|
||||
// Visibility graph
|
||||
doc = doc.add(self.render_visibility_graph(graph, height as f32, true));
|
||||
|
||||
// Planned path (if provided)
|
||||
if let Some(p) = path {
|
||||
doc = doc.add(self.render_planned_path(p, height as f32));
|
||||
}
|
||||
|
||||
// Ensure output directory exists
|
||||
if let Some(parent) = std::path::Path::new(&self.output_path).parent() {
|
||||
std::fs::create_dir_all(parent).ok();
|
||||
}
|
||||
|
||||
svg::save(&self.output_path, &doc).expect("Failed to save SVG");
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
|
||||
@@ -0,0 +1,503 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Floor Plan to PGM Converter
|
||||
|
||||
Converts floor plan images (PNG, JPG, SVG, PDF) to ROS-compatible PGM+YAML maps
|
||||
for use with vastu-map SLAM testing.
|
||||
|
||||
Usage:
|
||||
python floorplan_converter.py input.png --width 10.0 --output apartment.pgm
|
||||
python floorplan_converter.py input.jpg --resolution 0.05 --output test_map.pgm
|
||||
python floorplan_converter.py input.png --width 15.0 --add-furniture --output furnished.pgm
|
||||
|
||||
Dependencies:
|
||||
pip install opencv-python numpy pillow pyyaml
|
||||
|
||||
Author: VacuumTiger Project
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import os
|
||||
import sys
|
||||
from pathlib import Path
|
||||
from typing import Tuple, Optional, List
|
||||
import yaml
|
||||
|
||||
try:
|
||||
import cv2
|
||||
import numpy as np
|
||||
from PIL import Image
|
||||
except ImportError as e:
|
||||
print(f"Missing dependency: {e}")
|
||||
print("Install with: pip install opencv-python numpy pillow pyyaml")
|
||||
sys.exit(1)
|
||||
|
||||
|
||||
class FloorPlanConverter:
|
||||
"""Convert floor plan images to ROS-compatible PGM+YAML format."""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
resolution: float = 0.05,
|
||||
wall_thickness: int = 2,
|
||||
occupied_thresh: float = 0.65,
|
||||
free_thresh: float = 0.196,
|
||||
invert_colors: bool = False,
|
||||
):
|
||||
"""
|
||||
Initialize converter.
|
||||
|
||||
Args:
|
||||
resolution: Meters per pixel (default 0.05m = 5cm)
|
||||
wall_thickness: Minimum wall thickness in pixels after processing
|
||||
occupied_thresh: Threshold for occupied space (0-1, normalized)
|
||||
free_thresh: Threshold for free space (0-1, normalized)
|
||||
invert_colors: If True, treat dark areas as free space
|
||||
"""
|
||||
self.resolution = resolution
|
||||
self.wall_thickness = wall_thickness
|
||||
self.occupied_thresh = occupied_thresh
|
||||
self.free_thresh = free_thresh
|
||||
self.invert_colors = invert_colors
|
||||
|
||||
def load_image(self, path: str) -> np.ndarray:
|
||||
"""Load image from various formats."""
|
||||
path = Path(path)
|
||||
|
||||
if not path.exists():
|
||||
raise FileNotFoundError(f"Image not found: {path}")
|
||||
|
||||
# Handle different formats
|
||||
suffix = path.suffix.lower()
|
||||
|
||||
if suffix in ['.png', '.jpg', '.jpeg', '.bmp', '.tiff', '.tif']:
|
||||
img = cv2.imread(str(path), cv2.IMREAD_GRAYSCALE)
|
||||
if img is None:
|
||||
raise ValueError(f"Failed to load image: {path}")
|
||||
return img
|
||||
|
||||
elif suffix == '.svg':
|
||||
# SVG requires cairosvg or similar
|
||||
try:
|
||||
import cairosvg
|
||||
from io import BytesIO
|
||||
png_data = cairosvg.svg2png(url=str(path))
|
||||
pil_img = Image.open(BytesIO(png_data)).convert('L')
|
||||
return np.array(pil_img)
|
||||
except ImportError:
|
||||
raise ImportError("SVG support requires: pip install cairosvg")
|
||||
|
||||
elif suffix == '.pdf':
|
||||
# PDF requires pdf2image
|
||||
try:
|
||||
from pdf2image import convert_from_path
|
||||
images = convert_from_path(str(path), dpi=300)
|
||||
if images:
|
||||
return np.array(images[0].convert('L'))
|
||||
raise ValueError("No pages in PDF")
|
||||
except ImportError:
|
||||
raise ImportError("PDF support requires: pip install pdf2image")
|
||||
|
||||
else:
|
||||
raise ValueError(f"Unsupported format: {suffix}")
|
||||
|
||||
def preprocess(self, img: np.ndarray) -> np.ndarray:
|
||||
"""Apply preprocessing to enhance wall detection."""
|
||||
# Apply Gaussian blur to reduce noise
|
||||
blurred = cv2.GaussianBlur(img, (5, 5), 0)
|
||||
|
||||
# Apply adaptive thresholding for better wall detection
|
||||
# This works well with varying lighting in floor plans
|
||||
binary = cv2.adaptiveThreshold(
|
||||
blurred,
|
||||
255,
|
||||
cv2.ADAPTIVE_THRESH_GAUSSIAN_C,
|
||||
cv2.THRESH_BINARY,
|
||||
blockSize=11,
|
||||
C=2
|
||||
)
|
||||
|
||||
return binary
|
||||
|
||||
def detect_walls_otsu(self, img: np.ndarray) -> np.ndarray:
|
||||
"""Detect walls using Otsu's thresholding."""
|
||||
blurred = cv2.GaussianBlur(img, (5, 5), 0)
|
||||
_, binary = cv2.threshold(blurred, 0, 255, cv2.THRESH_BINARY + cv2.THRESH_OTSU)
|
||||
return binary
|
||||
|
||||
def detect_walls_adaptive(self, img: np.ndarray) -> np.ndarray:
|
||||
"""Detect walls using adaptive thresholding."""
|
||||
blurred = cv2.GaussianBlur(img, (3, 3), 0)
|
||||
binary = cv2.adaptiveThreshold(
|
||||
blurred, 255,
|
||||
cv2.ADAPTIVE_THRESH_GAUSSIAN_C,
|
||||
cv2.THRESH_BINARY,
|
||||
blockSize=15,
|
||||
C=3
|
||||
)
|
||||
return binary
|
||||
|
||||
def detect_walls_canny(self, img: np.ndarray) -> np.ndarray:
|
||||
"""Detect walls using Canny edge detection + morphology."""
|
||||
# Edge detection
|
||||
edges = cv2.Canny(img, 50, 150)
|
||||
|
||||
# Dilate to connect nearby edges
|
||||
kernel = np.ones((3, 3), np.uint8)
|
||||
dilated = cv2.dilate(edges, kernel, iterations=2)
|
||||
|
||||
# Fill holes
|
||||
filled = cv2.morphologyEx(dilated, cv2.MORPH_CLOSE, kernel, iterations=3)
|
||||
|
||||
# Invert so walls are black (0)
|
||||
return 255 - filled
|
||||
|
||||
def clean_walls(self, binary: np.ndarray) -> np.ndarray:
|
||||
"""Clean up detected walls with morphological operations."""
|
||||
kernel = np.ones((self.wall_thickness, self.wall_thickness), np.uint8)
|
||||
|
||||
# Remove small noise
|
||||
opened = cv2.morphologyEx(binary, cv2.MORPH_OPEN, kernel)
|
||||
|
||||
# Fill small gaps in walls
|
||||
closed = cv2.morphologyEx(opened, cv2.MORPH_CLOSE, kernel)
|
||||
|
||||
return closed
|
||||
|
||||
def scale_to_resolution(
|
||||
self,
|
||||
img: np.ndarray,
|
||||
target_width_m: Optional[float] = None,
|
||||
target_height_m: Optional[float] = None,
|
||||
) -> np.ndarray:
|
||||
"""
|
||||
Scale image to match desired physical dimensions.
|
||||
|
||||
Args:
|
||||
img: Input image
|
||||
target_width_m: Target width in meters (None to use height ratio)
|
||||
target_height_m: Target height in meters (None to use width ratio)
|
||||
|
||||
Returns:
|
||||
Scaled image where each pixel = self.resolution meters
|
||||
"""
|
||||
h, w = img.shape[:2]
|
||||
|
||||
if target_width_m is not None:
|
||||
target_width_px = int(target_width_m / self.resolution)
|
||||
scale = target_width_px / w
|
||||
target_height_px = int(h * scale)
|
||||
elif target_height_m is not None:
|
||||
target_height_px = int(target_height_m / self.resolution)
|
||||
scale = target_height_px / h
|
||||
target_width_px = int(w * scale)
|
||||
else:
|
||||
# Keep original size, assume source resolution matches
|
||||
return img
|
||||
|
||||
return cv2.resize(img, (target_width_px, target_height_px), interpolation=cv2.INTER_NEAREST)
|
||||
|
||||
def add_border(self, img: np.ndarray, border_px: int = 4) -> np.ndarray:
|
||||
"""Add solid wall border around the map."""
|
||||
# 0 = occupied (wall), 255 = free
|
||||
bordered = cv2.copyMakeBorder(
|
||||
img,
|
||||
border_px, border_px, border_px, border_px,
|
||||
cv2.BORDER_CONSTANT,
|
||||
value=0
|
||||
)
|
||||
return bordered
|
||||
|
||||
def add_furniture(
|
||||
self,
|
||||
img: np.ndarray,
|
||||
furniture_density: float = 0.1,
|
||||
seed: Optional[int] = None
|
||||
) -> np.ndarray:
|
||||
"""
|
||||
Add random furniture obstacles to the map.
|
||||
|
||||
Args:
|
||||
img: Input map (255=free, 0=occupied)
|
||||
furniture_density: Fraction of free space to fill with furniture
|
||||
seed: Random seed for reproducibility
|
||||
"""
|
||||
if seed is not None:
|
||||
np.random.seed(seed)
|
||||
|
||||
result = img.copy()
|
||||
h, w = result.shape
|
||||
|
||||
# Find free space pixels
|
||||
free_mask = result == 255
|
||||
free_pixels = np.sum(free_mask)
|
||||
|
||||
# Common furniture pieces (width, height in pixels at 5cm resolution)
|
||||
furniture_templates = [
|
||||
('table', (16, 20)), # 80cm x 100cm dining table
|
||||
('sofa', (40, 16)), # 200cm x 80cm sofa
|
||||
('bed', (36, 40)), # 180cm x 200cm bed
|
||||
('desk', (24, 12)), # 120cm x 60cm desk
|
||||
('chair', (8, 8)), # 40cm x 40cm chair
|
||||
('wardrobe', (24, 12)), # 120cm x 60cm wardrobe
|
||||
('nightstand', (8, 8)), # 40cm x 40cm nightstand
|
||||
('bookshelf', (16, 6)), # 80cm x 30cm bookshelf
|
||||
]
|
||||
|
||||
target_furniture_pixels = int(free_pixels * furniture_density)
|
||||
placed_pixels = 0
|
||||
attempts = 0
|
||||
max_attempts = 1000
|
||||
|
||||
while placed_pixels < target_furniture_pixels and attempts < max_attempts:
|
||||
attempts += 1
|
||||
|
||||
# Pick random furniture
|
||||
name, (fw, fh) = furniture_templates[np.random.randint(len(furniture_templates))]
|
||||
|
||||
# Random rotation (0 or 90 degrees)
|
||||
if np.random.random() > 0.5:
|
||||
fw, fh = fh, fw
|
||||
|
||||
# Random position
|
||||
x = np.random.randint(0, max(1, w - fw))
|
||||
y = np.random.randint(0, max(1, h - fh))
|
||||
|
||||
# Check if area is free (with margin)
|
||||
margin = 2
|
||||
x1, y1 = max(0, x - margin), max(0, y - margin)
|
||||
x2, y2 = min(w, x + fw + margin), min(h, y + fh + margin)
|
||||
|
||||
region = result[y1:y2, x1:x2]
|
||||
if np.all(region == 255):
|
||||
# Place furniture (walls = 0)
|
||||
result[y:y+fh, x:x+fw] = 0
|
||||
placed_pixels += fw * fh
|
||||
|
||||
return result
|
||||
|
||||
def convert(
|
||||
self,
|
||||
input_path: str,
|
||||
width_m: Optional[float] = None,
|
||||
height_m: Optional[float] = None,
|
||||
detection_method: str = 'adaptive',
|
||||
add_border: bool = True,
|
||||
add_furniture: bool = False,
|
||||
furniture_density: float = 0.05,
|
||||
furniture_seed: Optional[int] = None,
|
||||
) -> Tuple[np.ndarray, dict]:
|
||||
"""
|
||||
Convert floor plan image to occupancy map.
|
||||
|
||||
Args:
|
||||
input_path: Path to input image
|
||||
width_m: Physical width in meters
|
||||
height_m: Physical height in meters
|
||||
detection_method: 'adaptive', 'otsu', or 'canny'
|
||||
add_border: Add wall border around map
|
||||
add_furniture: Add random furniture obstacles
|
||||
furniture_density: Density of furniture (0-1)
|
||||
furniture_seed: Random seed for furniture placement
|
||||
|
||||
Returns:
|
||||
Tuple of (pgm_array, yaml_metadata)
|
||||
"""
|
||||
# Load image
|
||||
img = self.load_image(input_path)
|
||||
|
||||
# Invert if needed (some floor plans have white walls)
|
||||
if self.invert_colors:
|
||||
img = 255 - img
|
||||
|
||||
# Detect walls
|
||||
if detection_method == 'adaptive':
|
||||
binary = self.detect_walls_adaptive(img)
|
||||
elif detection_method == 'otsu':
|
||||
binary = self.detect_walls_otsu(img)
|
||||
elif detection_method == 'canny':
|
||||
binary = self.detect_walls_canny(img)
|
||||
else:
|
||||
raise ValueError(f"Unknown detection method: {detection_method}")
|
||||
|
||||
# Clean up
|
||||
cleaned = self.clean_walls(binary)
|
||||
|
||||
# Scale to physical dimensions
|
||||
scaled = self.scale_to_resolution(cleaned, width_m, height_m)
|
||||
|
||||
# Add border
|
||||
if add_border:
|
||||
scaled = self.add_border(scaled, border_px=2)
|
||||
|
||||
# Add furniture
|
||||
if add_furniture:
|
||||
scaled = self.add_furniture(scaled, furniture_density, furniture_seed)
|
||||
|
||||
# Calculate physical dimensions
|
||||
h, w = scaled.shape
|
||||
physical_width = w * self.resolution
|
||||
physical_height = h * self.resolution
|
||||
|
||||
# Create YAML metadata
|
||||
metadata = {
|
||||
'resolution': self.resolution,
|
||||
'origin': [0.0, 0.0, 0.0],
|
||||
'negate': 0,
|
||||
'occupied_thresh': self.occupied_thresh,
|
||||
'free_thresh': self.free_thresh,
|
||||
'physical_size': {
|
||||
'width_m': physical_width,
|
||||
'height_m': physical_height,
|
||||
},
|
||||
}
|
||||
|
||||
return scaled, metadata
|
||||
|
||||
def save_pgm(self, img: np.ndarray, output_path: str, comment: str = ""):
|
||||
"""Save image as ASCII PGM (P2 format)."""
|
||||
h, w = img.shape
|
||||
|
||||
with open(output_path, 'w') as f:
|
||||
f.write("P2\n")
|
||||
if comment:
|
||||
# Split comment into multiple lines if needed
|
||||
for line in comment.split('\n'):
|
||||
f.write(f"# {line}\n")
|
||||
f.write(f"{w} {h}\n")
|
||||
f.write("255\n")
|
||||
|
||||
for row in img:
|
||||
f.write(' '.join(map(str, row)) + '\n')
|
||||
|
||||
def save_yaml(self, metadata: dict, output_path: str, pgm_filename: str):
|
||||
"""Save YAML metadata file."""
|
||||
# Create ROS-compatible YAML
|
||||
yaml_data = {
|
||||
'image': pgm_filename,
|
||||
'resolution': metadata['resolution'],
|
||||
'origin': metadata['origin'],
|
||||
'negate': metadata['negate'],
|
||||
'occupied_thresh': metadata['occupied_thresh'],
|
||||
'free_thresh': metadata['free_thresh'],
|
||||
}
|
||||
|
||||
with open(output_path, 'w') as f:
|
||||
# Add comment header
|
||||
f.write(f"# Map: {pgm_filename}\n")
|
||||
f.write(f"# Physical size: {metadata['physical_size']['width_m']:.1f}m x {metadata['physical_size']['height_m']:.1f}m\n")
|
||||
yaml.dump(yaml_data, f, default_flow_style=False, sort_keys=False)
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(
|
||||
description='Convert floor plan images to ROS-compatible PGM+YAML maps',
|
||||
formatter_class=argparse.RawDescriptionHelpFormatter,
|
||||
epilog="""
|
||||
Examples:
|
||||
# Convert with specific physical width (height calculated proportionally)
|
||||
python floorplan_converter.py apartment.png --width 12.0 --output apartment.pgm
|
||||
|
||||
# Convert with both dimensions specified
|
||||
python floorplan_converter.py house.jpg --width 15.0 --height 10.0 --output house.pgm
|
||||
|
||||
# Add furniture obstacles for exploration testing
|
||||
python floorplan_converter.py layout.png --width 8.0 --add-furniture --output furnished.pgm
|
||||
|
||||
# Use Canny edge detection for blueprints with thin lines
|
||||
python floorplan_converter.py blueprint.png --width 20.0 --method canny --output blueprint.pgm
|
||||
|
||||
# Invert colors (for floor plans with white walls)
|
||||
python floorplan_converter.py inverted.png --width 10.0 --invert --output map.pgm
|
||||
"""
|
||||
)
|
||||
|
||||
parser.add_argument('input', help='Input floor plan image (PNG, JPG, SVG, PDF)')
|
||||
parser.add_argument('--output', '-o', required=True, help='Output PGM file path')
|
||||
parser.add_argument('--width', '-w', type=float, help='Physical width in meters')
|
||||
parser.add_argument('--height', '-H', type=float, help='Physical height in meters')
|
||||
parser.add_argument('--resolution', '-r', type=float, default=0.05,
|
||||
help='Meters per pixel (default: 0.05)')
|
||||
parser.add_argument('--method', '-m', choices=['adaptive', 'otsu', 'canny'],
|
||||
default='adaptive', help='Wall detection method (default: adaptive)')
|
||||
parser.add_argument('--invert', '-i', action='store_true',
|
||||
help='Invert colors (treat dark as free space)')
|
||||
parser.add_argument('--no-border', action='store_true',
|
||||
help='Do not add wall border around map')
|
||||
parser.add_argument('--add-furniture', '-f', action='store_true',
|
||||
help='Add random furniture obstacles')
|
||||
parser.add_argument('--furniture-density', type=float, default=0.05,
|
||||
help='Furniture density 0-1 (default: 0.05)')
|
||||
parser.add_argument('--furniture-seed', type=int,
|
||||
help='Random seed for furniture placement')
|
||||
parser.add_argument('--wall-thickness', type=int, default=2,
|
||||
help='Minimum wall thickness in pixels (default: 2)')
|
||||
parser.add_argument('--preview', '-p', action='store_true',
|
||||
help='Show preview window (requires display)')
|
||||
|
||||
args = parser.parse_args()
|
||||
|
||||
# Validate input
|
||||
if args.width is None and args.height is None:
|
||||
parser.error("At least one of --width or --height must be specified")
|
||||
|
||||
# Create converter
|
||||
converter = FloorPlanConverter(
|
||||
resolution=args.resolution,
|
||||
wall_thickness=args.wall_thickness,
|
||||
invert_colors=args.invert,
|
||||
)
|
||||
|
||||
print(f"Converting: {args.input}")
|
||||
print(f"Resolution: {args.resolution}m/pixel")
|
||||
|
||||
# Convert
|
||||
try:
|
||||
pgm, metadata = converter.convert(
|
||||
args.input,
|
||||
width_m=args.width,
|
||||
height_m=args.height,
|
||||
detection_method=args.method,
|
||||
add_border=not args.no_border,
|
||||
add_furniture=args.add_furniture,
|
||||
furniture_density=args.furniture_density,
|
||||
furniture_seed=args.furniture_seed,
|
||||
)
|
||||
except Exception as e:
|
||||
print(f"Error: {e}")
|
||||
sys.exit(1)
|
||||
|
||||
# Generate output paths
|
||||
output_pgm = Path(args.output)
|
||||
output_yaml = output_pgm.with_suffix('.yaml')
|
||||
|
||||
# Create comment for PGM
|
||||
comment = f"Converted from: {Path(args.input).name}\n"
|
||||
comment += f"Size: {metadata['physical_size']['width_m']:.1f}m x {metadata['physical_size']['height_m']:.1f}m"
|
||||
if args.add_furniture:
|
||||
comment += f"\nFurniture density: {args.furniture_density}"
|
||||
|
||||
# Save files
|
||||
converter.save_pgm(pgm, str(output_pgm), comment)
|
||||
converter.save_yaml(metadata, str(output_yaml), output_pgm.name)
|
||||
|
||||
h, w = pgm.shape
|
||||
print(f"Output: {output_pgm}")
|
||||
print(f" {output_yaml}")
|
||||
print(f"Size: {w}x{h} pixels ({metadata['physical_size']['width_m']:.1f}m x {metadata['physical_size']['height_m']:.1f}m)")
|
||||
|
||||
# Preview if requested
|
||||
if args.preview:
|
||||
try:
|
||||
cv2.imshow('Converted Map', pgm)
|
||||
print("Press any key to close preview...")
|
||||
cv2.waitKey(0)
|
||||
cv2.destroyAllWindows()
|
||||
except Exception as e:
|
||||
print(f"Preview not available: {e}")
|
||||
|
||||
print("Done!")
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,390 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
HouseExpo Dataset Converter
|
||||
|
||||
Converts HouseExpo floor plan images to ROS-compatible PGM+YAML maps.
|
||||
|
||||
HouseExpo format:
|
||||
- Binary grayscale images (0=wall, 255=free)
|
||||
- Resolution: 1 pixel = 1 cm
|
||||
- ~35,000 floor plans from real Chinese apartments
|
||||
|
||||
This converter:
|
||||
- Scales images from 1cm/px to 5cm/px (vastu-map standard)
|
||||
- Adds wall borders for closed environments
|
||||
- Optionally adds furniture obstacles
|
||||
- Supports batch conversion with filtering
|
||||
|
||||
Usage:
|
||||
# Convert single image
|
||||
python houseexpo_converter.py dataset/0004d52d1aeeb8ae6de39d6bd993e992.png -o maps/
|
||||
|
||||
# Batch convert with size filtering
|
||||
python houseexpo_converter.py dataset/ -o maps/ --min-size 5 --max-size 15 --count 50
|
||||
|
||||
# Convert with furniture
|
||||
python houseexpo_converter.py dataset/ -o maps/ --add-furniture --count 20
|
||||
|
||||
Author: VacuumTiger Project
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import os
|
||||
import sys
|
||||
from pathlib import Path
|
||||
from typing import List, Tuple, Optional
|
||||
import random
|
||||
import yaml
|
||||
|
||||
try:
|
||||
import cv2
|
||||
import numpy as np
|
||||
from PIL import Image
|
||||
except ImportError as e:
|
||||
print(f"Missing dependency: {e}")
|
||||
print("Install with: pip install opencv-python numpy pillow pyyaml")
|
||||
sys.exit(1)
|
||||
|
||||
|
||||
# HouseExpo resolution: 1 pixel = 1 cm = 0.01 m
|
||||
HOUSEEXPO_RESOLUTION = 0.01 # meters per pixel
|
||||
|
||||
# Target resolution for vastu-map
|
||||
TARGET_RESOLUTION = 0.05 # meters per pixel (5 cm)
|
||||
|
||||
# Scale factor: 1cm -> 5cm means divide by 5
|
||||
SCALE_FACTOR = int(TARGET_RESOLUTION / HOUSEEXPO_RESOLUTION)
|
||||
|
||||
|
||||
def load_houseexpo_image(path: str) -> Tuple[np.ndarray, float, float]:
|
||||
"""
|
||||
Load HouseExpo image and return with physical dimensions.
|
||||
|
||||
Returns:
|
||||
Tuple of (image_array, width_meters, height_meters)
|
||||
"""
|
||||
img = Image.open(path).convert('L')
|
||||
arr = np.array(img)
|
||||
|
||||
# Calculate physical size (1px = 1cm)
|
||||
width_m = img.size[0] * HOUSEEXPO_RESOLUTION
|
||||
height_m = img.size[1] * HOUSEEXPO_RESOLUTION
|
||||
|
||||
return arr, width_m, height_m
|
||||
|
||||
|
||||
def scale_to_target_resolution(img: np.ndarray) -> np.ndarray:
|
||||
"""Scale image from 1cm/px to 5cm/px resolution."""
|
||||
h, w = img.shape
|
||||
new_w = w // SCALE_FACTOR
|
||||
new_h = h // SCALE_FACTOR
|
||||
|
||||
# Use INTER_AREA for downscaling (best for reducing aliasing)
|
||||
scaled = cv2.resize(img, (new_w, new_h), interpolation=cv2.INTER_AREA)
|
||||
|
||||
# Re-binarize after scaling (threshold at 128)
|
||||
_, binary = cv2.threshold(scaled, 128, 255, cv2.THRESH_BINARY)
|
||||
|
||||
return binary
|
||||
|
||||
|
||||
def add_border(img: np.ndarray, thickness: int = 2) -> np.ndarray:
|
||||
"""Add wall border around the map."""
|
||||
bordered = cv2.copyMakeBorder(
|
||||
img,
|
||||
thickness, thickness, thickness, thickness,
|
||||
cv2.BORDER_CONSTANT,
|
||||
value=0
|
||||
)
|
||||
return bordered
|
||||
|
||||
|
||||
def clean_map(img: np.ndarray) -> np.ndarray:
|
||||
"""Clean up the map with morphological operations."""
|
||||
kernel = np.ones((2, 2), np.uint8)
|
||||
|
||||
# Close small gaps in walls
|
||||
closed = cv2.morphologyEx(img, cv2.MORPH_CLOSE, kernel)
|
||||
|
||||
# Remove small noise
|
||||
opened = cv2.morphologyEx(closed, cv2.MORPH_OPEN, kernel)
|
||||
|
||||
return opened
|
||||
|
||||
|
||||
def add_furniture(
|
||||
img: np.ndarray,
|
||||
density: float = 0.05,
|
||||
seed: Optional[int] = None
|
||||
) -> np.ndarray:
|
||||
"""Add random furniture obstacles."""
|
||||
if seed is not None:
|
||||
np.random.seed(seed)
|
||||
|
||||
result = img.copy()
|
||||
h, w = result.shape
|
||||
|
||||
# Furniture templates (width, height in pixels at 5cm resolution)
|
||||
furniture = [
|
||||
(16, 20), # 80cm x 100cm table
|
||||
(40, 16), # 200cm x 80cm sofa
|
||||
(36, 40), # 180cm x 200cm bed
|
||||
(24, 12), # 120cm x 60cm desk
|
||||
(8, 8), # 40cm x 40cm chair
|
||||
]
|
||||
|
||||
free_pixels = np.sum(result == 255)
|
||||
target_furniture = int(free_pixels * density)
|
||||
placed = 0
|
||||
attempts = 0
|
||||
|
||||
while placed < target_furniture and attempts < 500:
|
||||
attempts += 1
|
||||
|
||||
fw, fh = furniture[np.random.randint(len(furniture))]
|
||||
if np.random.random() > 0.5:
|
||||
fw, fh = fh, fw
|
||||
|
||||
x = np.random.randint(4, max(5, w - fw - 4))
|
||||
y = np.random.randint(4, max(5, h - fh - 4))
|
||||
|
||||
region = result[y:y+fh, x:x+fw]
|
||||
if region.size > 0 and np.all(region == 255):
|
||||
result[y:y+fh, x:x+fw] = 0
|
||||
placed += fw * fh
|
||||
|
||||
return result
|
||||
|
||||
|
||||
def convert_houseexpo(
|
||||
input_path: str,
|
||||
add_furniture_flag: bool = False,
|
||||
furniture_density: float = 0.05,
|
||||
furniture_seed: Optional[int] = None,
|
||||
) -> Tuple[np.ndarray, dict]:
|
||||
"""
|
||||
Convert a HouseExpo image to vastu-map format.
|
||||
|
||||
Returns:
|
||||
Tuple of (pgm_array, metadata_dict)
|
||||
"""
|
||||
# Load original image
|
||||
img, width_m, height_m = load_houseexpo_image(input_path)
|
||||
|
||||
# Scale to 5cm resolution
|
||||
scaled = scale_to_target_resolution(img)
|
||||
|
||||
# Clean up
|
||||
cleaned = clean_map(scaled)
|
||||
|
||||
# Add border
|
||||
bordered = add_border(cleaned, thickness=2)
|
||||
|
||||
# Add furniture if requested
|
||||
if add_furniture_flag:
|
||||
bordered = add_furniture(bordered, furniture_density, furniture_seed)
|
||||
|
||||
# Calculate final dimensions
|
||||
h, w = bordered.shape
|
||||
final_width_m = w * TARGET_RESOLUTION
|
||||
final_height_m = h * TARGET_RESOLUTION
|
||||
|
||||
metadata = {
|
||||
'resolution': TARGET_RESOLUTION,
|
||||
'origin': [0.0, 0.0, 0.0],
|
||||
'negate': 0,
|
||||
'occupied_thresh': 0.65,
|
||||
'free_thresh': 0.196,
|
||||
'source': 'HouseExpo',
|
||||
'original_size_m': {
|
||||
'width': width_m,
|
||||
'height': height_m,
|
||||
},
|
||||
'physical_size': {
|
||||
'width_m': final_width_m,
|
||||
'height_m': final_height_m,
|
||||
},
|
||||
}
|
||||
|
||||
return bordered, metadata
|
||||
|
||||
|
||||
def save_pgm(img: np.ndarray, output_path: str, comment: str = ""):
|
||||
"""Save image as ASCII PGM (P2 format)."""
|
||||
h, w = img.shape
|
||||
with open(output_path, 'w') as f:
|
||||
f.write("P2\n")
|
||||
if comment:
|
||||
for line in comment.split('\n'):
|
||||
f.write(f"# {line}\n")
|
||||
f.write(f"{w} {h}\n")
|
||||
f.write("255\n")
|
||||
for row in img:
|
||||
f.write(' '.join(map(str, row)) + '\n')
|
||||
|
||||
|
||||
def save_yaml(metadata: dict, output_path: str, pgm_filename: str):
|
||||
"""Save YAML metadata file."""
|
||||
yaml_data = {
|
||||
'image': pgm_filename,
|
||||
'resolution': metadata['resolution'],
|
||||
'origin': metadata['origin'],
|
||||
'negate': metadata['negate'],
|
||||
'occupied_thresh': metadata['occupied_thresh'],
|
||||
'free_thresh': metadata['free_thresh'],
|
||||
}
|
||||
with open(output_path, 'w') as f:
|
||||
f.write(f"# HouseExpo floor plan: {pgm_filename}\n")
|
||||
f.write(f"# Original size: {metadata['original_size_m']['width']:.1f}m x {metadata['original_size_m']['height']:.1f}m\n")
|
||||
f.write(f"# Scaled size: {metadata['physical_size']['width_m']:.1f}m x {metadata['physical_size']['height_m']:.1f}m\n")
|
||||
yaml.dump(yaml_data, f, default_flow_style=False, sort_keys=False)
|
||||
|
||||
|
||||
def get_image_size(path: str) -> Tuple[float, float]:
|
||||
"""Get physical size of HouseExpo image in meters."""
|
||||
img = Image.open(path)
|
||||
return img.size[0] * HOUSEEXPO_RESOLUTION, img.size[1] * HOUSEEXPO_RESOLUTION
|
||||
|
||||
|
||||
def filter_by_size(
|
||||
files: List[str],
|
||||
input_dir: str,
|
||||
min_size: float,
|
||||
max_size: float,
|
||||
) -> List[str]:
|
||||
"""Filter images by physical size (in meters)."""
|
||||
filtered = []
|
||||
for f in files:
|
||||
try:
|
||||
w, h = get_image_size(os.path.join(input_dir, f))
|
||||
# Check if both dimensions are within range
|
||||
if min_size <= w <= max_size and min_size <= h <= max_size:
|
||||
filtered.append(f)
|
||||
except Exception:
|
||||
continue
|
||||
return filtered
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(
|
||||
description='Convert HouseExpo floor plans to vastu-map PGM format',
|
||||
formatter_class=argparse.RawDescriptionHelpFormatter,
|
||||
epilog="""
|
||||
HouseExpo Dataset Info:
|
||||
- ~35,000 real floor plans from Chinese apartments
|
||||
- Resolution: 1 pixel = 1 cm
|
||||
- Binary format: 0=wall (black), 255=free (white)
|
||||
|
||||
Examples:
|
||||
# Convert a single floor plan
|
||||
python houseexpo_converter.py dataset/0004d52d1aeeb8ae6de39d6bd993e992.png -o maps/
|
||||
|
||||
# Batch convert 50 medium-sized apartments (5-15m)
|
||||
python houseexpo_converter.py dataset/ -o maps/ --min-size 5 --max-size 15 --count 50
|
||||
|
||||
# Convert with furniture for exploration testing
|
||||
python houseexpo_converter.py dataset/ -o maps/ --add-furniture --count 20
|
||||
|
||||
# Convert all apartments between 8-12m
|
||||
python houseexpo_converter.py dataset/ -o maps/ --min-size 8 --max-size 12
|
||||
"""
|
||||
)
|
||||
|
||||
parser.add_argument('input', help='Input PNG file or directory of HouseExpo images')
|
||||
parser.add_argument('--output', '-o', required=True, help='Output directory')
|
||||
parser.add_argument('--min-size', type=float, default=0,
|
||||
help='Minimum dimension in meters (filter)')
|
||||
parser.add_argument('--max-size', type=float, default=100,
|
||||
help='Maximum dimension in meters (filter)')
|
||||
parser.add_argument('--count', '-n', type=int, default=0,
|
||||
help='Maximum number of maps to convert (0=all)')
|
||||
parser.add_argument('--add-furniture', '-f', action='store_true',
|
||||
help='Add furniture obstacles')
|
||||
parser.add_argument('--furniture-density', type=float, default=0.05,
|
||||
help='Furniture density 0-1 (default: 0.05)')
|
||||
parser.add_argument('--seed', '-s', type=int,
|
||||
help='Random seed for reproducibility')
|
||||
parser.add_argument('--prefix', '-p', default='houseexpo',
|
||||
help='Output filename prefix (default: houseexpo)')
|
||||
parser.add_argument('--shuffle', action='store_true',
|
||||
help='Shuffle file order before selecting')
|
||||
|
||||
args = parser.parse_args()
|
||||
|
||||
# Create output directory
|
||||
os.makedirs(args.output, exist_ok=True)
|
||||
|
||||
input_path = Path(args.input)
|
||||
|
||||
if input_path.is_file():
|
||||
# Single file conversion
|
||||
files = [input_path.name]
|
||||
input_dir = str(input_path.parent)
|
||||
else:
|
||||
# Directory batch conversion
|
||||
input_dir = str(input_path)
|
||||
files = [f for f in os.listdir(input_dir) if f.endswith('.png')]
|
||||
files.sort()
|
||||
|
||||
# Filter by size
|
||||
if args.min_size > 0 or args.max_size < 100:
|
||||
print(f"Filtering by size: {args.min_size}m - {args.max_size}m...")
|
||||
files = filter_by_size(files, input_dir, args.min_size, args.max_size)
|
||||
print(f"Found {len(files)} matching files")
|
||||
|
||||
# Shuffle if requested
|
||||
if args.shuffle:
|
||||
random.seed(args.seed)
|
||||
random.shuffle(files)
|
||||
|
||||
# Limit count
|
||||
if args.count > 0:
|
||||
files = files[:args.count]
|
||||
|
||||
if not files:
|
||||
print("No files to convert")
|
||||
return
|
||||
|
||||
print(f"Converting {len(files)} HouseExpo floor plan(s)...")
|
||||
|
||||
for i, filename in enumerate(files):
|
||||
input_file = os.path.join(input_dir, filename)
|
||||
|
||||
try:
|
||||
# Convert
|
||||
pgm, metadata = convert_houseexpo(
|
||||
input_file,
|
||||
add_furniture_flag=args.add_furniture,
|
||||
furniture_density=args.furniture_density,
|
||||
furniture_seed=args.seed + i if args.seed else None,
|
||||
)
|
||||
|
||||
# Generate output names
|
||||
base_name = Path(filename).stem
|
||||
if len(files) > 1:
|
||||
output_name = f"{args.prefix}_{i:04d}"
|
||||
else:
|
||||
output_name = f"{args.prefix}_{base_name[:8]}"
|
||||
|
||||
pgm_path = os.path.join(args.output, f"{output_name}.pgm")
|
||||
yaml_path = os.path.join(args.output, f"{output_name}.yaml")
|
||||
|
||||
# Save
|
||||
comment = f"HouseExpo: {filename}\n"
|
||||
comment += f"Size: {metadata['physical_size']['width_m']:.1f}m x {metadata['physical_size']['height_m']:.1f}m"
|
||||
|
||||
save_pgm(pgm, pgm_path, comment)
|
||||
save_yaml(metadata, yaml_path, f"{output_name}.pgm")
|
||||
|
||||
h, w = pgm.shape
|
||||
print(f"[{i+1}/{len(files)}] {filename} -> {output_name}.pgm "
|
||||
f"({w}x{h}px, {metadata['physical_size']['width_m']:.1f}x{metadata['physical_size']['height_m']:.1f}m)")
|
||||
|
||||
except Exception as e:
|
||||
print(f"[{i+1}/{len(files)}] Error converting {filename}: {e}")
|
||||
|
||||
print(f"\nDone! Converted {len(files)} maps to {args.output}/")
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,11 @@
|
||||
# Floor plan converter and apartment generator dependencies
|
||||
opencv-python>=4.5.0
|
||||
numpy>=1.20.0
|
||||
pillow>=8.0.0
|
||||
pyyaml>=5.4.0
|
||||
|
||||
# Optional: for SVG support
|
||||
# cairosvg>=2.5.0
|
||||
|
||||
# Optional: for PDF support
|
||||
# pdf2image>=1.14.0
|
||||
Reference in New Issue
Block a user