Rename vastu-map to vastu-slam and reorganize modules
- Rename package from vastu-map to vastu-slam (v0.3.0) - Reorganize slam/ module to matching/ for clarity - Add modes/ module with localization mode - Add robust matching, traits, and motion core - Remove explore/, pathfinding/, query/ (moved to dhruva-slam daemon) - Add README documentation for each module
This commit is contained in:
codetiger
@@ -1,163 +0,0 @@
|
||||
# Vastu-Map
|
||||
|
||||
A high-performance feature-based 2D SLAM library with SIMD-friendly data layouts.
|
||||
|
||||
## Features
|
||||
|
||||
- **VectorMap Representation**: Uses line segments and corners instead of occupancy grids for memory-efficient mapping
|
||||
- **Full SLAM Pipeline**: Localization and mapping integrated into a clean API
|
||||
- **SIMD Optimized**: Explicit `std::simd` operations with `f32x4` for portable SIMD (ARM NEON, x86 SSE/AVX)
|
||||
- **SoA Data Layout**: Struct-of-Arrays for cache-friendly contiguous SIMD loads
|
||||
|
||||
## Quick Start
|
||||
|
||||
```rust
|
||||
use vastu_map::{VectorMap, VectorMapConfig, Map};
|
||||
use vastu_map::core::{Pose2D, PointCloud2D, Point2D};
|
||||
|
||||
// Create map with default configuration
|
||||
let config = VectorMapConfig::default();
|
||||
let mut map = VectorMap::new(config);
|
||||
|
||||
// Process a scan observation
|
||||
let scan = PointCloud2D::from_points(&[
|
||||
Point2D::new(1.0, 0.0),
|
||||
Point2D::new(1.0, 0.1),
|
||||
Point2D::new(1.0, 0.2),
|
||||
]);
|
||||
let odometry = Pose2D::identity();
|
||||
let result = map.observe(&scan, odometry);
|
||||
|
||||
println!("Pose: ({:.2}, {:.2}), confidence: {:.2}",
|
||||
result.pose.x, result.pose.y, result.confidence);
|
||||
```
|
||||
|
||||
## Architecture
|
||||
|
||||
```
|
||||
┌─────────────────┐
|
||||
│ Lidar Scan │
|
||||
│ (PolarScan) │
|
||||
└────────┬────────┘
|
||||
│ to_cartesian()
|
||||
▼
|
||||
┌─────────────────┐
|
||||
│ PointCloud2D │
|
||||
│ (SoA layout) │
|
||||
└────────┬────────┘
|
||||
│
|
||||
┌────────────────────┼────────────────────┐
|
||||
│ │ │
|
||||
▼ ▼ ▼
|
||||
┌────────────────┐ ┌────────────────┐ ┌────────────────┐
|
||||
│ Extraction │ │ Matching │ │ Loop Closure │
|
||||
│ (Split-Merge) │ │ (Point-Line │ │ (Keyframes + │
|
||||
│ │ │ ICP) │ │ Descriptors) │
|
||||
└───────┬────────┘ └───────┬────────┘ └───────┬────────┘
|
||||
│ │ │
|
||||
▼ ▼ │
|
||||
┌────────────────┐ ┌────────────────┐ │
|
||||
│ Lines/Corners │ │ Matched Pose │ │
|
||||
│ (FeatureSet) │ │ + Confidence │ │
|
||||
└───────┬────────┘ └───────┬────────┘ │
|
||||
│ │ │
|
||||
└──────────┬─────────┘ │
|
||||
│ │
|
||||
▼ │
|
||||
┌────────────────┐ │
|
||||
│ Integration │◄─────────────────────┘
|
||||
│ (Merge/Add to │ Loop constraints
|
||||
│ VectorMap) │
|
||||
└───────┬────────┘
|
||||
│
|
||||
▼
|
||||
┌────────────────┐
|
||||
│ VectorMap │──► Query (raycast, occupancy)
|
||||
│ (Lines + │──► Frontiers (exploration)
|
||||
│ Corners) │──► Path Planning
|
||||
└────────────────┘
|
||||
```
|
||||
|
||||
## Modules
|
||||
|
||||
| Module | Description | Documentation |
|
||||
|--------|-------------|---------------|
|
||||
| [`core`](src/core/) | Fundamental types (Point2D, Pose2D, PointCloud2D, Bounds) | [README](src/core/README.md) |
|
||||
| [`features`](src/features/) | Feature types (Line2D, Corner2D, descriptors) | [README](src/features/README.md) |
|
||||
| [`extraction`](src/extraction/) | Line extraction (split-merge, corner detection) | [README](src/extraction/README.md) |
|
||||
| [`matching`](src/matching/) | Scan matching (ICP, RANSAC, Gauss-Newton) | [README](src/matching/README.md) |
|
||||
| [`integration`](src/integration/) | Map integration (spatial index, association, merging) | [README](src/integration/README.md) |
|
||||
| [`query`](src/query/) | Map queries (raycast, occupancy, frontier detection) | [README](src/query/README.md) |
|
||||
| [`vector_map`](src/vector_map/) | Main VectorMap SLAM implementation | [README](src/vector_map/README.md) |
|
||||
| [`loop_closure`](src/loop_closure/) | Keyframe-based loop closure detection | [README](src/loop_closure/README.md) |
|
||||
| [`simd`](src/simd/) | SIMD documentation (uses `std::simd` / `portable_simd`) | — |
|
||||
|
||||
## Coordinate Frame
|
||||
|
||||
All coordinates follow the ROS REP-103 convention:
|
||||
- **X-forward**: Positive X is in front of the robot
|
||||
- **Y-left**: Positive Y is to the left of the robot
|
||||
- **Z-up**: Positive Z is upward (not used in 2D)
|
||||
- **Rotation**: Counter-clockwise positive
|
||||
|
||||
## SIMD and Portability
|
||||
|
||||
The library uses Rust's `std::simd` (`portable_simd` feature) with explicit `f32x4` operations for portable SIMD. This maps to ARM NEON on ARM targets and SSE/AVX on x86 targets. No platform-specific intrinsics are used—the compiler handles the architecture-specific codegen based on target features.
|
||||
|
||||
## Building
|
||||
|
||||
### Development (Host)
|
||||
|
||||
```bash
|
||||
cargo build --release
|
||||
cargo test
|
||||
```
|
||||
|
||||
### Cross-compilation
|
||||
|
||||
For cross-compilation, add your target and configure appropriate compiler flags:
|
||||
|
||||
```bash
|
||||
# Example: ARM target
|
||||
rustup target add armv7-unknown-linux-musleabihf
|
||||
cargo build --release --target armv7-unknown-linux-musleabihf
|
||||
```
|
||||
|
||||
### Integration Tests
|
||||
|
||||
Integration tests require the `integration-tests` feature:
|
||||
|
||||
```bash
|
||||
cargo test --features integration-tests
|
||||
```
|
||||
|
||||
## Performance Characteristics
|
||||
|
||||
- **Memory**: ~100 bytes per line feature (vs ~1MB per 1000x1000 occupancy grid)
|
||||
- **Matching**: <10ms for typical indoor scans on embedded ARM
|
||||
- **Binary Size**: ~100KB stripped
|
||||
|
||||
## Extensibility
|
||||
|
||||
The library provides traits for custom implementations:
|
||||
|
||||
```rust
|
||||
// Custom line extraction
|
||||
pub trait LineExtractor: Send + Sync {
|
||||
fn extract(&self, points: &[Point2D]) -> Vec<Line2D>;
|
||||
}
|
||||
|
||||
// Custom scan matching
|
||||
pub trait ScanMatcher: Send + Sync {
|
||||
fn match_scan(&self, points: &[Point2D], lines: &[Line2D], initial: Pose2D) -> MatchResult;
|
||||
}
|
||||
|
||||
// Custom loop closure detection
|
||||
pub trait LoopDetector: Send + Sync {
|
||||
fn detect(&mut self, pose: Pose2D, ...) -> Option<LoopClosure>;
|
||||
}
|
||||
```
|
||||
|
||||
## License
|
||||
|
||||
MIT
|
||||
@@ -1,725 +0,0 @@
|
||||
//! Mock exploration example with encoder odometry and scan matching
|
||||
//!
|
||||
//! Demonstrates autonomous frontier-based exploration using a simulated robot
|
||||
//! with realistic encoder drift and scan-to-map matching for correction.
|
||||
//!
|
||||
//! Usage:
|
||||
//! cargo run --example explore_mock -- --map simple_room
|
||||
//! cargo run --example explore_mock # Uses random map
|
||||
//!
|
||||
//! Enable debug logging to see drift metrics:
|
||||
//! RUST_LOG=info cargo run --example explore_mock
|
||||
|
||||
use clap::Parser;
|
||||
use rand::prelude::*;
|
||||
use std::f32::consts::{PI, TAU};
|
||||
use std::path::Path;
|
||||
use std::time::Instant;
|
||||
|
||||
// SangamIO mock simulation components
|
||||
use sangam_io::devices::mock::{
|
||||
config::{EncoderConfig, LidarConfig, RobotConfig},
|
||||
encoder_sim::EncoderSimulator,
|
||||
lidar_sim::LidarSimulator,
|
||||
map_loader::SimulationMap,
|
||||
noise::NoiseGenerator,
|
||||
physics::PhysicsState,
|
||||
};
|
||||
|
||||
// Vastu-map components
|
||||
use vastu_map::{
|
||||
LidarScan, OccupancyGridMap, Pose2D, VastuConfig,
|
||||
exploration::ExplorationController,
|
||||
io::{export_ros_map, save_vastu},
|
||||
slam::{CorrelativeMatcher, CorrelativeMatcherConfig},
|
||||
};
|
||||
|
||||
/// Mock exploration example
|
||||
#[derive(Parser, Debug)]
|
||||
#[command(author, version, about, long_about = None)]
|
||||
struct Args {
|
||||
/// Map name to load from maps/ folder (e.g., "simple_room")
|
||||
/// If not specified, picks a random map
|
||||
#[arg(short, long)]
|
||||
map: Option<String>,
|
||||
|
||||
/// Configuration file path
|
||||
#[arg(short, long, default_value = "configs/config.yaml")]
|
||||
config: String,
|
||||
|
||||
/// Output directory for saved maps
|
||||
#[arg(short, long, default_value = "./output")]
|
||||
output: String,
|
||||
|
||||
/// Maximum simulation steps (0 = unlimited)
|
||||
#[arg(long, default_value = "10000")]
|
||||
max_steps: usize,
|
||||
|
||||
/// Simulation speed factor (1.0 = realtime)
|
||||
#[arg(long, default_value = "10.0")]
|
||||
speed: f32,
|
||||
|
||||
/// Show progress every N steps
|
||||
#[arg(long, default_value = "100")]
|
||||
progress_interval: usize,
|
||||
}
|
||||
|
||||
fn main() {
|
||||
env_logger::init();
|
||||
let args = Args::parse();
|
||||
|
||||
// List available maps
|
||||
let maps_dir = Path::new("maps");
|
||||
let available_maps = list_available_maps(maps_dir);
|
||||
|
||||
if available_maps.is_empty() {
|
||||
eprintln!("No maps found in maps/ folder");
|
||||
std::process::exit(1);
|
||||
}
|
||||
|
||||
// Select map
|
||||
let map_name = args.map.unwrap_or_else(|| {
|
||||
let mut rng = rand::rng();
|
||||
let selected = available_maps.choose(&mut rng).unwrap().clone();
|
||||
println!("Randomly selected map: {}", selected);
|
||||
selected
|
||||
});
|
||||
|
||||
let map_yaml = maps_dir.join(format!("{}.yaml", map_name));
|
||||
if !map_yaml.exists() {
|
||||
eprintln!("Map not found: {}", map_yaml.display());
|
||||
eprintln!("Available maps: {:?}", available_maps);
|
||||
std::process::exit(1);
|
||||
}
|
||||
|
||||
// Load configuration
|
||||
let config_path = Path::new(&args.config);
|
||||
let vastu_config = if config_path.exists() {
|
||||
VastuConfig::load(config_path).unwrap_or_else(|e| {
|
||||
eprintln!("Warning: Failed to load config: {}, using defaults", e);
|
||||
VastuConfig::default()
|
||||
})
|
||||
} else {
|
||||
println!("Config not found, using defaults");
|
||||
VastuConfig::default()
|
||||
};
|
||||
|
||||
// Load simulation map
|
||||
println!("Loading map: {}", map_yaml.display());
|
||||
let sim_map = SimulationMap::load(&map_yaml).expect("Failed to load simulation map");
|
||||
println!(
|
||||
"Map size: {}x{} pixels, resolution: {}m/px",
|
||||
sim_map.width(),
|
||||
sim_map.height(),
|
||||
sim_map.resolution()
|
||||
);
|
||||
|
||||
// Find starting position (search for free space in grid bounds)
|
||||
let start_pose = find_free_start_position(&sim_map, &vastu_config);
|
||||
println!(
|
||||
"Starting at: ({:.2}, {:.2}, {:.1}°)",
|
||||
start_pose.x,
|
||||
start_pose.y,
|
||||
start_pose.theta.to_degrees()
|
||||
);
|
||||
|
||||
// Create output directory
|
||||
std::fs::create_dir_all(&args.output).ok();
|
||||
|
||||
// Run exploration
|
||||
let result = run_exploration(
|
||||
&sim_map,
|
||||
start_pose,
|
||||
&vastu_config,
|
||||
args.max_steps,
|
||||
args.speed,
|
||||
args.progress_interval,
|
||||
);
|
||||
|
||||
// Save map
|
||||
let output_base = Path::new(&args.output).join(&map_name);
|
||||
|
||||
println!("\nSaving maps...");
|
||||
save_vastu(result.map.storage(), &output_base.with_extension("vastu"))
|
||||
.expect("Failed to save .vastu file");
|
||||
println!(" Saved: {}.vastu", output_base.display());
|
||||
|
||||
export_ros_map(result.map.storage(), &output_base).expect("Failed to save ROS map");
|
||||
println!(
|
||||
" Saved: {}.pgm + {}.yaml",
|
||||
output_base.display(),
|
||||
output_base.display()
|
||||
);
|
||||
|
||||
// Print summary
|
||||
println!("\n=== Exploration Summary ===");
|
||||
println!("Status: {:?}", result.status);
|
||||
println!("Steps: {}", result.steps);
|
||||
println!("Time: {:.1}s", result.elapsed_secs);
|
||||
println!(
|
||||
"Distance traveled: {:.2}m",
|
||||
result.progress.distance_traveled
|
||||
);
|
||||
println!("Frontiers explored: {}", result.progress.frontiers_explored);
|
||||
|
||||
let map_stats = result.map.coverage_stats();
|
||||
println!(
|
||||
"Cells explored: {} floor, {} wall, {} unknown",
|
||||
map_stats.floor_cells, map_stats.wall_cells, map_stats.unknown_cells
|
||||
);
|
||||
println!("Coverage: {:.2}m²", map_stats.explored_area_m2);
|
||||
|
||||
// Drift and scan matching statistics
|
||||
println!("\n=== Scan Matching Stats ===");
|
||||
let ds = &result.drift_stats;
|
||||
let total_matches = ds.successful_matches + ds.failed_matches;
|
||||
let success_rate = if total_matches > 0 {
|
||||
100.0 * ds.successful_matches as f32 / total_matches as f32
|
||||
} else {
|
||||
0.0
|
||||
};
|
||||
println!(
|
||||
"Matches: {} successful, {} failed ({:.1}% success)",
|
||||
ds.successful_matches, ds.failed_matches, success_rate
|
||||
);
|
||||
println!(
|
||||
"Total drift corrected: {:.3}m translation, {:.2}° rotation",
|
||||
ds.total_translation_drift,
|
||||
ds.total_rotation_drift.to_degrees()
|
||||
);
|
||||
println!(
|
||||
"Max drift error: {:.3}m translation, {:.2}° rotation",
|
||||
ds.max_translation_error,
|
||||
ds.max_rotation_error.to_degrees()
|
||||
);
|
||||
}
|
||||
|
||||
/// List available map names in directory
|
||||
fn list_available_maps(dir: &Path) -> Vec<String> {
|
||||
let mut maps = Vec::new();
|
||||
if let Ok(entries) = std::fs::read_dir(dir) {
|
||||
for entry in entries.flatten() {
|
||||
let path = entry.path();
|
||||
if path.extension().is_some_and(|e| e == "yaml") {
|
||||
if let Some(stem) = path.file_stem() {
|
||||
maps.push(stem.to_string_lossy().to_string());
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
maps.sort();
|
||||
maps
|
||||
}
|
||||
|
||||
/// Find a free starting position in the map
|
||||
/// Constrains to the overlapping region of simulation map and vastu-map grid
|
||||
fn find_free_start_position(map: &SimulationMap, vastu_config: &VastuConfig) -> Pose2D {
|
||||
let mut rng = rand::rng();
|
||||
let sim_origin = map.origin();
|
||||
let sim_width_m = map.width() as f32 * map.resolution();
|
||||
let sim_height_m = map.height() as f32 * map.resolution();
|
||||
|
||||
// Vastu-map grid bounds (centered)
|
||||
let grid_size_x = vastu_config.grid.initial_width as f32 * vastu_config.grid.resolution;
|
||||
let grid_size_y = vastu_config.grid.initial_height as f32 * vastu_config.grid.resolution;
|
||||
let grid_min_x = -grid_size_x / 2.0;
|
||||
let grid_min_y = -grid_size_y / 2.0;
|
||||
let grid_max_x = grid_size_x / 2.0;
|
||||
let grid_max_y = grid_size_y / 2.0;
|
||||
|
||||
// Find overlap region
|
||||
let min_x = sim_origin.0.max(grid_min_x) + 0.5;
|
||||
let min_y = sim_origin.1.max(grid_min_y) + 0.5;
|
||||
let max_x = (sim_origin.0 + sim_width_m).min(grid_max_x) - 0.5;
|
||||
let max_y = (sim_origin.1 + sim_height_m).min(grid_max_y) - 0.5;
|
||||
|
||||
// Try random positions within overlap region
|
||||
for _ in 0..1000 {
|
||||
let x = min_x + rng.random::<f32>() * (max_x - min_x);
|
||||
let y = min_y + rng.random::<f32>() * (max_y - min_y);
|
||||
|
||||
// Check if position is free (not occupied, with margin)
|
||||
if !map.is_occupied(x, y)
|
||||
&& !map.is_occupied(x + 0.2, y)
|
||||
&& !map.is_occupied(x - 0.2, y)
|
||||
&& !map.is_occupied(x, y + 0.2)
|
||||
&& !map.is_occupied(x, y - 0.2)
|
||||
{
|
||||
let theta = rng.random::<f32>() * TAU;
|
||||
return Pose2D::new(x, y, theta);
|
||||
}
|
||||
}
|
||||
|
||||
// Fallback: scan for any free position in overlap
|
||||
for py in 0..map.height() {
|
||||
for px in 0..map.width() {
|
||||
let (x, y) = map.pixel_to_world(px, py);
|
||||
if x >= min_x && x <= max_x && y >= min_y && y <= max_y && !map.is_occupied(x, y) {
|
||||
return Pose2D::new(x, y, 0.0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Last resort: center of overlap
|
||||
Pose2D::new((min_x + max_x) / 2.0, (min_y + max_y) / 2.0, 0.0)
|
||||
}
|
||||
|
||||
/// Exploration result
|
||||
struct ExplorationResult {
|
||||
map: OccupancyGridMap,
|
||||
status: ExplorationStatus,
|
||||
steps: usize,
|
||||
elapsed_secs: f32,
|
||||
progress: vastu_map::exploration::ExplorationProgress,
|
||||
drift_stats: DriftStats,
|
||||
}
|
||||
|
||||
/// Drift statistics for encoder vs corrected pose
|
||||
#[derive(Debug, Default)]
|
||||
struct DriftStats {
|
||||
/// Total translation drift (meters)
|
||||
total_translation_drift: f32,
|
||||
/// Total rotation drift (radians)
|
||||
total_rotation_drift: f32,
|
||||
/// Maximum translation error observed
|
||||
max_translation_error: f32,
|
||||
/// Maximum rotation error observed
|
||||
max_rotation_error: f32,
|
||||
/// Number of successful scan matches
|
||||
successful_matches: usize,
|
||||
/// Number of failed scan matches
|
||||
failed_matches: usize,
|
||||
}
|
||||
|
||||
#[derive(Debug)]
|
||||
enum ExplorationStatus {
|
||||
Complete,
|
||||
MaxStepsReached,
|
||||
Failed,
|
||||
}
|
||||
|
||||
/// Simple wheel odometry tracker
|
||||
///
|
||||
/// Computes pose from encoder ticks using differential drive kinematics.
|
||||
struct WheelOdometry {
|
||||
wheel_base: f32,
|
||||
ticks_per_meter: f32,
|
||||
last_left: i32,
|
||||
last_right: i32,
|
||||
pose: Pose2D,
|
||||
initialized: bool,
|
||||
}
|
||||
|
||||
impl WheelOdometry {
|
||||
fn new(wheel_base: f32, ticks_per_meter: f32, initial_pose: Pose2D) -> Self {
|
||||
Self {
|
||||
wheel_base,
|
||||
ticks_per_meter,
|
||||
last_left: 0,
|
||||
last_right: 0,
|
||||
pose: initial_pose,
|
||||
initialized: false,
|
||||
}
|
||||
}
|
||||
|
||||
/// Update odometry from encoder ticks, returns pose delta
|
||||
fn update(&mut self, left_ticks: u16, right_ticks: u16) -> Pose2D {
|
||||
let left = left_ticks as i32;
|
||||
let right = right_ticks as i32;
|
||||
|
||||
if !self.initialized {
|
||||
self.last_left = left;
|
||||
self.last_right = right;
|
||||
self.initialized = true;
|
||||
return Pose2D::new(0.0, 0.0, 0.0);
|
||||
}
|
||||
|
||||
// Handle wrapping (u16 encoder ticks wrap at 65535)
|
||||
let delta_left = (left.wrapping_sub(self.last_left) as i16) as i32;
|
||||
let delta_right = (right.wrapping_sub(self.last_right) as i16) as i32;
|
||||
|
||||
self.last_left = left;
|
||||
self.last_right = right;
|
||||
|
||||
// Convert to meters
|
||||
let left_dist = delta_left as f32 / self.ticks_per_meter;
|
||||
let right_dist = delta_right as f32 / self.ticks_per_meter;
|
||||
|
||||
// Differential drive kinematics
|
||||
let delta_theta = (right_dist - left_dist) / self.wheel_base;
|
||||
let delta_linear = (left_dist + right_dist) / 2.0;
|
||||
|
||||
// Robot-frame delta
|
||||
let (dx, dy) = if delta_theta.abs() < 1e-6 {
|
||||
(delta_linear, 0.0)
|
||||
} else {
|
||||
let radius = delta_linear / delta_theta;
|
||||
(
|
||||
radius * delta_theta.sin(),
|
||||
radius * (1.0 - delta_theta.cos()),
|
||||
)
|
||||
};
|
||||
|
||||
// Transform to world frame
|
||||
let (sin_t, cos_t) = self.pose.theta.sin_cos();
|
||||
let dx_world = dx * cos_t - dy * sin_t;
|
||||
let dy_world = dx * sin_t + dy * cos_t;
|
||||
|
||||
// Update pose
|
||||
self.pose = Pose2D::new(
|
||||
self.pose.x + dx_world,
|
||||
self.pose.y + dy_world,
|
||||
normalize_angle(self.pose.theta + delta_theta),
|
||||
);
|
||||
|
||||
Pose2D::new(dx, dy, delta_theta)
|
||||
}
|
||||
|
||||
fn pose(&self) -> Pose2D {
|
||||
self.pose
|
||||
}
|
||||
|
||||
/// Reset odometry to a corrected pose (after scan matching)
|
||||
fn reset_to(&mut self, pose: Pose2D) {
|
||||
self.pose = pose;
|
||||
}
|
||||
}
|
||||
|
||||
/// Run the exploration simulation
|
||||
fn run_exploration(
|
||||
sim_map: &SimulationMap,
|
||||
start_pose: Pose2D,
|
||||
config: &VastuConfig,
|
||||
max_steps: usize,
|
||||
speed_factor: f32,
|
||||
progress_interval: usize,
|
||||
) -> ExplorationResult {
|
||||
// Create occupancy grid map
|
||||
let map_config = config.to_map_config();
|
||||
let mut map = OccupancyGridMap::new(map_config);
|
||||
|
||||
// Create exploration controller
|
||||
let explore_config = config.to_exploration_config();
|
||||
let mut explorer = ExplorationController::new(explore_config);
|
||||
|
||||
// Create physics simulation (ground truth)
|
||||
let robot_config = RobotConfig::default();
|
||||
let mut physics =
|
||||
PhysicsState::new(start_pose.x, start_pose.y, start_pose.theta, &robot_config);
|
||||
|
||||
// Create encoder simulator (with noise/slip for realistic odometry)
|
||||
let encoder_config = EncoderConfig::default();
|
||||
let encoder_noise = NoiseGenerator::new(42);
|
||||
let mut encoder =
|
||||
EncoderSimulator::new(&encoder_config, robot_config.ticks_per_meter, encoder_noise);
|
||||
|
||||
// Create wheel odometry tracker
|
||||
let mut odometry = WheelOdometry::new(
|
||||
robot_config.wheel_base,
|
||||
robot_config.ticks_per_meter,
|
||||
start_pose,
|
||||
);
|
||||
|
||||
// Create lidar simulator
|
||||
let lidar_config = LidarConfig::default();
|
||||
|
||||
// Create scan-to-map matcher with larger search window for encoder drift
|
||||
// Also set sensor offset to match lidar mounting position for accurate alignment
|
||||
let matcher_config = CorrelativeMatcherConfig {
|
||||
search_x: 0.5, // ±0.5m (default: 0.3m)
|
||||
search_y: 0.5, // ±0.5m
|
||||
search_theta: 1.0, // ±1.0 rad (~57°) - handles large rotation errors
|
||||
sensor_offset: (lidar_config.mounting_x, lidar_config.mounting_y),
|
||||
..CorrelativeMatcherConfig::default()
|
||||
};
|
||||
let matcher = CorrelativeMatcher::new(matcher_config);
|
||||
let lidar_noise = NoiseGenerator::new(43); // Different seed for lidar
|
||||
let mut lidar = LidarSimulator::new(&lidar_config, lidar_noise);
|
||||
|
||||
// Simulation parameters
|
||||
let dt = 0.05; // 20Hz physics
|
||||
let lidar_interval = 0.2; // 5Hz lidar
|
||||
let mut time_since_lidar = lidar_interval; // Trigger immediately
|
||||
let _ = speed_factor;
|
||||
|
||||
// Velocity state
|
||||
let mut linear_vel = 0.0f32;
|
||||
let mut angular_vel = 0.0f32;
|
||||
|
||||
// Corrected pose (from scan matching)
|
||||
let mut corrected_pose = start_pose;
|
||||
|
||||
// Drift statistics
|
||||
let mut drift_stats = DriftStats::default();
|
||||
|
||||
// Start exploration
|
||||
explorer.start();
|
||||
let start_time = Instant::now();
|
||||
|
||||
for step in 0..max_steps.max(1) {
|
||||
// Update encoder simulation based on wheel velocities
|
||||
let (left_vel, right_vel) =
|
||||
physics.wheel_velocities(linear_vel, angular_vel, robot_config.wheel_base);
|
||||
let (left_ticks, right_ticks) = encoder.update(left_vel, right_vel, dt);
|
||||
|
||||
// Update wheel odometry from encoder ticks
|
||||
odometry.update(left_ticks, right_ticks);
|
||||
let odom_pose = odometry.pose();
|
||||
|
||||
// Generate lidar scan periodically
|
||||
time_since_lidar += dt;
|
||||
if time_since_lidar >= lidar_interval {
|
||||
time_since_lidar = 0.0;
|
||||
|
||||
// Generate scan at ground truth position (what sensor actually sees)
|
||||
let gt_pose = Pose2D::new(physics.x(), physics.y(), physics.theta());
|
||||
let scan_data = lidar.generate_scan(sim_map, gt_pose.x, gt_pose.y, gt_pose.theta);
|
||||
|
||||
if !scan_data.is_empty() {
|
||||
let ranges: Vec<f32> = scan_data.iter().map(|(_, d, _)| *d).collect();
|
||||
// Include lidar angle_offset so map observation matches ray-cast direction
|
||||
let angles: Vec<f32> = scan_data
|
||||
.iter()
|
||||
.map(|(a, _, _)| a + lidar_config.angle_offset)
|
||||
.collect();
|
||||
|
||||
let scan = LidarScan::new(
|
||||
ranges,
|
||||
angles,
|
||||
lidar_config.min_range,
|
||||
lidar_config.max_range,
|
||||
);
|
||||
|
||||
// Use encoder odometry as initial guess for scan matching
|
||||
let match_result = matcher.match_scan(&scan, odom_pose, map.storage());
|
||||
|
||||
// Check map has enough structure for reliable matching
|
||||
let map_stats = map.coverage_stats();
|
||||
let has_enough_structure = map_stats.wall_cells > 100;
|
||||
|
||||
// Accept match if converged with reasonable score
|
||||
// Lower thresholds for closed-loop SLAM where map quality depends on matching
|
||||
let min_hit_ratio = if has_enough_structure { 0.25 } else { 0.35 };
|
||||
let accept_match =
|
||||
match_result.converged && match_result.hit_ratio() > min_hit_ratio;
|
||||
|
||||
// Use matched pose if successful, otherwise use odometry
|
||||
if accept_match {
|
||||
corrected_pose = match_result.pose;
|
||||
drift_stats.successful_matches += 1;
|
||||
|
||||
// Reset odometry to corrected pose to prevent drift accumulation
|
||||
odometry.reset_to(corrected_pose);
|
||||
|
||||
// Track drift: difference between encoder and corrected pose
|
||||
let dx = odom_pose.x - corrected_pose.x;
|
||||
let dy = odom_pose.y - corrected_pose.y;
|
||||
let trans_error = (dx * dx + dy * dy).sqrt();
|
||||
let rot_error = (odom_pose.theta - corrected_pose.theta).abs();
|
||||
|
||||
drift_stats.total_translation_drift += trans_error;
|
||||
drift_stats.total_rotation_drift += rot_error;
|
||||
drift_stats.max_translation_error =
|
||||
drift_stats.max_translation_error.max(trans_error);
|
||||
drift_stats.max_rotation_error = drift_stats.max_rotation_error.max(rot_error);
|
||||
|
||||
log::debug!(
|
||||
"[ScanMatch] SUCCESS: odom=({:.2},{:.2},{:.1}°) → corrected=({:.2},{:.2},{:.1}°), error={:.3}m, hits={:.0}%",
|
||||
odom_pose.x,
|
||||
odom_pose.y,
|
||||
odom_pose.theta.to_degrees(),
|
||||
corrected_pose.x,
|
||||
corrected_pose.y,
|
||||
corrected_pose.theta.to_degrees(),
|
||||
trans_error,
|
||||
match_result.hit_ratio() * 100.0
|
||||
);
|
||||
} else {
|
||||
// Match failed or not trusted - use odometry pose
|
||||
corrected_pose = odom_pose;
|
||||
drift_stats.failed_matches += 1;
|
||||
|
||||
log::warn!(
|
||||
"[ScanMatch] FAILED: using odom pose ({:.2},{:.2},{:.1}°), score={:.2}",
|
||||
odom_pose.x,
|
||||
odom_pose.y,
|
||||
odom_pose.theta.to_degrees(),
|
||||
match_result.score
|
||||
);
|
||||
}
|
||||
|
||||
// Update map with estimated pose (closed-loop SLAM)
|
||||
map.observe_lidar(&scan, corrected_pose);
|
||||
}
|
||||
}
|
||||
|
||||
// Update exploration controller with corrected pose
|
||||
let command = explorer.update(corrected_pose, map.storage());
|
||||
|
||||
// Convert command to velocities
|
||||
// Note: We use corrected_pose for control, but physics uses ground truth internally
|
||||
match command {
|
||||
vastu_map::exploration::ExplorationCommand::MoveTo { target, max_speed } => {
|
||||
let (lin, ang) = compute_velocity_to_target(
|
||||
corrected_pose.x,
|
||||
corrected_pose.y,
|
||||
corrected_pose.theta,
|
||||
target.x,
|
||||
target.y,
|
||||
max_speed,
|
||||
2.0, // max angular speed
|
||||
);
|
||||
linear_vel = lin;
|
||||
angular_vel = ang;
|
||||
}
|
||||
vastu_map::exploration::ExplorationCommand::Rotate {
|
||||
target_heading,
|
||||
max_angular_speed,
|
||||
} => {
|
||||
linear_vel = 0.0;
|
||||
angular_vel = compute_angular_velocity(
|
||||
corrected_pose.theta,
|
||||
target_heading,
|
||||
max_angular_speed,
|
||||
);
|
||||
}
|
||||
vastu_map::exploration::ExplorationCommand::Stop => {
|
||||
linear_vel = 0.0;
|
||||
angular_vel = 0.0;
|
||||
}
|
||||
vastu_map::exploration::ExplorationCommand::ExplorationComplete => {
|
||||
println!("\nExploration complete!");
|
||||
return ExplorationResult {
|
||||
map,
|
||||
status: ExplorationStatus::Complete,
|
||||
steps: step,
|
||||
elapsed_secs: start_time.elapsed().as_secs_f32(),
|
||||
progress: explorer.progress(),
|
||||
drift_stats,
|
||||
};
|
||||
}
|
||||
vastu_map::exploration::ExplorationCommand::ExplorationFailed { reason } => {
|
||||
println!("\nExploration failed: {}", reason);
|
||||
return ExplorationResult {
|
||||
map,
|
||||
status: ExplorationStatus::Failed,
|
||||
steps: step,
|
||||
elapsed_secs: start_time.elapsed().as_secs_f32(),
|
||||
progress: explorer.progress(),
|
||||
drift_stats,
|
||||
};
|
||||
}
|
||||
vastu_map::exploration::ExplorationCommand::None => {
|
||||
// Continue with current velocities
|
||||
}
|
||||
}
|
||||
|
||||
// Update physics and check for collision
|
||||
let collision = physics.update(dt, linear_vel, angular_vel, sim_map, &robot_config);
|
||||
if collision {
|
||||
// Trigger obstacle detection event for the exploration controller
|
||||
explorer.handle_event(vastu_map::exploration::ExplorationEvent::ObstacleDetected);
|
||||
}
|
||||
|
||||
// Progress output - also show when exploration changes state
|
||||
let progress = explorer.progress();
|
||||
if progress_interval > 0 && step % progress_interval == 0 && step > 0 {
|
||||
// Compare ground truth vs corrected pose for drift visualization
|
||||
let gt_pose = Pose2D::new(physics.x(), physics.y(), physics.theta());
|
||||
let pos_error = ((gt_pose.x - corrected_pose.x).powi(2)
|
||||
+ (gt_pose.y - corrected_pose.y).powi(2))
|
||||
.sqrt();
|
||||
|
||||
println!(
|
||||
"[Step {}] State: {}, Frontiers: {}, Dist: {:.1}m, Matches: {}/{}, PosErr: {:.3}m",
|
||||
step,
|
||||
progress.state,
|
||||
progress.frontiers_explored,
|
||||
progress.distance_traveled,
|
||||
drift_stats.successful_matches,
|
||||
drift_stats.successful_matches + drift_stats.failed_matches,
|
||||
pos_error
|
||||
);
|
||||
}
|
||||
|
||||
// Debug: show completion or state changes
|
||||
if matches!(
|
||||
command,
|
||||
vastu_map::exploration::ExplorationCommand::ExplorationComplete
|
||||
) {
|
||||
println!("[Step {}] Exploration complete!", step);
|
||||
}
|
||||
}
|
||||
|
||||
ExplorationResult {
|
||||
map,
|
||||
status: ExplorationStatus::MaxStepsReached,
|
||||
steps: max_steps,
|
||||
elapsed_secs: start_time.elapsed().as_secs_f32(),
|
||||
progress: explorer.progress(),
|
||||
drift_stats,
|
||||
}
|
||||
}
|
||||
|
||||
/// Compute velocity command to move towards target
|
||||
///
|
||||
/// Uses proportional control with concurrent linear + angular movement
|
||||
/// for smooth path following.
|
||||
fn compute_velocity_to_target(
|
||||
x: f32,
|
||||
y: f32,
|
||||
theta: f32,
|
||||
target_x: f32,
|
||||
target_y: f32,
|
||||
max_linear: f32,
|
||||
max_angular: f32,
|
||||
) -> (f32, f32) {
|
||||
let dx = target_x - x;
|
||||
let dy = target_y - y;
|
||||
let distance = (dx * dx + dy * dy).sqrt();
|
||||
|
||||
if distance < 0.05 {
|
||||
return (0.0, 0.0); // Close enough
|
||||
}
|
||||
|
||||
// Angle to target
|
||||
let target_angle = dy.atan2(dx);
|
||||
let angle_error = normalize_angle(target_angle - theta);
|
||||
let abs_error = angle_error.abs();
|
||||
|
||||
// Proportional angular velocity (gain = 1.5)
|
||||
let angular_vel = (angle_error * 1.5).clamp(-max_angular, max_angular);
|
||||
|
||||
// Only pure rotation if angle is very large (>45°, ~0.78 rad)
|
||||
if abs_error > 0.78 {
|
||||
return (0.0, angular_vel);
|
||||
}
|
||||
|
||||
// Move forward with concurrent angular correction
|
||||
// Scale linear velocity based on angle error (slower when turning more)
|
||||
let angle_factor = 1.0 - (abs_error / 0.78).min(0.8);
|
||||
let linear_vel = max_linear.min(distance) * angle_factor;
|
||||
|
||||
(linear_vel, angular_vel)
|
||||
}
|
||||
|
||||
/// Compute angular velocity to rotate to target heading
|
||||
fn compute_angular_velocity(current: f32, target: f32, max_speed: f32) -> f32 {
|
||||
let error = normalize_angle(target - current);
|
||||
if error.abs() < 0.05 {
|
||||
0.0
|
||||
} else {
|
||||
(error * 2.0).clamp(-max_speed, max_speed)
|
||||
}
|
||||
}
|
||||
|
||||
/// Normalize angle to [-PI, PI)
|
||||
fn normalize_angle(angle: f32) -> f32 {
|
||||
let mut a = angle % TAU;
|
||||
if a > PI {
|
||||
a -= TAU;
|
||||
} else if a < -PI {
|
||||
a += TAU;
|
||||
}
|
||||
a
|
||||
}
|
||||
@@ -1,305 +0,0 @@
|
||||
//! Simple exploration example using VastuExplorer API.
|
||||
//!
|
||||
//! This example demonstrates the high-level exploration API that handles
|
||||
//! all SLAM, scan matching, and navigation internally.
|
||||
//!
|
||||
//! Usage:
|
||||
//! cargo run --example explore_simple -- --map simple_room
|
||||
//! cargo run --example explore_simple # Uses random map
|
||||
|
||||
use clap::Parser;
|
||||
use rand::prelude::*;
|
||||
use std::f32::consts::TAU;
|
||||
use std::path::Path;
|
||||
|
||||
// SangamIO mock simulation components
|
||||
use sangam_io::devices::mock::{
|
||||
config::{LidarConfig, RobotConfig},
|
||||
lidar_sim::LidarSimulator,
|
||||
map_loader::SimulationMap,
|
||||
noise::NoiseGenerator,
|
||||
physics::PhysicsState,
|
||||
};
|
||||
|
||||
// Vastu-map components
|
||||
use vastu_map::{
|
||||
LidarScan, Pose2D, VastuConfig,
|
||||
explore::{ExploreResult, SensorSource, VastuExplorer},
|
||||
io::{export_ros_map, save_vastu},
|
||||
};
|
||||
|
||||
/// Simple exploration example
|
||||
#[derive(Parser, Debug)]
|
||||
#[command(author, version, about, long_about = None)]
|
||||
struct Args {
|
||||
/// Configuration file path
|
||||
#[arg(short, long, default_value = "configs/config.yaml")]
|
||||
config: String,
|
||||
|
||||
/// Map name to load from maps/ folder (e.g., "simple_room")
|
||||
#[arg(short, long)]
|
||||
map: Option<String>,
|
||||
|
||||
/// Output directory for saved maps
|
||||
#[arg(short, long, default_value = "./output")]
|
||||
output: String,
|
||||
}
|
||||
|
||||
/// Mock sensor source that wraps the SangamIO simulation.
|
||||
struct MockSensorSource<'a> {
|
||||
sim_map: &'a SimulationMap,
|
||||
physics: PhysicsState,
|
||||
lidar: LidarSimulator,
|
||||
lidar_config: LidarConfig,
|
||||
robot_config: RobotConfig,
|
||||
latest_scan: Option<LidarScan>,
|
||||
scan_counter: u32,
|
||||
/// Current commanded velocities
|
||||
linear_vel: f32,
|
||||
angular_vel: f32,
|
||||
}
|
||||
|
||||
impl<'a> MockSensorSource<'a> {
|
||||
fn new(sim_map: &'a SimulationMap, start_pose: Pose2D) -> Self {
|
||||
let robot_config = RobotConfig::default();
|
||||
let lidar_config = LidarConfig::default();
|
||||
let noise = NoiseGenerator::new(42);
|
||||
let lidar = LidarSimulator::new(&lidar_config, noise);
|
||||
let physics =
|
||||
PhysicsState::new(start_pose.x, start_pose.y, start_pose.theta, &robot_config);
|
||||
|
||||
Self {
|
||||
sim_map,
|
||||
physics,
|
||||
lidar,
|
||||
lidar_config,
|
||||
robot_config,
|
||||
latest_scan: None,
|
||||
scan_counter: 0,
|
||||
linear_vel: 0.0,
|
||||
angular_vel: 0.0,
|
||||
}
|
||||
}
|
||||
|
||||
fn update_physics(&mut self, dt: f32, linear: f32, angular: f32) {
|
||||
self.physics
|
||||
.update(dt, linear, angular, self.sim_map, &self.robot_config);
|
||||
}
|
||||
}
|
||||
|
||||
impl SensorSource for MockSensorSource<'_> {
|
||||
fn get_pose(&self) -> Pose2D {
|
||||
Pose2D::new(self.physics.x(), self.physics.y(), self.physics.theta())
|
||||
}
|
||||
|
||||
fn get_lidar_scan(&self) -> Option<LidarScan> {
|
||||
self.latest_scan.clone()
|
||||
}
|
||||
|
||||
fn send_velocity(&mut self, linear: f32, angular: f32) {
|
||||
// Store commanded velocities (physics updated in poll)
|
||||
self.linear_vel = linear;
|
||||
self.angular_vel = angular;
|
||||
}
|
||||
|
||||
fn is_connected(&self) -> bool {
|
||||
true
|
||||
}
|
||||
|
||||
fn poll(&mut self) {
|
||||
// Update physics with current velocities (10Hz control loop)
|
||||
let dt = 0.1;
|
||||
self.update_physics(dt, self.linear_vel, self.angular_vel);
|
||||
|
||||
// Generate lidar scan every 2nd poll (5Hz lidar at 10Hz control)
|
||||
// Start at 1 so first poll generates a scan
|
||||
self.scan_counter += 1;
|
||||
if self.scan_counter == 1 || self.scan_counter % 2 == 0 {
|
||||
let scan_data = self.lidar.generate_scan(
|
||||
self.sim_map,
|
||||
self.physics.x(),
|
||||
self.physics.y(),
|
||||
self.physics.theta(),
|
||||
);
|
||||
|
||||
if !scan_data.is_empty() {
|
||||
let ranges: Vec<f32> = scan_data.iter().map(|(_, d, _)| *d).collect();
|
||||
let angles: Vec<f32> = scan_data.iter().map(|(a, _, _)| *a).collect();
|
||||
|
||||
self.latest_scan = Some(LidarScan::new(
|
||||
ranges,
|
||||
angles,
|
||||
self.lidar_config.min_range,
|
||||
self.lidar_config.max_range,
|
||||
));
|
||||
}
|
||||
} else {
|
||||
self.latest_scan = None;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fn main() {
|
||||
env_logger::init();
|
||||
let args = Args::parse();
|
||||
|
||||
// Load configuration
|
||||
let config_path = Path::new(&args.config);
|
||||
let vastu_config = if config_path.exists() {
|
||||
VastuConfig::load(config_path).unwrap_or_else(|e| {
|
||||
eprintln!("Warning: Failed to load config: {}, using defaults", e);
|
||||
VastuConfig::default()
|
||||
})
|
||||
} else {
|
||||
println!(
|
||||
"Config not found at {}, using defaults",
|
||||
config_path.display()
|
||||
);
|
||||
VastuConfig::default()
|
||||
};
|
||||
|
||||
// List available maps
|
||||
let maps_dir = Path::new("maps");
|
||||
let available_maps = list_available_maps(maps_dir);
|
||||
|
||||
if available_maps.is_empty() {
|
||||
eprintln!("No maps found in maps/ folder");
|
||||
std::process::exit(1);
|
||||
}
|
||||
|
||||
// Select map
|
||||
let map_name = args.map.unwrap_or_else(|| {
|
||||
let mut rng = rand::rng();
|
||||
let selected = available_maps.choose(&mut rng).unwrap().clone();
|
||||
println!("Randomly selected map: {}", selected);
|
||||
selected
|
||||
});
|
||||
|
||||
let map_yaml = maps_dir.join(format!("{}.yaml", map_name));
|
||||
if !map_yaml.exists() {
|
||||
eprintln!("Map not found: {}", map_yaml.display());
|
||||
eprintln!("Available maps: {:?}", available_maps);
|
||||
std::process::exit(1);
|
||||
}
|
||||
|
||||
// Load simulation map
|
||||
println!("Loading map: {}", map_yaml.display());
|
||||
let sim_map = SimulationMap::load(&map_yaml).expect("Failed to load simulation map");
|
||||
println!(
|
||||
"Map size: {}x{} pixels, resolution: {}m/px",
|
||||
sim_map.width(),
|
||||
sim_map.height(),
|
||||
sim_map.resolution()
|
||||
);
|
||||
|
||||
// Find starting position
|
||||
let start_pose = find_free_start_position(&sim_map);
|
||||
println!(
|
||||
"Starting at: ({:.2}, {:.2}, {:.1}°)",
|
||||
start_pose.x,
|
||||
start_pose.y,
|
||||
start_pose.theta.to_degrees()
|
||||
);
|
||||
|
||||
// Create output directory
|
||||
std::fs::create_dir_all(&args.output).ok();
|
||||
|
||||
// Create explorer with loaded configuration
|
||||
let explorer_config = vastu_config.to_explorer_config();
|
||||
let explorer = VastuExplorer::new(explorer_config);
|
||||
|
||||
// Create sensor source
|
||||
let mut source = MockSensorSource::new(&sim_map, start_pose);
|
||||
|
||||
// Run exploration - the library handles everything!
|
||||
println!("\nStarting exploration...");
|
||||
match explorer.explore(&mut source) {
|
||||
ExploreResult::Complete(map) => {
|
||||
println!("\nExploration complete!");
|
||||
|
||||
// Save maps
|
||||
let output_base = Path::new(&args.output).join(&map_name);
|
||||
|
||||
save_vastu(map.storage(), &output_base.with_extension("vastu"))
|
||||
.expect("Failed to save .vastu file");
|
||||
println!("Saved: {}.vastu", output_base.display());
|
||||
|
||||
export_ros_map(map.storage(), &output_base).expect("Failed to save ROS map");
|
||||
println!(
|
||||
"Saved: {}.pgm + {}.yaml",
|
||||
output_base.display(),
|
||||
output_base.display()
|
||||
);
|
||||
|
||||
let stats = map.coverage_stats();
|
||||
println!("\n=== Map Statistics ===");
|
||||
println!("Floor cells: {}", stats.floor_cells);
|
||||
println!("Wall cells: {}", stats.wall_cells);
|
||||
println!("Coverage: {:.2}m²", stats.explored_area_m2);
|
||||
}
|
||||
ExploreResult::Failed { map, error } => {
|
||||
println!("\nExploration failed: {}", error);
|
||||
|
||||
// Save partial map
|
||||
let output_base = Path::new(&args.output).join(format!("{}_partial", map_name));
|
||||
save_vastu(map.storage(), &output_base.with_extension("vastu")).ok();
|
||||
println!("Saved partial map: {}.vastu", output_base.display());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// List available map names in directory.
|
||||
fn list_available_maps(dir: &Path) -> Vec<String> {
|
||||
let mut maps = Vec::new();
|
||||
if let Ok(entries) = std::fs::read_dir(dir) {
|
||||
for entry in entries.flatten() {
|
||||
let path = entry.path();
|
||||
if path.extension().is_some_and(|e| e == "yaml") {
|
||||
if let Some(stem) = path.file_stem() {
|
||||
maps.push(stem.to_string_lossy().to_string());
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
maps.sort();
|
||||
maps
|
||||
}
|
||||
|
||||
/// Find a free starting position in the map.
|
||||
fn find_free_start_position(map: &SimulationMap) -> Pose2D {
|
||||
let mut rng = rand::rng();
|
||||
let sim_origin = map.origin();
|
||||
let sim_width_m = map.width() as f32 * map.resolution();
|
||||
let sim_height_m = map.height() as f32 * map.resolution();
|
||||
|
||||
// Search in center region
|
||||
let margin = 0.5;
|
||||
let min_x = sim_origin.0 + margin;
|
||||
let min_y = sim_origin.1 + margin;
|
||||
let max_x = sim_origin.0 + sim_width_m - margin;
|
||||
let max_y = sim_origin.1 + sim_height_m - margin;
|
||||
|
||||
// Try random positions
|
||||
for _ in 0..1000 {
|
||||
let x = min_x + rng.random::<f32>() * (max_x - min_x);
|
||||
let y = min_y + rng.random::<f32>() * (max_y - min_y);
|
||||
|
||||
// Check if position is free with margin
|
||||
if !map.is_occupied(x, y)
|
||||
&& !map.is_occupied(x + 0.2, y)
|
||||
&& !map.is_occupied(x - 0.2, y)
|
||||
&& !map.is_occupied(x, y + 0.2)
|
||||
&& !map.is_occupied(x, y - 0.2)
|
||||
{
|
||||
let theta = rng.random::<f32>() * TAU;
|
||||
return Pose2D::new(x, y, theta);
|
||||
}
|
||||
}
|
||||
|
||||
// Fallback: center of map
|
||||
Pose2D::new(
|
||||
sim_origin.0 + sim_width_m / 2.0,
|
||||
sim_origin.1 + sim_height_m / 2.0,
|
||||
0.0,
|
||||
)
|
||||
}
|
||||
@@ -1,671 +0,0 @@
|
||||
//! Scan Matching Accuracy Test
|
||||
//!
|
||||
//! Controlled tests to measure scan-to-map matching accuracy using YAML scenario files.
|
||||
//!
|
||||
//! Scenarios define:
|
||||
//! - Which map to use
|
||||
//! - Starting pose (optional)
|
||||
//! - Sequence of movement commands (move/stay)
|
||||
//! - SVG output configuration
|
||||
//!
|
||||
//! Usage:
|
||||
//! cargo run --example scan_match_test -- --scenario scenarios/straight_line.yaml
|
||||
//! cargo run --example scan_match_test -- -s scenarios/rotation_360.yaml
|
||||
//! cargo run --example scan_match_test -- -s scenarios/square_path.yaml -o ./my_output
|
||||
|
||||
use clap::Parser;
|
||||
use std::f32::consts::{PI, TAU};
|
||||
use std::path::Path;
|
||||
|
||||
// SangamIO mock simulation components
|
||||
use sangam_io::devices::mock::{
|
||||
config::{EncoderConfig, LidarConfig, RobotConfig},
|
||||
encoder_sim::EncoderSimulator,
|
||||
lidar_sim::LidarSimulator,
|
||||
map_loader::SimulationMap,
|
||||
noise::NoiseGenerator,
|
||||
physics::PhysicsState,
|
||||
};
|
||||
|
||||
// Vastu-map components
|
||||
use vastu_map::{
|
||||
LidarScan, OccupancyGridMap, Pose2D, VastuConfig,
|
||||
io::{Command, Scenario, SvgConfig, SvgVisualizer, markers_by_distance, markers_by_time},
|
||||
slam::{CorrelativeMatcher, CorrelativeMatcherConfig},
|
||||
};
|
||||
|
||||
/// Scan matching accuracy test using YAML scenario files
|
||||
#[derive(Parser, Debug)]
|
||||
#[command(author, version, about, long_about = None)]
|
||||
struct Args {
|
||||
/// Scenario YAML file defining the test
|
||||
#[arg(short = 's', long)]
|
||||
scenario: String,
|
||||
|
||||
/// Configuration file path
|
||||
#[arg(short, long, default_value = "configs/config.yaml")]
|
||||
config: String,
|
||||
|
||||
/// Output directory for SVG audit file
|
||||
#[arg(short, long, default_value = "./output")]
|
||||
output: String,
|
||||
}
|
||||
|
||||
/// Per-scan metrics
|
||||
#[derive(Clone, Debug)]
|
||||
struct ScanMetrics {
|
||||
/// Ground truth pose
|
||||
gt_pose: Pose2D,
|
||||
/// Encoder odometry pose
|
||||
odom_pose: Pose2D,
|
||||
/// Scan-matched corrected pose
|
||||
corrected_pose: Pose2D,
|
||||
/// Match score (0-1)
|
||||
match_score: f32,
|
||||
/// Hit ratio (0-1)
|
||||
hit_ratio: f32,
|
||||
/// Whether match converged
|
||||
converged: bool,
|
||||
}
|
||||
|
||||
/// Test results
|
||||
struct TestResults {
|
||||
test_name: String,
|
||||
start_pose: Pose2D,
|
||||
gt_final: Pose2D,
|
||||
odom_final: Pose2D,
|
||||
corrected_final: Pose2D,
|
||||
scans: Vec<ScanMetrics>,
|
||||
wall_cells: usize,
|
||||
map: OccupancyGridMap,
|
||||
}
|
||||
|
||||
/// Simple wheel odometry tracker
|
||||
struct WheelOdometry {
|
||||
wheel_base: f32,
|
||||
ticks_per_meter: f32,
|
||||
last_left: i32,
|
||||
last_right: i32,
|
||||
pose: Pose2D,
|
||||
initialized: bool,
|
||||
}
|
||||
|
||||
impl WheelOdometry {
|
||||
fn new(wheel_base: f32, ticks_per_meter: f32, initial_pose: Pose2D) -> Self {
|
||||
Self {
|
||||
wheel_base,
|
||||
ticks_per_meter,
|
||||
last_left: 0,
|
||||
last_right: 0,
|
||||
pose: initial_pose,
|
||||
initialized: false,
|
||||
}
|
||||
}
|
||||
|
||||
fn update(&mut self, left_ticks: u16, right_ticks: u16) {
|
||||
let left = left_ticks as i32;
|
||||
let right = right_ticks as i32;
|
||||
|
||||
if !self.initialized {
|
||||
self.last_left = left;
|
||||
self.last_right = right;
|
||||
self.initialized = true;
|
||||
return;
|
||||
}
|
||||
|
||||
let delta_left = (left.wrapping_sub(self.last_left) as i16) as i32;
|
||||
let delta_right = (right.wrapping_sub(self.last_right) as i16) as i32;
|
||||
|
||||
self.last_left = left;
|
||||
self.last_right = right;
|
||||
|
||||
let left_dist = delta_left as f32 / self.ticks_per_meter;
|
||||
let right_dist = delta_right as f32 / self.ticks_per_meter;
|
||||
|
||||
let delta_theta = (right_dist - left_dist) / self.wheel_base;
|
||||
let delta_linear = (left_dist + right_dist) / 2.0;
|
||||
|
||||
let (dx, dy) = if delta_theta.abs() < 1e-6 {
|
||||
(delta_linear, 0.0)
|
||||
} else {
|
||||
let radius = delta_linear / delta_theta;
|
||||
(
|
||||
radius * delta_theta.sin(),
|
||||
radius * (1.0 - delta_theta.cos()),
|
||||
)
|
||||
};
|
||||
|
||||
let (sin_t, cos_t) = self.pose.theta.sin_cos();
|
||||
let dx_world = dx * cos_t - dy * sin_t;
|
||||
let dy_world = dx * sin_t + dy * cos_t;
|
||||
|
||||
self.pose = Pose2D::new(
|
||||
self.pose.x + dx_world,
|
||||
self.pose.y + dy_world,
|
||||
normalize_angle(self.pose.theta + delta_theta),
|
||||
);
|
||||
}
|
||||
|
||||
fn pose(&self) -> Pose2D {
|
||||
self.pose
|
||||
}
|
||||
|
||||
fn reset_to(&mut self, pose: Pose2D) {
|
||||
self.pose = pose;
|
||||
}
|
||||
}
|
||||
|
||||
fn main() {
|
||||
env_logger::init();
|
||||
let args = Args::parse();
|
||||
|
||||
// Load configuration
|
||||
let config_path = Path::new(&args.config);
|
||||
let vastu_config = if config_path.exists() {
|
||||
VastuConfig::load(config_path).unwrap_or_else(|e| {
|
||||
eprintln!("Warning: Failed to load config: {}, using defaults", e);
|
||||
VastuConfig::default()
|
||||
})
|
||||
} else {
|
||||
println!(
|
||||
"Config not found at {}, using defaults",
|
||||
config_path.display()
|
||||
);
|
||||
VastuConfig::default()
|
||||
};
|
||||
|
||||
// Create output directory
|
||||
let output_dir = Path::new(&args.output);
|
||||
std::fs::create_dir_all(output_dir).ok();
|
||||
|
||||
// Load scenario
|
||||
let scenario_file = Path::new(&args.scenario);
|
||||
if !scenario_file.exists() {
|
||||
eprintln!("Scenario file not found: {}", scenario_file.display());
|
||||
std::process::exit(1);
|
||||
}
|
||||
|
||||
println!("Loading scenario: {}", scenario_file.display());
|
||||
let scenario = Scenario::load(scenario_file).unwrap_or_else(|e| {
|
||||
eprintln!("Failed to load scenario: {}", e);
|
||||
std::process::exit(1);
|
||||
});
|
||||
|
||||
// Resolve and load map from scenario
|
||||
let maps_dir = Path::new("maps");
|
||||
let map_yaml = scenario.resolve_map_path(maps_dir);
|
||||
if !map_yaml.exists() {
|
||||
eprintln!("Map not found: {}", map_yaml.display());
|
||||
std::process::exit(1);
|
||||
}
|
||||
|
||||
println!("Loading map: {}", map_yaml.display());
|
||||
let sim_map = SimulationMap::load(&map_yaml).expect("Failed to load simulation map");
|
||||
println!(
|
||||
"Map size: {}x{} pixels, resolution: {}m/px",
|
||||
sim_map.width(),
|
||||
sim_map.height(),
|
||||
sim_map.resolution()
|
||||
);
|
||||
|
||||
// Run the scenario test
|
||||
let results = run_scenario_test(&sim_map, &scenario, &vastu_config);
|
||||
print_results(&results);
|
||||
save_output(&results, output_dir, &scenario);
|
||||
}
|
||||
|
||||
/// Find a free starting position with good clearance
|
||||
fn find_start_position(sim_map: &SimulationMap) -> Pose2D {
|
||||
let sim_origin = sim_map.origin();
|
||||
let sim_width_m = sim_map.width() as f32 * sim_map.resolution();
|
||||
let sim_height_m = sim_map.height() as f32 * sim_map.resolution();
|
||||
|
||||
// Search for position with good clearance
|
||||
let clearance = 1.0; // 1m from walls
|
||||
for py in 0..sim_map.height() {
|
||||
for px in 0..sim_map.width() {
|
||||
let (x, y) = sim_map.pixel_to_world(px, py);
|
||||
|
||||
// Check bounds
|
||||
if x < sim_origin.0 + clearance
|
||||
|| x > sim_origin.0 + sim_width_m - clearance
|
||||
|| y < sim_origin.1 + clearance
|
||||
|| y > sim_origin.1 + sim_height_m - clearance
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
// Check clearance in all directions
|
||||
let mut clear = true;
|
||||
for angle in 0..8 {
|
||||
let a = angle as f32 * PI / 4.0;
|
||||
for dist in [0.3, 0.6, 0.9] {
|
||||
if sim_map.is_occupied(x + dist * a.cos(), y + dist * a.sin()) {
|
||||
clear = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if !clear {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if clear {
|
||||
return Pose2D::new(x, y, 0.0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Fallback
|
||||
Pose2D::new(
|
||||
sim_origin.0 + sim_width_m / 2.0,
|
||||
sim_origin.1 + sim_height_m / 2.0,
|
||||
0.0,
|
||||
)
|
||||
}
|
||||
|
||||
/// Run a scenario-based test from YAML specification
|
||||
fn run_scenario_test(
|
||||
sim_map: &SimulationMap,
|
||||
scenario: &Scenario,
|
||||
vastu_config: &VastuConfig,
|
||||
) -> TestResults {
|
||||
println!("\n=== Running Scenario: {} ===", scenario.name);
|
||||
if !scenario.description.is_empty() {
|
||||
println!("{}", scenario.description);
|
||||
}
|
||||
|
||||
// Determine start pose
|
||||
let start_pose = if let Some(ref sp) = scenario.start_pose {
|
||||
Pose2D::new(sp.x, sp.y, sp.theta)
|
||||
} else {
|
||||
find_start_position(sim_map)
|
||||
};
|
||||
|
||||
println!(
|
||||
"Start position: ({:.2}, {:.2}, {:.1}°)",
|
||||
start_pose.x,
|
||||
start_pose.y,
|
||||
start_pose.theta.to_degrees()
|
||||
);
|
||||
|
||||
// Setup simulation
|
||||
let map_config = vastu_config.to_map_config();
|
||||
let mut map = OccupancyGridMap::new(map_config);
|
||||
|
||||
let robot_config = RobotConfig::default();
|
||||
let mut physics =
|
||||
PhysicsState::new(start_pose.x, start_pose.y, start_pose.theta, &robot_config);
|
||||
|
||||
let encoder_config = EncoderConfig::default();
|
||||
let encoder_noise = NoiseGenerator::new(42);
|
||||
let mut encoder =
|
||||
EncoderSimulator::new(&encoder_config, robot_config.ticks_per_meter, encoder_noise);
|
||||
|
||||
let mut pure_odometry = WheelOdometry::new(
|
||||
robot_config.wheel_base,
|
||||
robot_config.ticks_per_meter,
|
||||
start_pose,
|
||||
);
|
||||
let mut corrected_odometry = WheelOdometry::new(
|
||||
robot_config.wheel_base,
|
||||
robot_config.ticks_per_meter,
|
||||
start_pose,
|
||||
);
|
||||
|
||||
let lidar_config = LidarConfig::default();
|
||||
|
||||
let matcher_config = CorrelativeMatcherConfig {
|
||||
sensor_offset: (lidar_config.mounting_x, lidar_config.mounting_y),
|
||||
..CorrelativeMatcherConfig::default()
|
||||
};
|
||||
let matcher = CorrelativeMatcher::new(matcher_config);
|
||||
let lidar_noise = NoiseGenerator::new(43);
|
||||
let mut lidar = LidarSimulator::new(&lidar_config, lidar_noise);
|
||||
|
||||
// Simulation parameters
|
||||
let dt = 0.05; // 20Hz physics
|
||||
let lidar_interval = 0.2; // 5Hz lidar
|
||||
|
||||
let mut time_since_lidar = lidar_interval;
|
||||
let mut corrected_pose = start_pose;
|
||||
let mut scans: Vec<ScanMetrics> = Vec::new();
|
||||
|
||||
// Phase 1: Seed the map
|
||||
if scenario.seed.rotation {
|
||||
println!(
|
||||
"Seeding map with {} rotational scans...",
|
||||
scenario.seed.scans
|
||||
);
|
||||
for i in 0..scenario.seed.scans {
|
||||
let angle = i as f32 * TAU / scenario.seed.scans as f32;
|
||||
let seed_pose = Pose2D::new(start_pose.x, start_pose.y, angle);
|
||||
let scan_data = lidar.generate_scan(sim_map, seed_pose.x, seed_pose.y, seed_pose.theta);
|
||||
if !scan_data.is_empty() {
|
||||
let ranges: Vec<f32> = scan_data.iter().map(|(_, d, _)| *d).collect();
|
||||
let angles: Vec<f32> = scan_data
|
||||
.iter()
|
||||
.map(|(a, _, _)| a + lidar_config.angle_offset)
|
||||
.collect();
|
||||
let scan = LidarScan::new(
|
||||
ranges,
|
||||
angles,
|
||||
lidar_config.min_range,
|
||||
lidar_config.max_range,
|
||||
);
|
||||
map.observe_lidar(&scan, seed_pose);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
let map_stats = map.coverage_stats();
|
||||
println!(
|
||||
"Map seeded: {} wall cells, {} floor cells",
|
||||
map_stats.wall_cells, map_stats.floor_cells
|
||||
);
|
||||
|
||||
// Phase 2: Execute scenario commands
|
||||
println!(
|
||||
"Executing {} commands (total: {:.1}s)...",
|
||||
scenario.commands.len(),
|
||||
scenario.total_duration_secs()
|
||||
);
|
||||
|
||||
for (cmd_idx, command) in scenario.commands.iter().enumerate() {
|
||||
let (linear_vel, angular_vel) = command.velocities();
|
||||
let duration_secs = command.duration_secs();
|
||||
let steps = (duration_secs / dt) as usize;
|
||||
|
||||
let cmd_type = match command {
|
||||
Command::Move { .. } => "move",
|
||||
Command::Stay { .. } => "stay",
|
||||
};
|
||||
println!(
|
||||
" Command {}: {} (v={:.2}m/s, w={:.2}rad/s) for {:.1}s",
|
||||
cmd_idx + 1,
|
||||
cmd_type,
|
||||
linear_vel,
|
||||
angular_vel,
|
||||
duration_secs
|
||||
);
|
||||
|
||||
for _ in 0..steps {
|
||||
// Update encoders
|
||||
let (left_vel, right_vel) =
|
||||
physics.wheel_velocities(linear_vel, angular_vel, robot_config.wheel_base);
|
||||
let (left_ticks, right_ticks) = encoder.update(left_vel, right_vel, dt);
|
||||
|
||||
pure_odometry.update(left_ticks, right_ticks);
|
||||
corrected_odometry.update(left_ticks, right_ticks);
|
||||
|
||||
// Update physics
|
||||
let collision = physics.update(dt, linear_vel, angular_vel, sim_map, &robot_config);
|
||||
if collision {
|
||||
println!("Warning: Collision detected, stopping command early");
|
||||
break;
|
||||
}
|
||||
|
||||
// Lidar scan
|
||||
time_since_lidar += dt;
|
||||
if time_since_lidar >= lidar_interval {
|
||||
time_since_lidar = 0.0;
|
||||
|
||||
let gt_pose = Pose2D::new(physics.x(), physics.y(), physics.theta());
|
||||
let pure_odom_pose = pure_odometry.pose();
|
||||
let corrected_odom_pose = corrected_odometry.pose();
|
||||
|
||||
let scan_data = lidar.generate_scan(sim_map, gt_pose.x, gt_pose.y, gt_pose.theta);
|
||||
if !scan_data.is_empty() {
|
||||
let ranges: Vec<f32> = scan_data.iter().map(|(_, d, _)| *d).collect();
|
||||
let angles: Vec<f32> = scan_data
|
||||
.iter()
|
||||
.map(|(a, _, _)| a + lidar_config.angle_offset)
|
||||
.collect();
|
||||
let scan = LidarScan::new(
|
||||
ranges,
|
||||
angles,
|
||||
lidar_config.min_range,
|
||||
lidar_config.max_range,
|
||||
);
|
||||
|
||||
let match_result =
|
||||
matcher.match_scan(&scan, corrected_odom_pose, map.storage());
|
||||
|
||||
let match_dist = ((match_result.pose.x - corrected_odom_pose.x).powi(2)
|
||||
+ (match_result.pose.y - corrected_odom_pose.y).powi(2))
|
||||
.sqrt();
|
||||
|
||||
let valid_match = match_result.converged
|
||||
&& match_result.hit_ratio() > 0.3
|
||||
&& match_dist < 0.5;
|
||||
|
||||
if valid_match {
|
||||
corrected_pose = match_result.pose;
|
||||
corrected_odometry.reset_to(corrected_pose);
|
||||
} else {
|
||||
corrected_pose = corrected_odom_pose;
|
||||
}
|
||||
|
||||
map.observe_lidar(&scan, corrected_pose);
|
||||
|
||||
scans.push(ScanMetrics {
|
||||
gt_pose,
|
||||
odom_pose: pure_odom_pose,
|
||||
corrected_pose,
|
||||
match_score: match_result.score,
|
||||
hit_ratio: match_result.hit_ratio(),
|
||||
converged: match_result.converged,
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
let map_stats = map.coverage_stats();
|
||||
|
||||
// Use the last scan's poses for fair comparison
|
||||
// (corrected_pose is from the last scan, so odom should be too)
|
||||
let (odom_final, corrected_final, gt_final) = if let Some(last_scan) = scans.last() {
|
||||
(
|
||||
last_scan.odom_pose,
|
||||
last_scan.corrected_pose,
|
||||
last_scan.gt_pose,
|
||||
)
|
||||
} else {
|
||||
// Fallback if no scans
|
||||
(
|
||||
pure_odometry.pose(),
|
||||
corrected_pose,
|
||||
Pose2D::new(physics.x(), physics.y(), physics.theta()),
|
||||
)
|
||||
};
|
||||
|
||||
TestResults {
|
||||
test_name: scenario.name.clone(),
|
||||
start_pose,
|
||||
gt_final,
|
||||
odom_final,
|
||||
corrected_final,
|
||||
scans,
|
||||
wall_cells: map_stats.wall_cells,
|
||||
map,
|
||||
}
|
||||
}
|
||||
|
||||
/// Generate SVG audit file
|
||||
fn save_output(results: &TestResults, output_dir: &Path, scenario: &Scenario) {
|
||||
println!("\nGenerating SVG audit file...");
|
||||
|
||||
// Collect trajectories from scan metrics
|
||||
let gt_poses: Vec<Pose2D> = results.scans.iter().map(|s| s.gt_pose).collect();
|
||||
let est_poses: Vec<Pose2D> = results.scans.iter().map(|s| s.corrected_pose).collect();
|
||||
let odom_poses: Vec<Pose2D> = results.scans.iter().map(|s| s.odom_pose).collect();
|
||||
|
||||
// Calculate markers based on scenario config
|
||||
let marker_indices = if let Some(interval_m) = scenario.svg_output.marker_interval_m {
|
||||
markers_by_distance(>_poses, interval_m)
|
||||
} else if let Some(interval_s) = scenario.svg_output.marker_interval_s {
|
||||
markers_by_time(gt_poses.len(), scenario.total_duration_secs(), interval_s)
|
||||
} else {
|
||||
markers_by_distance(>_poses, 0.5)
|
||||
};
|
||||
|
||||
let svg_config = SvgConfig {
|
||||
scale: scenario.svg_output.scale,
|
||||
trajectory_width: scenario.svg_output.trajectory_width,
|
||||
..SvgConfig::default()
|
||||
};
|
||||
|
||||
let visualizer = SvgVisualizer::new(results.map.storage().clone(), svg_config)
|
||||
.with_title(&scenario.name)
|
||||
.with_ground_truth(gt_poses, marker_indices.clone())
|
||||
.with_estimated(est_poses, marker_indices.clone())
|
||||
.with_odometry(odom_poses, marker_indices);
|
||||
|
||||
let svg_filename = scenario.svg_filename();
|
||||
let svg_path = output_dir.join(&svg_filename);
|
||||
|
||||
if let Err(e) = visualizer.save(&svg_path) {
|
||||
eprintln!(" Failed to save SVG: {}", e);
|
||||
} else {
|
||||
println!(" Saved: {}", svg_path.display());
|
||||
}
|
||||
}
|
||||
|
||||
/// Print test results
|
||||
fn print_results(results: &TestResults) {
|
||||
println!("\n=== {} Results ===", results.test_name);
|
||||
println!("Scans collected: {}", results.scans.len());
|
||||
println!("Map wall cells: {}", results.wall_cells);
|
||||
|
||||
// Pose comparison
|
||||
println!("\nPose Trajectory:");
|
||||
println!(
|
||||
" Start: ({:7.3}, {:7.3}, {:7.2}°)",
|
||||
results.start_pose.x,
|
||||
results.start_pose.y,
|
||||
results.start_pose.theta.to_degrees()
|
||||
);
|
||||
println!(
|
||||
" GT Final: ({:7.3}, {:7.3}, {:7.2}°)",
|
||||
results.gt_final.x,
|
||||
results.gt_final.y,
|
||||
results.gt_final.theta.to_degrees()
|
||||
);
|
||||
println!(
|
||||
" Odom Final:({:7.3}, {:7.3}, {:7.2}°)",
|
||||
results.odom_final.x,
|
||||
results.odom_final.y,
|
||||
results.odom_final.theta.to_degrees()
|
||||
);
|
||||
println!(
|
||||
" Corrected: ({:7.3}, {:7.3}, {:7.2}°)",
|
||||
results.corrected_final.x,
|
||||
results.corrected_final.y,
|
||||
results.corrected_final.theta.to_degrees()
|
||||
);
|
||||
|
||||
// Calculate errors
|
||||
let odom_pos_error = ((results.odom_final.x - results.gt_final.x).powi(2)
|
||||
+ (results.odom_final.y - results.gt_final.y).powi(2))
|
||||
.sqrt();
|
||||
let odom_ang_error = normalize_angle(results.odom_final.theta - results.gt_final.theta).abs();
|
||||
|
||||
let corr_pos_error = ((results.corrected_final.x - results.gt_final.x).powi(2)
|
||||
+ (results.corrected_final.y - results.gt_final.y).powi(2))
|
||||
.sqrt();
|
||||
let corr_ang_error =
|
||||
normalize_angle(results.corrected_final.theta - results.gt_final.theta).abs();
|
||||
|
||||
println!("\nFinal Errors (vs Ground Truth):");
|
||||
println!(
|
||||
" Encoder: position = {:6.4}m, heading = {:6.2}°",
|
||||
odom_pos_error,
|
||||
odom_ang_error.to_degrees()
|
||||
);
|
||||
println!(
|
||||
" Corrected: position = {:6.4}m, heading = {:6.2}°",
|
||||
corr_pos_error,
|
||||
corr_ang_error.to_degrees()
|
||||
);
|
||||
|
||||
// Improvement ratio
|
||||
if odom_pos_error > 0.0 {
|
||||
let pos_improvement = (odom_pos_error - corr_pos_error) / odom_pos_error * 100.0;
|
||||
println!("\n Position improvement: {:+.1}%", pos_improvement);
|
||||
}
|
||||
if odom_ang_error > 0.0 {
|
||||
let ang_improvement = (odom_ang_error - corr_ang_error) / odom_ang_error * 100.0;
|
||||
println!(" Heading improvement: {:+.1}%", ang_improvement);
|
||||
}
|
||||
|
||||
// Scan matching statistics
|
||||
let converged_count = results.scans.iter().filter(|s| s.converged).count();
|
||||
let avg_score: f32 = if !results.scans.is_empty() {
|
||||
results.scans.iter().map(|s| s.match_score).sum::<f32>() / results.scans.len() as f32
|
||||
} else {
|
||||
0.0
|
||||
};
|
||||
let avg_hit_ratio: f32 = if !results.scans.is_empty() {
|
||||
results.scans.iter().map(|s| s.hit_ratio).sum::<f32>() / results.scans.len() as f32
|
||||
} else {
|
||||
0.0
|
||||
};
|
||||
|
||||
println!("\nScan Matching Statistics:");
|
||||
println!(
|
||||
" Success rate: {}/{} ({:.1}%)",
|
||||
converged_count,
|
||||
results.scans.len(),
|
||||
if results.scans.is_empty() {
|
||||
0.0
|
||||
} else {
|
||||
100.0 * converged_count as f32 / results.scans.len() as f32
|
||||
}
|
||||
);
|
||||
println!(" Avg score: {:.3}", avg_score);
|
||||
println!(" Avg hit ratio:{:.1}%", avg_hit_ratio * 100.0);
|
||||
|
||||
// Per-scan error analysis
|
||||
if !results.scans.is_empty() {
|
||||
let mut odom_errors: Vec<f32> = Vec::new();
|
||||
let mut corr_errors: Vec<f32> = Vec::new();
|
||||
|
||||
for scan in &results.scans {
|
||||
let odom_err = ((scan.odom_pose.x - scan.gt_pose.x).powi(2)
|
||||
+ (scan.odom_pose.y - scan.gt_pose.y).powi(2))
|
||||
.sqrt();
|
||||
let corr_err = ((scan.corrected_pose.x - scan.gt_pose.x).powi(2)
|
||||
+ (scan.corrected_pose.y - scan.gt_pose.y).powi(2))
|
||||
.sqrt();
|
||||
odom_errors.push(odom_err);
|
||||
corr_errors.push(corr_err);
|
||||
}
|
||||
|
||||
let odom_max = odom_errors.iter().cloned().fold(0.0f32, f32::max);
|
||||
let corr_max = corr_errors.iter().cloned().fold(0.0f32, f32::max);
|
||||
let odom_avg = odom_errors.iter().sum::<f32>() / odom_errors.len() as f32;
|
||||
let corr_avg = corr_errors.iter().sum::<f32>() / corr_errors.len() as f32;
|
||||
|
||||
println!("\nPer-Scan Position Error:");
|
||||
println!(
|
||||
" Encoder: avg = {:6.4}m, max = {:6.4}m",
|
||||
odom_avg, odom_max
|
||||
);
|
||||
println!(
|
||||
" Corrected: avg = {:6.4}m, max = {:6.4}m",
|
||||
corr_avg, corr_max
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
/// Normalize angle to [-PI, PI)
|
||||
fn normalize_angle(angle: f32) -> f32 {
|
||||
let mut a = angle % TAU;
|
||||
if a > PI {
|
||||
a -= TAU;
|
||||
} else if a < -PI {
|
||||
a += TAU;
|
||||
}
|
||||
a
|
||||
}
|
||||
@@ -1,113 +0,0 @@
|
||||
//! Default value functions for serde deserialization.
|
||||
|
||||
pub fn resolution() -> f32 {
|
||||
0.025
|
||||
}
|
||||
|
||||
pub fn grid_size() -> usize {
|
||||
800
|
||||
}
|
||||
|
||||
pub fn origin_mode() -> String {
|
||||
"center".to_string()
|
||||
}
|
||||
|
||||
pub fn enabled() -> bool {
|
||||
true
|
||||
}
|
||||
|
||||
pub fn lidar_offset_x() -> f32 {
|
||||
-0.110
|
||||
}
|
||||
|
||||
pub fn min_range() -> f32 {
|
||||
0.15
|
||||
}
|
||||
|
||||
pub fn max_range() -> f32 {
|
||||
8.0
|
||||
}
|
||||
|
||||
pub fn robot_radius() -> f32 {
|
||||
0.17
|
||||
}
|
||||
|
||||
pub fn left_bumper_angle() -> f32 {
|
||||
0.5
|
||||
}
|
||||
|
||||
pub fn right_bumper_angle() -> f32 {
|
||||
-0.5
|
||||
}
|
||||
|
||||
pub fn min_frontier_size() -> usize {
|
||||
5
|
||||
}
|
||||
|
||||
pub fn safety_margin() -> f32 {
|
||||
0.05
|
||||
}
|
||||
|
||||
pub fn diagonal_cost() -> f32 {
|
||||
std::f32::consts::SQRT_2
|
||||
}
|
||||
|
||||
pub fn max_iterations() -> usize {
|
||||
100_000
|
||||
}
|
||||
|
||||
pub fn los_step() -> f32 {
|
||||
0.05
|
||||
}
|
||||
|
||||
pub fn smooth_iterations() -> usize {
|
||||
100
|
||||
}
|
||||
|
||||
pub fn frontier_threshold() -> f32 {
|
||||
0.3
|
||||
}
|
||||
|
||||
pub fn waypoint_threshold() -> f32 {
|
||||
0.1
|
||||
}
|
||||
|
||||
pub fn max_plan_distance() -> f32 {
|
||||
5.0
|
||||
}
|
||||
|
||||
pub fn replan_interval() -> f32 {
|
||||
1.0
|
||||
}
|
||||
|
||||
pub fn max_failures() -> usize {
|
||||
5
|
||||
}
|
||||
|
||||
pub fn turn_angle() -> f32 {
|
||||
std::f32::consts::FRAC_PI_4
|
||||
}
|
||||
|
||||
pub fn backup_distance() -> f32 {
|
||||
0.1
|
||||
}
|
||||
|
||||
pub fn output_format() -> String {
|
||||
"both".to_string()
|
||||
}
|
||||
|
||||
pub fn output_dir() -> String {
|
||||
"./output".to_string()
|
||||
}
|
||||
|
||||
pub fn motion_max_time() -> f32 {
|
||||
5.0
|
||||
}
|
||||
|
||||
pub fn motion_max_distance() -> f32 {
|
||||
0.2
|
||||
}
|
||||
|
||||
pub fn motion_max_angle() -> f32 {
|
||||
0.17
|
||||
}
|
||||
@@ -1,82 +0,0 @@
|
||||
//! Exploration configuration section.
|
||||
|
||||
use serde::{Deserialize, Serialize};
|
||||
|
||||
use super::defaults;
|
||||
|
||||
/// Exploration settings section
|
||||
#[derive(Clone, Debug, Serialize, Deserialize)]
|
||||
pub struct ExplorationSection {
|
||||
/// Minimum cells for valid frontier
|
||||
#[serde(default = "defaults::min_frontier_size")]
|
||||
pub min_frontier_size: usize,
|
||||
|
||||
/// Distance to consider frontier reached (meters)
|
||||
#[serde(default = "defaults::frontier_threshold")]
|
||||
pub frontier_reached_threshold: f32,
|
||||
|
||||
/// Distance for waypoint reached (meters)
|
||||
#[serde(default = "defaults::waypoint_threshold")]
|
||||
pub waypoint_threshold: f32,
|
||||
|
||||
/// Maximum planning distance (meters)
|
||||
#[serde(default = "defaults::max_plan_distance")]
|
||||
pub max_plan_distance: f32,
|
||||
|
||||
/// Seconds between replans when blocked
|
||||
#[serde(default = "defaults::replan_interval")]
|
||||
pub replan_interval: f32,
|
||||
|
||||
/// Max consecutive planning failures before giving up
|
||||
#[serde(default = "defaults::max_failures")]
|
||||
pub max_consecutive_failures: usize,
|
||||
|
||||
/// Penalty for paths near obstacles
|
||||
#[serde(default)]
|
||||
pub path_clearance_weight: f32,
|
||||
|
||||
/// Enable path smoothing
|
||||
#[serde(default = "defaults::enabled")]
|
||||
pub smooth_paths: bool,
|
||||
|
||||
/// Recovery behavior settings
|
||||
#[serde(default)]
|
||||
pub recovery: RecoverySettings,
|
||||
}
|
||||
|
||||
impl Default for ExplorationSection {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
min_frontier_size: 5,
|
||||
frontier_reached_threshold: 0.3,
|
||||
waypoint_threshold: 0.1,
|
||||
max_plan_distance: 5.0,
|
||||
replan_interval: 1.0,
|
||||
max_consecutive_failures: 5,
|
||||
path_clearance_weight: 0.0,
|
||||
smooth_paths: true,
|
||||
recovery: RecoverySettings::default(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Recovery behavior settings
|
||||
#[derive(Clone, Debug, Serialize, Deserialize)]
|
||||
pub struct RecoverySettings {
|
||||
/// Turn angle for recovery (radians)
|
||||
#[serde(default = "defaults::turn_angle")]
|
||||
pub turn_angle: f32,
|
||||
|
||||
/// Backup distance for recovery (meters)
|
||||
#[serde(default = "defaults::backup_distance")]
|
||||
pub backup_distance: f32,
|
||||
}
|
||||
|
||||
impl Default for RecoverySettings {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
turn_angle: std::f32::consts::FRAC_PI_4,
|
||||
backup_distance: 0.1,
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,31 +0,0 @@
|
||||
//! Unified configuration loading for VastuMap.
|
||||
//!
|
||||
//! Loads all configuration from a single YAML file.
|
||||
|
||||
mod defaults;
|
||||
mod error;
|
||||
mod exploration;
|
||||
mod grid;
|
||||
mod motion_filter;
|
||||
mod pathfinding;
|
||||
mod persistence;
|
||||
mod query;
|
||||
mod sensor;
|
||||
mod slam;
|
||||
mod vastu;
|
||||
|
||||
// Re-export main types
|
||||
pub use error::ConfigLoadError;
|
||||
pub use vastu::VastuConfig;
|
||||
|
||||
// Re-export section types
|
||||
pub use exploration::{ExplorationSection, RecoverySettings};
|
||||
pub use grid::GridSection;
|
||||
pub use motion_filter::MotionFilterSection;
|
||||
pub use pathfinding::{AStarSettings, PathfindingSection, SmoothingSettings};
|
||||
pub use persistence::PersistenceSection;
|
||||
pub use query::{FrontierSettings, QuerySection, TraversabilitySettings};
|
||||
pub use sensor::{
|
||||
BumperSettings, CliffSensorSettings, LidarSettings, RobotSettings, SensorSection,
|
||||
};
|
||||
pub use slam::SlamSection;
|
||||
@@ -1,53 +0,0 @@
|
||||
//! Motion filter configuration section.
|
||||
|
||||
use serde::{Deserialize, Serialize};
|
||||
|
||||
use crate::explore::MotionFilterConfig;
|
||||
|
||||
use super::defaults;
|
||||
|
||||
/// Motion filter settings section
|
||||
///
|
||||
/// Gates scan processing based on robot movement to reduce redundant observations.
|
||||
/// A scan is processed if ANY threshold is exceeded.
|
||||
#[derive(Clone, Debug, Serialize, Deserialize)]
|
||||
pub struct MotionFilterSection {
|
||||
/// Whether motion filtering is enabled
|
||||
#[serde(default = "defaults::enabled")]
|
||||
pub enabled: bool,
|
||||
|
||||
/// Maximum time between scan insertions (seconds)
|
||||
#[serde(default = "defaults::motion_max_time")]
|
||||
pub max_time_secs: f32,
|
||||
|
||||
/// Maximum distance before forcing scan insertion (meters)
|
||||
#[serde(default = "defaults::motion_max_distance")]
|
||||
pub max_distance: f32,
|
||||
|
||||
/// Maximum rotation before forcing scan insertion (radians)
|
||||
#[serde(default = "defaults::motion_max_angle")]
|
||||
pub max_angle: f32,
|
||||
}
|
||||
|
||||
impl Default for MotionFilterSection {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
enabled: true,
|
||||
max_time_secs: defaults::motion_max_time(),
|
||||
max_distance: defaults::motion_max_distance(),
|
||||
max_angle: defaults::motion_max_angle(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl MotionFilterSection {
|
||||
/// Convert to MotionFilterConfig
|
||||
pub fn to_motion_filter_config(&self) -> MotionFilterConfig {
|
||||
MotionFilterConfig {
|
||||
enabled: self.enabled,
|
||||
max_time_secs: self.max_time_secs,
|
||||
max_distance: self.max_distance,
|
||||
max_angle: self.max_angle,
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,74 +0,0 @@
|
||||
//! Pathfinding configuration section.
|
||||
|
||||
use serde::{Deserialize, Serialize};
|
||||
|
||||
use super::defaults;
|
||||
|
||||
/// Pathfinding settings section
|
||||
#[derive(Clone, Debug, Serialize, Deserialize, Default)]
|
||||
pub struct PathfindingSection {
|
||||
/// A* algorithm settings
|
||||
#[serde(default)]
|
||||
pub astar: AStarSettings,
|
||||
|
||||
/// Path smoothing settings
|
||||
#[serde(default)]
|
||||
pub smoothing: SmoothingSettings,
|
||||
}
|
||||
|
||||
/// A* algorithm settings
|
||||
#[derive(Clone, Debug, Serialize, Deserialize)]
|
||||
pub struct AStarSettings {
|
||||
/// Enable 8-directional movement
|
||||
#[serde(default = "defaults::enabled")]
|
||||
pub allow_diagonal: bool,
|
||||
|
||||
/// Cost multiplier for diagonal moves (sqrt(2))
|
||||
#[serde(default = "defaults::diagonal_cost")]
|
||||
pub diagonal_cost: f32,
|
||||
|
||||
/// Maximum nodes to expand
|
||||
#[serde(default = "defaults::max_iterations")]
|
||||
pub max_iterations: usize,
|
||||
|
||||
/// Penalty for paths near obstacles (0=disabled)
|
||||
#[serde(default)]
|
||||
pub clearance_weight: f32,
|
||||
}
|
||||
|
||||
impl Default for AStarSettings {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
allow_diagonal: true,
|
||||
diagonal_cost: std::f32::consts::SQRT_2,
|
||||
max_iterations: 100_000,
|
||||
clearance_weight: 0.0,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Path smoothing settings
|
||||
#[derive(Clone, Debug, Serialize, Deserialize)]
|
||||
pub struct SmoothingSettings {
|
||||
/// Enable path smoothing
|
||||
#[serde(default = "defaults::enabled")]
|
||||
pub enabled: bool,
|
||||
|
||||
/// Line-of-sight check step (meters)
|
||||
#[serde(default = "defaults::los_step")]
|
||||
pub los_step_size: f32,
|
||||
|
||||
/// Midpoint smoothing iterations
|
||||
#[serde(default = "defaults::smooth_iterations")]
|
||||
pub max_iterations: usize,
|
||||
}
|
||||
|
||||
impl Default for SmoothingSettings {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
enabled: true,
|
||||
los_step_size: 0.05,
|
||||
max_iterations: 100,
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,54 +0,0 @@
|
||||
//! Query configuration section.
|
||||
|
||||
use serde::{Deserialize, Serialize};
|
||||
|
||||
use super::defaults;
|
||||
|
||||
/// Query settings section
|
||||
#[derive(Clone, Debug, Serialize, Deserialize, Default)]
|
||||
pub struct QuerySection {
|
||||
/// Frontier detection settings
|
||||
#[serde(default)]
|
||||
pub frontier: FrontierSettings,
|
||||
|
||||
/// Traversability checking settings
|
||||
#[serde(default)]
|
||||
pub traversability: TraversabilitySettings,
|
||||
}
|
||||
|
||||
/// Frontier detection settings
|
||||
#[derive(Clone, Debug, Serialize, Deserialize)]
|
||||
pub struct FrontierSettings {
|
||||
/// Minimum cells for valid frontier
|
||||
#[serde(default = "defaults::min_frontier_size")]
|
||||
pub min_frontier_size: usize,
|
||||
|
||||
/// Use 8-connected grid (vs 4-connected)
|
||||
#[serde(default = "defaults::enabled")]
|
||||
pub use_8_connectivity: bool,
|
||||
}
|
||||
|
||||
impl Default for FrontierSettings {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
min_frontier_size: 5,
|
||||
use_8_connectivity: true,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Traversability settings
|
||||
#[derive(Clone, Debug, Serialize, Deserialize)]
|
||||
pub struct TraversabilitySettings {
|
||||
/// Extra clearance around robot (meters)
|
||||
#[serde(default = "defaults::safety_margin")]
|
||||
pub safety_margin: f32,
|
||||
}
|
||||
|
||||
impl Default for TraversabilitySettings {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
safety_margin: 0.05,
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,207 +0,0 @@
|
||||
//! Main VastuConfig and conversion methods.
|
||||
|
||||
use std::path::Path;
|
||||
|
||||
use serde::{Deserialize, Serialize};
|
||||
|
||||
use crate::explore::{ExplorationConfig, ExplorerConfig};
|
||||
use crate::grid::MapConfig;
|
||||
use crate::pathfinding::{AStarConfig, SmoothingConfig};
|
||||
use crate::query::{FrontierDetector, RobotFootprint};
|
||||
use crate::slam::{
|
||||
BackgroundOptimizerConfig, CorrelativeMatcherConfig, LoopClosureConfig, PoseGraphConfig,
|
||||
};
|
||||
|
||||
use super::error::ConfigLoadError;
|
||||
use super::exploration::ExplorationSection;
|
||||
use super::grid::GridSection;
|
||||
use super::motion_filter::MotionFilterSection;
|
||||
use super::pathfinding::PathfindingSection;
|
||||
use super::persistence::PersistenceSection;
|
||||
use super::query::QuerySection;
|
||||
use super::sensor::SensorSection;
|
||||
use super::slam::SlamSection;
|
||||
|
||||
/// Full VastuMap configuration loaded from YAML
|
||||
#[derive(Clone, Debug, Serialize, Deserialize, Default)]
|
||||
pub struct VastuConfig {
|
||||
/// Grid settings
|
||||
#[serde(default)]
|
||||
pub grid: GridSection,
|
||||
|
||||
/// Sensor settings
|
||||
#[serde(default)]
|
||||
pub sensor: SensorSection,
|
||||
|
||||
/// SLAM settings (Cartographer-style defaults)
|
||||
#[serde(default)]
|
||||
pub slam: SlamSection,
|
||||
|
||||
/// Query settings
|
||||
#[serde(default)]
|
||||
pub query: QuerySection,
|
||||
|
||||
/// Pathfinding settings
|
||||
#[serde(default)]
|
||||
pub pathfinding: PathfindingSection,
|
||||
|
||||
/// Motion filter settings
|
||||
#[serde(default)]
|
||||
pub motion_filter: MotionFilterSection,
|
||||
|
||||
/// Exploration settings
|
||||
#[serde(default)]
|
||||
pub exploration: ExplorationSection,
|
||||
|
||||
/// Persistence settings
|
||||
#[serde(default)]
|
||||
pub persistence: PersistenceSection,
|
||||
}
|
||||
|
||||
impl VastuConfig {
|
||||
/// Load configuration from a YAML file
|
||||
pub fn load(path: &Path) -> Result<Self, ConfigLoadError> {
|
||||
let contents =
|
||||
std::fs::read_to_string(path).map_err(|e| ConfigLoadError::Io(e.to_string()))?;
|
||||
Self::from_yaml(&contents)
|
||||
}
|
||||
|
||||
/// Load from default config path (configs/config.yaml)
|
||||
pub fn load_default() -> Result<Self, ConfigLoadError> {
|
||||
let path = Path::new("configs/config.yaml");
|
||||
if path.exists() {
|
||||
Self::load(path)
|
||||
} else {
|
||||
Ok(Self::default())
|
||||
}
|
||||
}
|
||||
|
||||
/// Parse from YAML string
|
||||
pub fn from_yaml(yaml: &str) -> Result<Self, ConfigLoadError> {
|
||||
serde_yaml::from_str(yaml).map_err(|e| ConfigLoadError::Parse(e.to_string()))
|
||||
}
|
||||
|
||||
/// Convert to MapConfig for OccupancyGridMap
|
||||
pub fn to_map_config(&self) -> MapConfig {
|
||||
MapConfig {
|
||||
grid: self.grid.to_grid_config(),
|
||||
sensor: self.sensor.to_sensor_config(),
|
||||
log_odds: self.slam.log_odds.clone(),
|
||||
}
|
||||
}
|
||||
|
||||
/// Get the correlative matcher config
|
||||
pub fn correlative_matcher_config(&self) -> CorrelativeMatcherConfig {
|
||||
self.slam.correlative.clone()
|
||||
}
|
||||
|
||||
/// Get the loop closure config
|
||||
pub fn loop_closure_config(&self) -> LoopClosureConfig {
|
||||
self.slam.loop_closure.clone()
|
||||
}
|
||||
|
||||
/// Get the pose graph config
|
||||
pub fn pose_graph_config(&self) -> PoseGraphConfig {
|
||||
self.slam.pose_graph.clone()
|
||||
}
|
||||
|
||||
/// Get the background optimizer config
|
||||
pub fn background_optimizer_config(&self) -> BackgroundOptimizerConfig {
|
||||
self.slam.background_optimizer.clone()
|
||||
}
|
||||
|
||||
/// Get robot footprint
|
||||
pub fn robot_footprint(&self) -> RobotFootprint {
|
||||
RobotFootprint::new(
|
||||
self.sensor.robot.radius,
|
||||
self.query.traversability.safety_margin,
|
||||
)
|
||||
}
|
||||
|
||||
/// Convert to ExplorationConfig
|
||||
pub fn to_exploration_config(&self) -> ExplorationConfig {
|
||||
ExplorationConfig {
|
||||
footprint: self.robot_footprint(),
|
||||
min_frontier_size: self.exploration.min_frontier_size,
|
||||
frontier_reached_threshold: self.exploration.frontier_reached_threshold,
|
||||
waypoint_threshold: self.exploration.waypoint_threshold,
|
||||
max_plan_distance: self.exploration.max_plan_distance,
|
||||
replan_interval: self.exploration.replan_interval,
|
||||
max_consecutive_failures: self.exploration.max_consecutive_failures,
|
||||
path_clearance_weight: self.exploration.path_clearance_weight,
|
||||
smooth_paths: self.exploration.smooth_paths,
|
||||
recovery_turn_angle: self.exploration.recovery.turn_angle,
|
||||
recovery_backup_distance: self.exploration.recovery.backup_distance,
|
||||
// Use defaults for new fields
|
||||
..Default::default()
|
||||
}
|
||||
}
|
||||
|
||||
/// Convert to AStarConfig
|
||||
pub fn to_astar_config(&self) -> AStarConfig {
|
||||
AStarConfig {
|
||||
footprint: self.robot_footprint(),
|
||||
allow_diagonal: self.pathfinding.astar.allow_diagonal,
|
||||
diagonal_cost: self.pathfinding.astar.diagonal_cost,
|
||||
max_iterations: self.pathfinding.astar.max_iterations,
|
||||
clearance_weight: self.pathfinding.astar.clearance_weight,
|
||||
exploration_mode: false, // Default to safe mode
|
||||
}
|
||||
}
|
||||
|
||||
/// Convert to SmoothingConfig
|
||||
pub fn to_smoothing_config(&self) -> SmoothingConfig {
|
||||
SmoothingConfig {
|
||||
footprint: self.robot_footprint(),
|
||||
los_step_size: self.pathfinding.smoothing.los_step_size,
|
||||
max_iterations: self.pathfinding.smoothing.max_iterations,
|
||||
}
|
||||
}
|
||||
|
||||
/// Convert to ExplorerConfig for VastuExplorer
|
||||
pub fn to_explorer_config(&self) -> ExplorerConfig {
|
||||
ExplorerConfig {
|
||||
grid: self.grid.to_grid_config(),
|
||||
sensor: self.sensor.to_sensor_config(),
|
||||
exploration: self.to_exploration_config(),
|
||||
motion_filter: self.motion_filter.to_motion_filter_config(),
|
||||
..ExplorerConfig::default()
|
||||
}
|
||||
}
|
||||
|
||||
/// Create frontier detector with config settings
|
||||
pub fn frontier_detector(&self) -> FrontierDetector {
|
||||
FrontierDetector {
|
||||
min_frontier_size: self.query.frontier.min_frontier_size,
|
||||
use_8_connectivity: self.query.frontier.use_8_connectivity,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn test_default_config() {
|
||||
let config = VastuConfig::default();
|
||||
assert_eq!(config.grid.resolution, 0.025);
|
||||
assert_eq!(config.sensor.robot.radius, 0.17);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_yaml_roundtrip() {
|
||||
let config = VastuConfig::default();
|
||||
let yaml = serde_yaml::to_string(&config).unwrap();
|
||||
let parsed: VastuConfig = serde_yaml::from_str(&yaml).unwrap();
|
||||
assert_eq!(parsed.grid.resolution, config.grid.resolution);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_to_map_config() {
|
||||
let config = VastuConfig::default();
|
||||
let map_config = config.to_map_config();
|
||||
assert_eq!(map_config.grid.resolution, 0.025);
|
||||
assert_eq!(map_config.sensor.robot_radius, 0.17);
|
||||
}
|
||||
}
|
||||
@@ -1,21 +0,0 @@
|
||||
//! Core types for the vastu-map occupancy grid library.
|
||||
//!
|
||||
//! This module provides the fundamental types used throughout the library:
|
||||
//! - [`Cell`] and [`CellType`]: Grid cell representation with semantic types
|
||||
//! - [`GridCoord`] and [`WorldPoint`]: Coordinate types
|
||||
//! - [`Pose2D`]: Robot pose (position + orientation)
|
||||
//! - [`PointCloud`]: SIMD-optimized 2D point cloud (SoA layout)
|
||||
//! - Sensor types: [`LidarScan`], [`CliffSensors`], [`BumperSensors`]
|
||||
|
||||
mod cell;
|
||||
mod point;
|
||||
mod pose;
|
||||
mod sensors;
|
||||
|
||||
pub mod simd;
|
||||
|
||||
pub use cell::{Cell, CellType};
|
||||
pub use point::{GridCoord, WorldPoint};
|
||||
pub use pose::{Pose2D, interpolate_pose, normalize_angle};
|
||||
pub use sensors::{BumperSensors, CliffSensors, LidarScan, SensorObservation};
|
||||
pub use simd::{GridCoordBatch, PointCloud, RotationMatrix4};
|
||||
@@ -1,134 +0,0 @@
|
||||
//! Explorer configuration.
|
||||
|
||||
use serde::{Deserialize, Serialize};
|
||||
|
||||
use super::controller::ExplorationConfig;
|
||||
use crate::grid::{GridConfig, MapConfig, SensorConfig};
|
||||
use crate::slam::CorrelativeMatcherConfig;
|
||||
use crate::slam::loop_closure::{LoopClosureConfig, PoseGraphConfig};
|
||||
|
||||
use super::motion_filter::MotionFilterConfig;
|
||||
|
||||
/// Configuration for the VastuExplorer.
|
||||
///
|
||||
/// This combines all the configuration needed for autonomous exploration:
|
||||
/// - Grid/map settings
|
||||
/// - Sensor integration
|
||||
/// - Scan matching
|
||||
/// - Loop closure
|
||||
/// - Exploration behavior
|
||||
#[derive(Clone, Debug, Serialize, Deserialize)]
|
||||
pub struct ExplorerConfig {
|
||||
/// Grid storage configuration.
|
||||
#[serde(default)]
|
||||
pub grid: GridConfig,
|
||||
|
||||
/// Sensor integration configuration.
|
||||
#[serde(default)]
|
||||
pub sensor: SensorConfig,
|
||||
|
||||
/// Exploration controller configuration.
|
||||
#[serde(default)]
|
||||
pub exploration: ExplorationConfig,
|
||||
|
||||
/// Scan-to-map matching configuration.
|
||||
#[serde(default)]
|
||||
pub matching: CorrelativeMatcherConfig,
|
||||
|
||||
/// Loop closure detection configuration.
|
||||
#[serde(default)]
|
||||
pub loop_closure: LoopClosureConfig,
|
||||
|
||||
/// Pose graph optimization configuration.
|
||||
#[serde(default)]
|
||||
pub pose_graph: PoseGraphConfig,
|
||||
|
||||
/// Motion filter configuration.
|
||||
/// Gates scan processing based on robot movement.
|
||||
#[serde(default)]
|
||||
pub motion_filter: MotionFilterConfig,
|
||||
|
||||
/// Control loop update rate in Hz.
|
||||
#[serde(default = "default_update_rate")]
|
||||
pub update_rate_hz: f32,
|
||||
|
||||
/// Maximum exploration time in seconds (0 = unlimited).
|
||||
#[serde(default)]
|
||||
pub max_time_secs: f32,
|
||||
|
||||
/// Stop if battery falls below this percentage.
|
||||
#[serde(default = "default_min_battery")]
|
||||
pub min_battery_percent: u8,
|
||||
}
|
||||
|
||||
fn default_update_rate() -> f32 {
|
||||
10.0 // 10 Hz
|
||||
}
|
||||
|
||||
fn default_min_battery() -> u8 {
|
||||
20 // 20%
|
||||
}
|
||||
|
||||
impl Default for ExplorerConfig {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
grid: GridConfig::default(),
|
||||
sensor: SensorConfig::default(),
|
||||
exploration: ExplorationConfig::default(),
|
||||
matching: CorrelativeMatcherConfig::default(),
|
||||
loop_closure: LoopClosureConfig::default(),
|
||||
pose_graph: PoseGraphConfig::default(),
|
||||
motion_filter: MotionFilterConfig::default(),
|
||||
update_rate_hz: default_update_rate(),
|
||||
max_time_secs: 0.0,
|
||||
min_battery_percent: default_min_battery(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl ExplorerConfig {
|
||||
/// Create a fast configuration for quick testing.
|
||||
pub fn fast() -> Self {
|
||||
Self {
|
||||
matching: CorrelativeMatcherConfig::fast(),
|
||||
update_rate_hz: 5.0,
|
||||
..Default::default()
|
||||
}
|
||||
}
|
||||
|
||||
/// Create a thorough configuration for best quality maps.
|
||||
pub fn thorough() -> Self {
|
||||
Self {
|
||||
matching: CorrelativeMatcherConfig::thorough(),
|
||||
update_rate_hz: 10.0,
|
||||
..Default::default()
|
||||
}
|
||||
}
|
||||
|
||||
/// Convert to MapConfig for OccupancyGridMap creation.
|
||||
pub fn to_map_config(&self) -> MapConfig {
|
||||
MapConfig {
|
||||
grid: self.grid.clone(),
|
||||
sensor: self.sensor.clone(),
|
||||
log_odds: Default::default(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn test_default_config() {
|
||||
let config = ExplorerConfig::default();
|
||||
assert!((config.update_rate_hz - 10.0).abs() < 0.001);
|
||||
assert_eq!(config.min_battery_percent, 20);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_fast_config() {
|
||||
let config = ExplorerConfig::fast();
|
||||
assert!((config.update_rate_hz - 5.0).abs() < 0.001);
|
||||
}
|
||||
}
|
||||
@@ -1,180 +0,0 @@
|
||||
//! Exploration controller configuration.
|
||||
|
||||
use serde::{Deserialize, Serialize};
|
||||
|
||||
use crate::query::RobotFootprint;
|
||||
|
||||
/// Exploration controller configuration
|
||||
#[derive(Clone, Debug, Serialize, Deserialize)]
|
||||
pub struct ExplorationConfig {
|
||||
/// Robot footprint for collision checking.
|
||||
#[serde(default)]
|
||||
pub footprint: RobotFootprint,
|
||||
|
||||
/// Minimum frontier size to consider (cells).
|
||||
#[serde(default = "default_min_frontier_size")]
|
||||
pub min_frontier_size: usize,
|
||||
|
||||
/// Distance threshold to consider frontier reached (meters).
|
||||
#[serde(default = "default_frontier_reached_threshold")]
|
||||
pub frontier_reached_threshold: f32,
|
||||
|
||||
/// Distance threshold for waypoint reached (meters).
|
||||
#[serde(default = "default_waypoint_threshold")]
|
||||
pub waypoint_threshold: f32,
|
||||
|
||||
/// Maximum distance to plan at once (meters).
|
||||
#[serde(default = "default_max_plan_distance")]
|
||||
pub max_plan_distance: f32,
|
||||
|
||||
/// Replan interval when path is blocked (seconds).
|
||||
#[serde(default = "default_replan_interval")]
|
||||
pub replan_interval: f32,
|
||||
|
||||
/// Maximum consecutive failures before giving up.
|
||||
#[serde(default = "default_max_consecutive_failures")]
|
||||
pub max_consecutive_failures: usize,
|
||||
|
||||
/// Clearance weight for path planning (higher = prefer more clearance).
|
||||
#[serde(default)]
|
||||
pub path_clearance_weight: f32,
|
||||
|
||||
/// Enable path smoothing.
|
||||
#[serde(default = "default_smooth_paths")]
|
||||
pub smooth_paths: bool,
|
||||
|
||||
/// Recovery turn angle (radians) when stuck.
|
||||
#[serde(default = "default_recovery_turn_angle")]
|
||||
pub recovery_turn_angle: f32,
|
||||
|
||||
/// Recovery backup distance (meters) when stuck.
|
||||
#[serde(default = "default_recovery_backup_distance")]
|
||||
pub recovery_backup_distance: f32,
|
||||
|
||||
/// Maximum linear speed during navigation (m/s).
|
||||
#[serde(default = "default_navigation_max_speed")]
|
||||
pub navigation_max_speed: f32,
|
||||
|
||||
/// Maximum linear speed during recovery (m/s).
|
||||
#[serde(default = "default_recovery_max_speed")]
|
||||
pub recovery_max_speed: f32,
|
||||
|
||||
/// Angular speed during scanning rotation (rad/s).
|
||||
#[serde(default = "default_scan_angular_speed")]
|
||||
pub scan_angular_speed: f32,
|
||||
|
||||
/// Rotation step size during scanning (radians per update).
|
||||
#[serde(default = "default_scan_rotation_step")]
|
||||
pub scan_rotation_step: f32,
|
||||
|
||||
/// Distance tolerance for matching recently failed frontiers (meters).
|
||||
#[serde(default = "default_frontier_blacklist_tolerance")]
|
||||
pub frontier_blacklist_tolerance: f32,
|
||||
|
||||
/// Maximum number of steps without progress before declaring stuck.
|
||||
#[serde(default = "default_max_stuck_steps")]
|
||||
pub max_stuck_steps: usize,
|
||||
|
||||
/// Minimum progress distance to reset stuck counter (meters).
|
||||
#[serde(default = "default_min_progress_distance")]
|
||||
pub min_progress_distance: f32,
|
||||
}
|
||||
|
||||
fn default_min_frontier_size() -> usize {
|
||||
5
|
||||
}
|
||||
|
||||
fn default_frontier_reached_threshold() -> f32 {
|
||||
0.3
|
||||
}
|
||||
|
||||
fn default_waypoint_threshold() -> f32 {
|
||||
0.1
|
||||
}
|
||||
|
||||
fn default_max_plan_distance() -> f32 {
|
||||
5.0
|
||||
}
|
||||
|
||||
fn default_replan_interval() -> f32 {
|
||||
1.0
|
||||
}
|
||||
|
||||
fn default_max_consecutive_failures() -> usize {
|
||||
5
|
||||
}
|
||||
|
||||
fn default_smooth_paths() -> bool {
|
||||
true
|
||||
}
|
||||
|
||||
fn default_recovery_turn_angle() -> f32 {
|
||||
std::f32::consts::FRAC_PI_4 // 45 degrees
|
||||
}
|
||||
|
||||
fn default_recovery_backup_distance() -> f32 {
|
||||
0.1
|
||||
}
|
||||
|
||||
fn default_navigation_max_speed() -> f32 {
|
||||
0.3 // 30 cm/s
|
||||
}
|
||||
|
||||
fn default_recovery_max_speed() -> f32 {
|
||||
0.1 // 10 cm/s
|
||||
}
|
||||
|
||||
fn default_scan_angular_speed() -> f32 {
|
||||
0.5 // rad/s
|
||||
}
|
||||
|
||||
fn default_scan_rotation_step() -> f32 {
|
||||
0.1 // ~6 degrees per step
|
||||
}
|
||||
|
||||
fn default_frontier_blacklist_tolerance() -> f32 {
|
||||
0.5 // 50 cm
|
||||
}
|
||||
|
||||
fn default_max_stuck_steps() -> usize {
|
||||
50 // ~5 seconds at 10Hz
|
||||
}
|
||||
|
||||
fn default_min_progress_distance() -> f32 {
|
||||
0.05 // 5 cm
|
||||
}
|
||||
|
||||
impl Default for ExplorationConfig {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
footprint: RobotFootprint::default(),
|
||||
min_frontier_size: default_min_frontier_size(),
|
||||
frontier_reached_threshold: default_frontier_reached_threshold(),
|
||||
waypoint_threshold: default_waypoint_threshold(),
|
||||
max_plan_distance: default_max_plan_distance(),
|
||||
replan_interval: default_replan_interval(),
|
||||
max_consecutive_failures: default_max_consecutive_failures(),
|
||||
path_clearance_weight: 0.0,
|
||||
smooth_paths: default_smooth_paths(),
|
||||
recovery_turn_angle: default_recovery_turn_angle(),
|
||||
recovery_backup_distance: default_recovery_backup_distance(),
|
||||
navigation_max_speed: default_navigation_max_speed(),
|
||||
recovery_max_speed: default_recovery_max_speed(),
|
||||
scan_angular_speed: default_scan_angular_speed(),
|
||||
scan_rotation_step: default_scan_rotation_step(),
|
||||
frontier_blacklist_tolerance: default_frontier_blacklist_tolerance(),
|
||||
max_stuck_steps: default_max_stuck_steps(),
|
||||
min_progress_distance: default_min_progress_distance(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl ExplorationConfig {
|
||||
/// Create with custom robot footprint
|
||||
pub fn with_footprint(footprint: RobotFootprint) -> Self {
|
||||
Self {
|
||||
footprint,
|
||||
..Default::default()
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,891 +0,0 @@
|
||||
//! Exploration controller implementation.
|
||||
|
||||
use crate::core::{Pose2D, WorldPoint};
|
||||
use crate::grid::GridStorage;
|
||||
use crate::pathfinding::{AStarConfig, AStarPlanner, PathFailure, PathSmoother, SmoothingConfig};
|
||||
use crate::query::{Frontier, FrontierDetector, TraversabilityChecker};
|
||||
use log::{debug, info, trace, warn};
|
||||
|
||||
use super::config::ExplorationConfig;
|
||||
use super::state::{ExplorationCommand, ExplorationEvent, ExplorationState, RecoveryAction};
|
||||
|
||||
/// Exploration controller
|
||||
///
|
||||
/// Implements a state machine for autonomous frontier-based exploration.
|
||||
pub struct ExplorationController {
|
||||
/// Current state
|
||||
state: ExplorationState,
|
||||
/// Configuration
|
||||
config: ExplorationConfig,
|
||||
/// Consecutive failure count
|
||||
consecutive_failures: usize,
|
||||
/// Total frontiers explored
|
||||
frontiers_explored: usize,
|
||||
/// Total distance traveled (approximate)
|
||||
distance_traveled: f32,
|
||||
/// Last known robot pose
|
||||
last_pose: Pose2D,
|
||||
/// Recently failed frontier centroids (to avoid re-selecting)
|
||||
failed_frontier_centroids: Vec<WorldPoint>,
|
||||
/// Steps since last significant progress during navigation
|
||||
steps_without_progress: usize,
|
||||
/// Last recorded position for stuck detection
|
||||
stuck_check_position: WorldPoint,
|
||||
/// Consecutive StartBlocked events (localization drift)
|
||||
start_blocked_count: usize,
|
||||
}
|
||||
|
||||
impl ExplorationController {
|
||||
/// Create a new exploration controller
|
||||
pub fn new(config: ExplorationConfig) -> Self {
|
||||
Self {
|
||||
state: ExplorationState::Idle,
|
||||
config,
|
||||
consecutive_failures: 0,
|
||||
frontiers_explored: 0,
|
||||
distance_traveled: 0.0,
|
||||
last_pose: Pose2D::default(),
|
||||
failed_frontier_centroids: Vec::new(),
|
||||
steps_without_progress: 0,
|
||||
stuck_check_position: WorldPoint::ZERO,
|
||||
start_blocked_count: 0,
|
||||
}
|
||||
}
|
||||
|
||||
/// Create with default configuration
|
||||
pub fn with_defaults() -> Self {
|
||||
Self::new(ExplorationConfig::default())
|
||||
}
|
||||
|
||||
/// Get current state
|
||||
pub fn state(&self) -> &ExplorationState {
|
||||
&self.state
|
||||
}
|
||||
|
||||
/// Get configuration
|
||||
pub fn config(&self) -> &ExplorationConfig {
|
||||
&self.config
|
||||
}
|
||||
|
||||
/// Get exploration progress
|
||||
pub fn progress(&self) -> ExplorationProgress {
|
||||
ExplorationProgress {
|
||||
state: self.state.name().to_string(),
|
||||
frontiers_explored: self.frontiers_explored,
|
||||
distance_traveled: self.distance_traveled,
|
||||
consecutive_failures: self.consecutive_failures,
|
||||
is_active: self.state.is_active(),
|
||||
is_complete: matches!(self.state, ExplorationState::Complete),
|
||||
}
|
||||
}
|
||||
|
||||
/// Start exploration
|
||||
pub fn start(&mut self) {
|
||||
if matches!(self.state, ExplorationState::Idle) {
|
||||
self.state = ExplorationState::SearchingFrontiers;
|
||||
self.consecutive_failures = 0;
|
||||
}
|
||||
}
|
||||
|
||||
/// Stop exploration
|
||||
pub fn stop(&mut self) {
|
||||
self.state = ExplorationState::Idle;
|
||||
}
|
||||
|
||||
/// Update controller with current pose and map
|
||||
///
|
||||
/// Returns the command to execute
|
||||
pub fn update(&mut self, pose: Pose2D, storage: &GridStorage) -> ExplorationCommand {
|
||||
// Track distance traveled
|
||||
let dist = pose.position().distance(&self.last_pose.position());
|
||||
if dist > 0.01 {
|
||||
// Only count movement > 1cm
|
||||
self.distance_traveled += dist;
|
||||
}
|
||||
self.last_pose = pose;
|
||||
|
||||
match &self.state {
|
||||
ExplorationState::Idle => ExplorationCommand::None,
|
||||
|
||||
ExplorationState::SearchingFrontiers => self.search_frontiers(pose, storage),
|
||||
|
||||
ExplorationState::Planning { target } => self.plan_path(pose, target.clone(), storage),
|
||||
|
||||
ExplorationState::Navigating {
|
||||
path,
|
||||
current_waypoint,
|
||||
target,
|
||||
} => self.navigate(
|
||||
pose,
|
||||
path.clone(),
|
||||
*current_waypoint,
|
||||
target.clone(),
|
||||
storage,
|
||||
),
|
||||
|
||||
ExplorationState::Scanning {
|
||||
position,
|
||||
rotation_remaining,
|
||||
} => self.handle_scanning(*position, *rotation_remaining, pose),
|
||||
|
||||
ExplorationState::Recovering { action, attempts } => {
|
||||
self.handle_recovery(action.clone(), *attempts)
|
||||
}
|
||||
|
||||
ExplorationState::Complete => ExplorationCommand::ExplorationComplete,
|
||||
|
||||
ExplorationState::Failed { reason } => ExplorationCommand::ExplorationFailed {
|
||||
reason: reason.clone(),
|
||||
},
|
||||
}
|
||||
}
|
||||
|
||||
/// Handle an external event.
|
||||
///
|
||||
/// Events are used to notify the controller of external conditions
|
||||
/// that should trigger state transitions.
|
||||
pub fn handle_event(&mut self, event: ExplorationEvent) {
|
||||
match event {
|
||||
ExplorationEvent::Start => self.start(),
|
||||
ExplorationEvent::Stop => self.stop(),
|
||||
ExplorationEvent::RecoveryComplete => {
|
||||
// Go back to searching after recovery
|
||||
self.state = ExplorationState::SearchingFrontiers;
|
||||
}
|
||||
ExplorationEvent::ScanComplete => {
|
||||
// Finished scanning, look for next frontier
|
||||
self.frontiers_explored += 1;
|
||||
self.consecutive_failures = 0;
|
||||
self.state = ExplorationState::SearchingFrontiers;
|
||||
}
|
||||
ExplorationEvent::ObstacleDetected | ExplorationEvent::PathBlocked => {
|
||||
self.enter_recovery();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Private state handlers
|
||||
|
||||
fn search_frontiers(&mut self, pose: Pose2D, storage: &GridStorage) -> ExplorationCommand {
|
||||
let detector = FrontierDetector::with_min_size(self.config.min_frontier_size);
|
||||
let robot_grid = storage.world_to_grid(pose.position());
|
||||
|
||||
// Get all frontiers and filter out recently failed ones
|
||||
let frontiers = detector.detect_frontiers(storage);
|
||||
let total_frontiers = frontiers.len();
|
||||
let blacklisted_count = frontiers
|
||||
.iter()
|
||||
.filter(|f| self.is_recently_failed_frontier(&f.centroid_world))
|
||||
.count();
|
||||
|
||||
let valid_frontiers: Vec<_> = frontiers
|
||||
.into_iter()
|
||||
.filter(|f| !self.is_recently_failed_frontier(&f.centroid_world))
|
||||
.collect();
|
||||
|
||||
debug!(
|
||||
"[Explore] SearchingFrontiers: {} total, {} blacklisted, {} valid",
|
||||
total_frontiers,
|
||||
blacklisted_count,
|
||||
valid_frontiers.len()
|
||||
);
|
||||
|
||||
// Find the best frontier from the valid ones
|
||||
let best = valid_frontiers.into_iter().max_by(|a, b| {
|
||||
let distance_a = robot_grid.manhattan_distance(&a.centroid_grid) as f32;
|
||||
let distance_b = robot_grid.manhattan_distance(&b.centroid_grid) as f32;
|
||||
let score_a = if distance_a > 0.0 {
|
||||
a.exploration_score() / (1.0 + distance_a * 0.1)
|
||||
} else {
|
||||
a.exploration_score() * 2.0
|
||||
};
|
||||
let score_b = if distance_b > 0.0 {
|
||||
b.exploration_score() / (1.0 + distance_b * 0.1)
|
||||
} else {
|
||||
b.exploration_score() * 2.0
|
||||
};
|
||||
score_a.total_cmp(&score_b)
|
||||
});
|
||||
|
||||
if let Some(frontier) = best {
|
||||
let distance = robot_grid.manhattan_distance(&frontier.centroid_grid);
|
||||
info!(
|
||||
"[Explore] Selected frontier: ({:.2}, {:.2}), size={}, score={:.1}, dist={}",
|
||||
frontier.centroid_world.x,
|
||||
frontier.centroid_world.y,
|
||||
frontier.size,
|
||||
frontier.exploration_score(),
|
||||
distance
|
||||
);
|
||||
// Reset stuck detection when starting to plan for a new frontier
|
||||
self.steps_without_progress = 0;
|
||||
self.stuck_check_position = pose.position();
|
||||
self.state = ExplorationState::Planning { target: frontier };
|
||||
ExplorationCommand::None
|
||||
} else {
|
||||
// Check if map has enough explored area before declaring complete
|
||||
// If we have very few floor cells, we haven't really started exploring yet
|
||||
let counts = storage.count_by_type();
|
||||
let resolution = storage.resolution();
|
||||
let explored_area_m2 = counts.floor as f32 * resolution * resolution;
|
||||
|
||||
if counts.floor < 100 {
|
||||
// Not enough explored yet - wait for more sensor data
|
||||
debug!(
|
||||
"[Explore] No valid frontiers, but only {} floor cells ({:.2}m²) - waiting",
|
||||
counts.floor, explored_area_m2
|
||||
);
|
||||
ExplorationCommand::None
|
||||
} else {
|
||||
// No frontiers and we have explored a reasonable amount - complete
|
||||
info!(
|
||||
"[Explore] COMPLETE: No frontiers remaining. Floor={} ({:.2}m²), Wall={}, Unknown={}",
|
||||
counts.floor, explored_area_m2, counts.wall, counts.unknown
|
||||
);
|
||||
self.state = ExplorationState::Complete;
|
||||
ExplorationCommand::ExplorationComplete
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Check if a frontier centroid is in the recently failed list
|
||||
fn is_recently_failed_frontier(&self, centroid: &WorldPoint) -> bool {
|
||||
self.failed_frontier_centroids
|
||||
.iter()
|
||||
.any(|failed| centroid.distance(failed) < self.config.frontier_blacklist_tolerance)
|
||||
}
|
||||
|
||||
fn plan_path(
|
||||
&mut self,
|
||||
pose: Pose2D,
|
||||
target: Frontier,
|
||||
storage: &GridStorage,
|
||||
) -> ExplorationCommand {
|
||||
let astar_config = AStarConfig {
|
||||
footprint: self.config.footprint.clone(),
|
||||
clearance_weight: self.config.path_clearance_weight,
|
||||
exploration_mode: true, // Allow Unknown cells in footprint during exploration
|
||||
..Default::default()
|
||||
};
|
||||
|
||||
let robot_pos = pose.position();
|
||||
let robot_grid = storage.world_to_grid(robot_pos);
|
||||
|
||||
// Try to find a valid observation point for this frontier
|
||||
let goal_world = match self.find_observation_point(&target, robot_pos, robot_grid, storage)
|
||||
{
|
||||
Some(goal) => goal,
|
||||
None => {
|
||||
// No valid observation point found - blacklist this frontier but don't
|
||||
// count as a "real" failure (it's a goal selection issue, not pathfinding)
|
||||
debug!(
|
||||
"[Explore] No valid observation point for frontier at ({:.2},{:.2}), skipping",
|
||||
target.centroid_world.x, target.centroid_world.y
|
||||
);
|
||||
self.failed_frontier_centroids.push(target.centroid_world);
|
||||
const MAX_FAILED_FRONTIERS: usize = 20;
|
||||
if self.failed_frontier_centroids.len() > MAX_FAILED_FRONTIERS {
|
||||
self.failed_frontier_centroids.remove(0);
|
||||
}
|
||||
self.state = ExplorationState::SearchingFrontiers;
|
||||
return ExplorationCommand::None;
|
||||
}
|
||||
};
|
||||
|
||||
trace!(
|
||||
"[Explore] Planning: robot=({:.2},{:.2}) → goal=({:.2},{:.2}), frontier=({:.2},{:.2})",
|
||||
robot_pos.x,
|
||||
robot_pos.y,
|
||||
goal_world.x,
|
||||
goal_world.y,
|
||||
target.centroid_world.x,
|
||||
target.centroid_world.y
|
||||
);
|
||||
|
||||
let planner = AStarPlanner::new(storage, astar_config.clone());
|
||||
let result = planner.find_path_world(robot_pos, goal_world);
|
||||
|
||||
if result.success {
|
||||
// Reset failure counts on successful planning
|
||||
self.consecutive_failures = 0;
|
||||
self.start_blocked_count = 0;
|
||||
|
||||
let path_length_m = result.length_meters();
|
||||
let path = if self.config.smooth_paths {
|
||||
let smoother_config = SmoothingConfig {
|
||||
footprint: self.config.footprint.clone(),
|
||||
..Default::default()
|
||||
};
|
||||
let smoother = PathSmoother::new(storage, smoother_config);
|
||||
smoother.smooth(&result.path_world)
|
||||
} else {
|
||||
result.path_world
|
||||
};
|
||||
|
||||
info!(
|
||||
"[Explore] Planning SUCCESS: path length={} waypoints, {:.2}m",
|
||||
path.len(),
|
||||
path_length_m
|
||||
);
|
||||
|
||||
self.state = ExplorationState::Navigating {
|
||||
path,
|
||||
current_waypoint: 0,
|
||||
target,
|
||||
};
|
||||
ExplorationCommand::None
|
||||
} else {
|
||||
// Planning failed - handle different failure types
|
||||
let failure_reason = result.failure_reason.clone();
|
||||
|
||||
// StartBlocked: Robot's current position is considered blocked.
|
||||
// This typically happens due to localization drift - trigger recovery
|
||||
// to move the robot to a safer position.
|
||||
let is_start_blocked = matches!(failure_reason, Some(PathFailure::StartBlocked));
|
||||
if is_start_blocked {
|
||||
self.start_blocked_count += 1;
|
||||
|
||||
// After too many consecutive StartBlocked events, the robot is likely stuck
|
||||
// due to localization drift. Check if we've explored enough to call it complete.
|
||||
const MAX_START_BLOCKED: usize = 10;
|
||||
if self.start_blocked_count >= MAX_START_BLOCKED {
|
||||
let counts = storage.count_by_type();
|
||||
let explored_area_m2 =
|
||||
counts.floor as f32 * storage.resolution() * storage.resolution();
|
||||
|
||||
if explored_area_m2 > 10.0 && self.frontiers_explored > 0 {
|
||||
info!(
|
||||
"[Explore] COMPLETE (localization drift): explored {:.2}m², {} frontiers done after {} StartBlocked events",
|
||||
explored_area_m2, self.frontiers_explored, self.start_blocked_count
|
||||
);
|
||||
self.state = ExplorationState::Complete;
|
||||
return ExplorationCommand::ExplorationComplete;
|
||||
} else {
|
||||
warn!(
|
||||
"[Explore] {} consecutive StartBlocked events, only {:.2}m² explored - entering recovery",
|
||||
self.start_blocked_count, explored_area_m2
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
debug!(
|
||||
"[Explore] StartBlocked at robot position ({}/{})",
|
||||
self.start_blocked_count, MAX_START_BLOCKED
|
||||
);
|
||||
self.enter_recovery();
|
||||
return ExplorationCommand::Stop;
|
||||
} else {
|
||||
// Reset counter on non-StartBlocked events
|
||||
self.start_blocked_count = 0;
|
||||
}
|
||||
|
||||
// GoalBlocked means our observation point selection failed (shouldn't happen after
|
||||
// find_observation_point, but handle it gracefully). Don't count as consecutive failure.
|
||||
let is_goal_blocked = matches!(failure_reason, Some(PathFailure::GoalBlocked));
|
||||
|
||||
if is_goal_blocked {
|
||||
debug!(
|
||||
"[Explore] Goal blocked at ({:.2},{:.2}), blacklisting frontier",
|
||||
goal_world.x, goal_world.y
|
||||
);
|
||||
} else {
|
||||
warn!(
|
||||
"[Explore] Planning FAILED to ({:.2},{:.2}): {:?}, failures={}/{}",
|
||||
goal_world.x,
|
||||
goal_world.y,
|
||||
failure_reason,
|
||||
self.consecutive_failures + 1,
|
||||
self.config.max_consecutive_failures
|
||||
);
|
||||
self.consecutive_failures += 1;
|
||||
}
|
||||
|
||||
self.failed_frontier_centroids.push(target.centroid_world);
|
||||
const MAX_FAILED_FRONTIERS: usize = 20;
|
||||
if self.failed_frontier_centroids.len() > MAX_FAILED_FRONTIERS {
|
||||
self.failed_frontier_centroids.remove(0);
|
||||
}
|
||||
|
||||
if self.consecutive_failures >= self.config.max_consecutive_failures {
|
||||
// Before failing, check if we've explored a significant area
|
||||
let counts = storage.count_by_type();
|
||||
let explored_area_m2 =
|
||||
counts.floor as f32 * storage.resolution() * storage.resolution();
|
||||
|
||||
if explored_area_m2 > 10.0 && self.frontiers_explored > 0 {
|
||||
info!(
|
||||
"[Explore] COMPLETE (planning failures): explored {:.2}m², {} frontiers done",
|
||||
explored_area_m2, self.frontiers_explored
|
||||
);
|
||||
self.state = ExplorationState::Complete;
|
||||
return ExplorationCommand::ExplorationComplete;
|
||||
} else if explored_area_m2 > 20.0 {
|
||||
info!(
|
||||
"[Explore] COMPLETE (large area): explored {:.2}m² with {} frontiers",
|
||||
explored_area_m2, self.frontiers_explored
|
||||
);
|
||||
self.state = ExplorationState::Complete;
|
||||
return ExplorationCommand::ExplorationComplete;
|
||||
} else {
|
||||
warn!(
|
||||
"[Explore] FAILED: {} consecutive planning failures, only {:.2}m² explored",
|
||||
self.consecutive_failures, explored_area_m2
|
||||
);
|
||||
self.state = ExplorationState::Failed {
|
||||
reason: "Too many planning failures".to_string(),
|
||||
};
|
||||
}
|
||||
} else {
|
||||
self.state = ExplorationState::SearchingFrontiers;
|
||||
}
|
||||
ExplorationCommand::None
|
||||
}
|
||||
}
|
||||
|
||||
/// Find a valid observation point for a frontier.
|
||||
///
|
||||
/// Tries multiple candidate positions near the frontier and returns the first
|
||||
/// one that is traversable (robot can fit there without hitting obstacles).
|
||||
fn find_observation_point(
|
||||
&self,
|
||||
target: &Frontier,
|
||||
robot_pos: WorldPoint,
|
||||
robot_grid: crate::core::GridCoord,
|
||||
storage: &GridStorage,
|
||||
) -> Option<WorldPoint> {
|
||||
let checker = TraversabilityChecker::new(storage, self.config.footprint.clone());
|
||||
let offset_distance = self.config.footprint.total_radius() + storage.resolution() * 2.0;
|
||||
|
||||
// Get the closest frontier cell to the robot
|
||||
let frontier_world = target
|
||||
.closest_cell_to(robot_grid)
|
||||
.map(|c| storage.grid_to_world(c.coord))
|
||||
.unwrap_or(target.centroid_world);
|
||||
|
||||
// Generate candidate observation points
|
||||
let candidates = self.generate_observation_candidates(
|
||||
frontier_world,
|
||||
robot_pos,
|
||||
offset_distance,
|
||||
storage,
|
||||
);
|
||||
|
||||
// Find the first valid candidate (traversable in exploration mode)
|
||||
for candidate in candidates {
|
||||
if checker.is_position_safe_mode(candidate, true) {
|
||||
trace!(
|
||||
"[Explore] Found valid observation point at ({:.2},{:.2})",
|
||||
candidate.x, candidate.y
|
||||
);
|
||||
return Some(candidate);
|
||||
}
|
||||
}
|
||||
|
||||
// If no candidate worked, try frontier cells directly
|
||||
// (the robot might be able to observe from multiple cells in the frontier)
|
||||
for cell in target.cells.iter().take(10) {
|
||||
let cell_world = storage.grid_to_world(cell.coord);
|
||||
let dx = robot_pos.x - cell_world.x;
|
||||
let dy = robot_pos.y - cell_world.y;
|
||||
let dist = (dx * dx + dy * dy).sqrt();
|
||||
|
||||
if dist > offset_distance {
|
||||
let ratio = offset_distance / dist;
|
||||
let candidate =
|
||||
WorldPoint::new(cell_world.x + dx * ratio, cell_world.y + dy * ratio);
|
||||
|
||||
if checker.is_position_safe_mode(candidate, true) {
|
||||
trace!(
|
||||
"[Explore] Found valid observation point from cell at ({:.2},{:.2})",
|
||||
candidate.x, candidate.y
|
||||
);
|
||||
return Some(candidate);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
None
|
||||
}
|
||||
|
||||
/// Generate candidate observation points around a frontier.
|
||||
fn generate_observation_candidates(
|
||||
&self,
|
||||
frontier_world: WorldPoint,
|
||||
robot_pos: WorldPoint,
|
||||
offset_distance: f32,
|
||||
_storage: &GridStorage,
|
||||
) -> Vec<WorldPoint> {
|
||||
let mut candidates = Vec::with_capacity(9);
|
||||
|
||||
let dx = robot_pos.x - frontier_world.x;
|
||||
let dy = robot_pos.y - frontier_world.y;
|
||||
let dist = (dx * dx + dy * dy).sqrt();
|
||||
|
||||
// Primary candidate: offset toward robot
|
||||
if dist > offset_distance {
|
||||
let ratio = offset_distance / dist;
|
||||
candidates.push(WorldPoint::new(
|
||||
frontier_world.x + dx * ratio,
|
||||
frontier_world.y + dy * ratio,
|
||||
));
|
||||
} else if dist > 0.01 {
|
||||
// Robot is close, just move slightly toward robot
|
||||
candidates.push(robot_pos);
|
||||
}
|
||||
|
||||
// Additional candidates at different angles around the frontier
|
||||
let base_angle = dy.atan2(dx);
|
||||
let angles = [
|
||||
0.0,
|
||||
std::f32::consts::FRAC_PI_4,
|
||||
-std::f32::consts::FRAC_PI_4,
|
||||
std::f32::consts::FRAC_PI_2,
|
||||
-std::f32::consts::FRAC_PI_2,
|
||||
std::f32::consts::FRAC_PI_4 * 3.0,
|
||||
-std::f32::consts::FRAC_PI_4 * 3.0,
|
||||
std::f32::consts::PI,
|
||||
];
|
||||
|
||||
for angle_offset in angles {
|
||||
let angle = base_angle + angle_offset;
|
||||
candidates.push(WorldPoint::new(
|
||||
frontier_world.x + offset_distance * angle.cos(),
|
||||
frontier_world.y + offset_distance * angle.sin(),
|
||||
));
|
||||
}
|
||||
|
||||
candidates
|
||||
}
|
||||
|
||||
fn navigate(
|
||||
&mut self,
|
||||
pose: Pose2D,
|
||||
path: Vec<WorldPoint>,
|
||||
current_waypoint: usize,
|
||||
target: Frontier,
|
||||
_storage: &GridStorage,
|
||||
) -> ExplorationCommand {
|
||||
if current_waypoint >= path.len() {
|
||||
// Reached end of path - start scanning rotation
|
||||
self.state = ExplorationState::Scanning {
|
||||
position: pose.position(),
|
||||
rotation_remaining: std::f32::consts::TAU, // Full 360° rotation
|
||||
};
|
||||
return ExplorationCommand::Stop;
|
||||
}
|
||||
|
||||
let waypoint = path[current_waypoint];
|
||||
let distance = pose.position().distance(&waypoint);
|
||||
|
||||
// Check if waypoint reached
|
||||
if distance < self.config.waypoint_threshold {
|
||||
// Move to next waypoint
|
||||
let next_waypoint = current_waypoint + 1;
|
||||
|
||||
// Check if we've reached the frontier (within threshold of centroid)
|
||||
let distance_to_target = pose.position().distance(&target.centroid_world);
|
||||
if distance_to_target < self.config.frontier_reached_threshold {
|
||||
self.state = ExplorationState::Scanning {
|
||||
position: pose.position(),
|
||||
rotation_remaining: std::f32::consts::TAU,
|
||||
};
|
||||
return ExplorationCommand::Stop;
|
||||
}
|
||||
|
||||
if next_waypoint < path.len() {
|
||||
let next_target = path[next_waypoint];
|
||||
self.state = ExplorationState::Navigating {
|
||||
path,
|
||||
current_waypoint: next_waypoint,
|
||||
target,
|
||||
};
|
||||
return ExplorationCommand::MoveTo {
|
||||
target: next_target,
|
||||
max_speed: self.config.navigation_max_speed,
|
||||
};
|
||||
} else {
|
||||
// Reached end of path - transition to Scanning
|
||||
// (we're close enough to observe the frontier)
|
||||
self.state = ExplorationState::Scanning {
|
||||
position: pose.position(),
|
||||
rotation_remaining: std::f32::consts::TAU,
|
||||
};
|
||||
return ExplorationCommand::Stop;
|
||||
}
|
||||
}
|
||||
|
||||
// Stuck detection: only check when robot should be moving forward
|
||||
// Calculate angle to waypoint
|
||||
let dx = waypoint.x - pose.x;
|
||||
let dy = waypoint.y - pose.y;
|
||||
let target_angle = dy.atan2(dx);
|
||||
let angle_error = (target_angle - pose.theta).abs();
|
||||
// Normalize to [0, PI]
|
||||
let angle_error = if angle_error > std::f32::consts::PI {
|
||||
std::f32::consts::TAU - angle_error
|
||||
} else {
|
||||
angle_error
|
||||
};
|
||||
|
||||
// Only check stuck if angle is small (robot should be moving forward)
|
||||
if angle_error < 0.5 {
|
||||
// ~30 degrees
|
||||
let progress = pose.position().distance(&self.stuck_check_position);
|
||||
if progress > self.config.min_progress_distance {
|
||||
// Made meaningful progress - reset stuck counter
|
||||
self.steps_without_progress = 0;
|
||||
self.stuck_check_position = pose.position();
|
||||
} else {
|
||||
self.steps_without_progress += 1;
|
||||
}
|
||||
|
||||
// If stuck for too long, trigger recovery
|
||||
if self.steps_without_progress > self.config.max_stuck_steps {
|
||||
warn!(
|
||||
"[Explore] Stuck detected! {} steps without progress at ({:.2},{:.2})",
|
||||
self.steps_without_progress, pose.x, pose.y
|
||||
);
|
||||
self.steps_without_progress = 0;
|
||||
// Add this frontier to failed list and try recovery
|
||||
self.failed_frontier_centroids.push(target.centroid_world);
|
||||
self.enter_recovery();
|
||||
return ExplorationCommand::Stop;
|
||||
}
|
||||
} else {
|
||||
// Robot is rotating to face waypoint - reset stuck counter
|
||||
self.steps_without_progress = 0;
|
||||
}
|
||||
|
||||
// Continue to current waypoint
|
||||
ExplorationCommand::MoveTo {
|
||||
target: waypoint,
|
||||
max_speed: self.config.navigation_max_speed,
|
||||
}
|
||||
}
|
||||
|
||||
fn handle_scanning(
|
||||
&mut self,
|
||||
position: WorldPoint,
|
||||
rotation_remaining: f32,
|
||||
pose: Pose2D,
|
||||
) -> ExplorationCommand {
|
||||
// Rotate to scan the area - each update rotates by a fixed amount
|
||||
let rotation_step = self.config.scan_rotation_step;
|
||||
let new_remaining = rotation_remaining - rotation_step;
|
||||
|
||||
if new_remaining <= 0.0 {
|
||||
// Scan complete - frontier successfully explored
|
||||
self.frontiers_explored += 1;
|
||||
self.consecutive_failures = 0;
|
||||
self.failed_frontier_centroids.clear(); // Reset on successful exploration
|
||||
info!(
|
||||
"[Explore] Scan complete at ({:.2},{:.2}), frontiers_explored={}",
|
||||
position.x, position.y, self.frontiers_explored
|
||||
);
|
||||
self.state = ExplorationState::SearchingFrontiers;
|
||||
ExplorationCommand::None
|
||||
} else {
|
||||
// Continue rotating
|
||||
self.state = ExplorationState::Scanning {
|
||||
position,
|
||||
rotation_remaining: new_remaining,
|
||||
};
|
||||
ExplorationCommand::Rotate {
|
||||
target_heading: pose.theta + rotation_step,
|
||||
max_angular_speed: self.config.scan_angular_speed,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fn handle_recovery(&mut self, action: RecoveryAction, attempts: usize) -> ExplorationCommand {
|
||||
const MAX_RECOVERY_ATTEMPTS: usize = 3;
|
||||
|
||||
debug!(
|
||||
"[Explore] Recovery: {:?}, attempt {}/{}",
|
||||
action,
|
||||
attempts + 1,
|
||||
MAX_RECOVERY_ATTEMPTS
|
||||
);
|
||||
|
||||
if attempts >= MAX_RECOVERY_ATTEMPTS {
|
||||
// Too many recovery attempts
|
||||
self.consecutive_failures += 1;
|
||||
warn!(
|
||||
"[Explore] Recovery exhausted after {} attempts, failures={}/{}",
|
||||
MAX_RECOVERY_ATTEMPTS,
|
||||
self.consecutive_failures,
|
||||
self.config.max_consecutive_failures
|
||||
);
|
||||
if self.consecutive_failures >= self.config.max_consecutive_failures {
|
||||
self.state = ExplorationState::Failed {
|
||||
reason: "Too many recovery failures".to_string(),
|
||||
};
|
||||
} else {
|
||||
self.state = ExplorationState::SearchingFrontiers;
|
||||
}
|
||||
return ExplorationCommand::None;
|
||||
}
|
||||
|
||||
match action {
|
||||
RecoveryAction::BackUp(distance) => {
|
||||
// After backing up, try turning
|
||||
self.state = ExplorationState::Recovering {
|
||||
action: RecoveryAction::TurnInPlace(self.config.recovery_turn_angle),
|
||||
attempts: attempts + 1,
|
||||
};
|
||||
ExplorationCommand::MoveTo {
|
||||
target: WorldPoint::new(
|
||||
self.last_pose.x - distance * self.last_pose.theta.cos(),
|
||||
self.last_pose.y - distance * self.last_pose.theta.sin(),
|
||||
),
|
||||
max_speed: self.config.recovery_max_speed,
|
||||
}
|
||||
}
|
||||
RecoveryAction::TurnInPlace(angle) => {
|
||||
// After turning, go back to searching
|
||||
self.state = ExplorationState::SearchingFrontiers;
|
||||
ExplorationCommand::Rotate {
|
||||
target_heading: self.last_pose.theta + angle,
|
||||
max_angular_speed: self.config.scan_angular_speed,
|
||||
}
|
||||
}
|
||||
RecoveryAction::Wait => {
|
||||
// Just wait, then try again
|
||||
self.state = ExplorationState::SearchingFrontiers;
|
||||
ExplorationCommand::None
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fn enter_recovery(&mut self) {
|
||||
self.state = ExplorationState::Recovering {
|
||||
action: RecoveryAction::BackUp(self.config.recovery_backup_distance),
|
||||
attempts: 0,
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
/// Exploration progress tracking
|
||||
#[derive(Clone, Debug, Default)]
|
||||
pub struct ExplorationProgress {
|
||||
/// Current state name
|
||||
pub state: String,
|
||||
/// Number of frontiers explored
|
||||
pub frontiers_explored: usize,
|
||||
/// Total distance traveled (meters)
|
||||
pub distance_traveled: f32,
|
||||
/// Current consecutive failure count
|
||||
pub consecutive_failures: usize,
|
||||
/// Is exploration active?
|
||||
pub is_active: bool,
|
||||
/// Is exploration complete?
|
||||
pub is_complete: bool,
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use crate::core::{CellType, GridCoord};
|
||||
|
||||
fn create_test_storage() -> GridStorage {
|
||||
GridStorage::centered(50, 50, 0.1) // 5m x 5m at 10cm resolution
|
||||
}
|
||||
|
||||
fn fill_floor_with_frontiers(storage: &mut GridStorage) {
|
||||
// Create a floor area with unknown edges (frontiers)
|
||||
for x in 10..40 {
|
||||
for y in 10..40 {
|
||||
storage.set_type(GridCoord::new(x, y), CellType::Floor);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_controller_creation() {
|
||||
let controller = ExplorationController::with_defaults();
|
||||
assert!(matches!(controller.state(), ExplorationState::Idle));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_start_exploration() {
|
||||
let mut controller = ExplorationController::with_defaults();
|
||||
controller.start();
|
||||
assert!(matches!(
|
||||
controller.state(),
|
||||
ExplorationState::SearchingFrontiers
|
||||
));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_stop_exploration() {
|
||||
let mut controller = ExplorationController::with_defaults();
|
||||
controller.start();
|
||||
controller.stop();
|
||||
assert!(matches!(controller.state(), ExplorationState::Idle));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_exploration_finds_frontiers() {
|
||||
let mut storage = create_test_storage();
|
||||
fill_floor_with_frontiers(&mut storage);
|
||||
|
||||
let config = ExplorationConfig {
|
||||
footprint: crate::query::RobotFootprint::new(0.05, 0.01),
|
||||
..Default::default()
|
||||
};
|
||||
let mut controller = ExplorationController::new(config);
|
||||
|
||||
controller.start();
|
||||
|
||||
// Update with a pose inside the floor area
|
||||
let pose = Pose2D::new(2.0, 2.0, 0.0);
|
||||
let _cmd = controller.update(pose, &storage);
|
||||
|
||||
// Should be planning or navigating after finding frontier
|
||||
assert!(
|
||||
matches!(controller.state(), ExplorationState::Planning { .. })
|
||||
|| matches!(controller.state(), ExplorationState::Navigating { .. })
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_exploration_completes_when_no_frontiers() {
|
||||
let mut storage = create_test_storage();
|
||||
// Larger fully explored area (no frontiers - all cells are known)
|
||||
// Need >100 floor cells for exploration to complete
|
||||
let center = storage.world_to_grid(WorldPoint::ZERO);
|
||||
for dx in -6..=6 {
|
||||
for dy in -6..=6 {
|
||||
let coord = center + GridCoord::new(dx, dy);
|
||||
storage.set_type(coord, CellType::Floor);
|
||||
}
|
||||
}
|
||||
|
||||
let config = ExplorationConfig {
|
||||
footprint: crate::query::RobotFootprint::new(0.05, 0.01),
|
||||
// Very large min size means no frontiers found (floor area is ~13x13 = 169 cells,
|
||||
// frontier edges would be ~48 cells, so 100 ensures none qualify)
|
||||
min_frontier_size: 100,
|
||||
..Default::default()
|
||||
};
|
||||
let mut controller = ExplorationController::new(config);
|
||||
|
||||
controller.start();
|
||||
let pose = Pose2D::new(0.0, 0.0, 0.0);
|
||||
let cmd = controller.update(pose, &storage);
|
||||
|
||||
assert!(matches!(controller.state(), ExplorationState::Complete));
|
||||
assert!(matches!(cmd, ExplorationCommand::ExplorationComplete));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_exploration_progress() {
|
||||
let controller = ExplorationController::with_defaults();
|
||||
let progress = controller.progress();
|
||||
|
||||
assert_eq!(progress.state, "Idle");
|
||||
assert_eq!(progress.frontiers_explored, 0);
|
||||
assert!(!progress.is_active);
|
||||
assert!(!progress.is_complete);
|
||||
}
|
||||
}
|
||||
@@ -1,52 +0,0 @@
|
||||
//! Autonomous exploration controller.
|
||||
//!
|
||||
//! This module provides a state machine for frontier-based exploration:
|
||||
//!
|
||||
//! - **Frontier detection**: Find unexplored boundaries
|
||||
//! - **Path planning**: Navigate to frontiers
|
||||
//! - **Recovery**: Handle obstacles and stuck conditions
|
||||
//!
|
||||
//! ## Usage
|
||||
//!
|
||||
//! ```rust,ignore
|
||||
//! use vastu_map::explore::{ExplorationController, ExplorationConfig};
|
||||
//!
|
||||
//! let config = ExplorationConfig::default();
|
||||
//! let mut controller = ExplorationController::new(config);
|
||||
//!
|
||||
//! controller.start();
|
||||
//!
|
||||
//! loop {
|
||||
//! let cmd = controller.update(robot_pose, map.storage());
|
||||
//! match cmd {
|
||||
//! ExplorationCommand::MoveTo { target, max_speed } => {
|
||||
//! // Send motion command to robot
|
||||
//! }
|
||||
//! ExplorationCommand::ExplorationComplete => {
|
||||
//! break;
|
||||
//! }
|
||||
//! _ => {}
|
||||
//! }
|
||||
//! }
|
||||
//! ```
|
||||
//!
|
||||
//! ## State Machine
|
||||
//!
|
||||
//! The controller cycles through these states:
|
||||
//!
|
||||
//! 1. **Idle** - Waiting to start
|
||||
//! 2. **SearchingFrontiers** - Looking for unexplored areas
|
||||
//! 3. **Planning** - Computing path to selected frontier
|
||||
//! 4. **Navigating** - Following path to frontier
|
||||
//! 5. **Scanning** - At frontier, allowing sensors to update map
|
||||
//! 6. **Recovering** - Handling obstacles or stuck conditions
|
||||
//! 7. **Complete** - All frontiers explored
|
||||
//! 8. **Failed** - Too many errors
|
||||
|
||||
pub mod config;
|
||||
mod machine;
|
||||
pub mod state;
|
||||
|
||||
pub use config::ExplorationConfig;
|
||||
pub use machine::{ExplorationController, ExplorationProgress};
|
||||
pub use state::{ExplorationCommand, ExplorationEvent, ExplorationState, RecoveryAction};
|
||||
@@ -1,165 +0,0 @@
|
||||
//! Exploration state machine states.
|
||||
|
||||
use crate::core::WorldPoint;
|
||||
use crate::query::Frontier;
|
||||
|
||||
// Note: Frontier and WorldPoint are still used by ExplorationState and ExplorationCommand
|
||||
|
||||
/// Exploration state
|
||||
#[derive(Clone, Debug)]
|
||||
pub enum ExplorationState {
|
||||
/// Idle - waiting to start exploration
|
||||
Idle,
|
||||
|
||||
/// Searching for frontiers
|
||||
SearchingFrontiers,
|
||||
|
||||
/// Planning path to selected frontier
|
||||
Planning {
|
||||
/// Target frontier
|
||||
target: Frontier,
|
||||
},
|
||||
|
||||
/// Navigating to frontier
|
||||
Navigating {
|
||||
/// Current path as world points
|
||||
path: Vec<WorldPoint>,
|
||||
/// Current waypoint index
|
||||
current_waypoint: usize,
|
||||
/// Target frontier
|
||||
target: Frontier,
|
||||
},
|
||||
|
||||
/// Reached frontier, scanning area by rotating
|
||||
Scanning {
|
||||
/// Position where we're scanning
|
||||
position: WorldPoint,
|
||||
/// Remaining rotation in radians (full rotation = 2π)
|
||||
rotation_remaining: f32,
|
||||
},
|
||||
|
||||
/// Recovering from obstacle or stuck condition
|
||||
Recovering {
|
||||
/// Recovery action being performed
|
||||
action: RecoveryAction,
|
||||
/// Number of recovery attempts
|
||||
attempts: usize,
|
||||
},
|
||||
|
||||
/// Exploration complete (no more frontiers)
|
||||
Complete,
|
||||
|
||||
/// Failed (too many errors)
|
||||
Failed {
|
||||
/// Reason for failure
|
||||
reason: String,
|
||||
},
|
||||
}
|
||||
|
||||
impl ExplorationState {
|
||||
/// Is this a terminal state?
|
||||
pub fn is_terminal(&self) -> bool {
|
||||
matches!(
|
||||
self,
|
||||
ExplorationState::Complete | ExplorationState::Failed { .. }
|
||||
)
|
||||
}
|
||||
|
||||
/// Is exploration active?
|
||||
pub fn is_active(&self) -> bool {
|
||||
!matches!(
|
||||
self,
|
||||
ExplorationState::Idle | ExplorationState::Complete | ExplorationState::Failed { .. }
|
||||
)
|
||||
}
|
||||
|
||||
/// State name for logging
|
||||
pub fn name(&self) -> &'static str {
|
||||
match self {
|
||||
ExplorationState::Idle => "Idle",
|
||||
ExplorationState::SearchingFrontiers => "SearchingFrontiers",
|
||||
ExplorationState::Planning { .. } => "Planning",
|
||||
ExplorationState::Navigating { .. } => "Navigating",
|
||||
ExplorationState::Scanning { .. } => "Scanning",
|
||||
ExplorationState::Recovering { .. } => "Recovering",
|
||||
ExplorationState::Complete => "Complete",
|
||||
ExplorationState::Failed { .. } => "Failed",
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Recovery action types
|
||||
#[derive(Clone, Debug)]
|
||||
pub enum RecoveryAction {
|
||||
/// Turn in place by specified angle (radians)
|
||||
TurnInPlace(f32),
|
||||
/// Back up by specified distance (meters)
|
||||
BackUp(f32),
|
||||
/// Wait for obstacle to clear
|
||||
Wait,
|
||||
}
|
||||
|
||||
/// Command output from exploration controller
|
||||
#[derive(Clone, Debug)]
|
||||
pub enum ExplorationCommand {
|
||||
/// No action needed
|
||||
None,
|
||||
|
||||
/// Move to target position
|
||||
MoveTo {
|
||||
/// Target position in world coordinates
|
||||
target: WorldPoint,
|
||||
/// Maximum speed (m/s)
|
||||
max_speed: f32,
|
||||
},
|
||||
|
||||
/// Rotate in place
|
||||
Rotate {
|
||||
/// Target heading (radians)
|
||||
target_heading: f32,
|
||||
/// Maximum angular speed (rad/s)
|
||||
max_angular_speed: f32,
|
||||
},
|
||||
|
||||
/// Stop immediately
|
||||
Stop,
|
||||
|
||||
/// Exploration is complete
|
||||
ExplorationComplete,
|
||||
|
||||
/// Exploration failed
|
||||
ExplorationFailed {
|
||||
/// Reason for failure
|
||||
reason: String,
|
||||
},
|
||||
}
|
||||
|
||||
/// Event that can trigger state transitions.
|
||||
///
|
||||
/// These events are used to communicate external conditions to the exploration
|
||||
/// controller, allowing it to respond appropriately (e.g., entering recovery
|
||||
/// mode when an obstacle is detected).
|
||||
#[derive(Clone, Debug)]
|
||||
pub enum ExplorationEvent {
|
||||
/// Start exploration.
|
||||
Start,
|
||||
|
||||
/// Stop exploration and return to Idle state.
|
||||
Stop,
|
||||
|
||||
/// Obstacle detected by sensors (cliff, bumper, etc.).
|
||||
/// Triggers recovery behavior.
|
||||
ObstacleDetected,
|
||||
|
||||
/// Path is blocked and cannot continue.
|
||||
/// Triggers recovery behavior.
|
||||
PathBlocked,
|
||||
|
||||
/// Recovery maneuver completed successfully.
|
||||
/// Returns to frontier searching.
|
||||
RecoveryComplete,
|
||||
|
||||
/// Scan rotation completed at frontier.
|
||||
/// Increments frontiers explored and searches for next.
|
||||
ScanComplete,
|
||||
}
|
||||
@@ -1,186 +0,0 @@
|
||||
//! Exploration error types.
|
||||
//!
|
||||
//! Provides structured error types for exploration failures,
|
||||
//! replacing String-based error messages.
|
||||
|
||||
use std::fmt;
|
||||
|
||||
use crate::core::WorldPoint;
|
||||
|
||||
/// Errors that can occur during autonomous exploration.
|
||||
#[derive(Debug, Clone)]
|
||||
pub enum ExplorationError {
|
||||
/// Robot connection was lost during exploration.
|
||||
ConnectionLost,
|
||||
|
||||
/// Battery level dropped below minimum threshold.
|
||||
LowBattery {
|
||||
/// Current battery percentage.
|
||||
percent: u8,
|
||||
/// Configured minimum threshold.
|
||||
threshold: u8,
|
||||
},
|
||||
|
||||
/// Exploration exceeded the configured time limit.
|
||||
TimeoutExceeded {
|
||||
/// Elapsed time in seconds.
|
||||
elapsed_secs: f32,
|
||||
/// Configured limit in seconds.
|
||||
limit_secs: f32,
|
||||
},
|
||||
|
||||
/// Too many consecutive planning/navigation failures.
|
||||
TooManyFailures {
|
||||
/// Number of consecutive failures.
|
||||
count: usize,
|
||||
/// Configured maximum.
|
||||
max: usize,
|
||||
},
|
||||
|
||||
/// Robot appears to be stuck and recovery failed.
|
||||
StuckRecoveryFailed {
|
||||
/// Position where robot got stuck.
|
||||
position: WorldPoint,
|
||||
/// Number of recovery attempts made.
|
||||
attempts: usize,
|
||||
},
|
||||
|
||||
/// Localization drift detected (robot position appears blocked).
|
||||
LocalizationDrift {
|
||||
/// Number of consecutive "start blocked" events.
|
||||
consecutive_events: usize,
|
||||
},
|
||||
|
||||
/// No valid frontiers found with insufficient exploration.
|
||||
InsufficientExploration {
|
||||
/// Explored floor area in square meters.
|
||||
explored_area_m2: f32,
|
||||
/// Number of frontiers successfully explored.
|
||||
frontiers_explored: usize,
|
||||
},
|
||||
|
||||
/// Custom error with message.
|
||||
Other(String),
|
||||
}
|
||||
|
||||
impl ExplorationError {
|
||||
/// Create an error from a string message (for backwards compatibility).
|
||||
pub fn from_message(msg: impl Into<String>) -> Self {
|
||||
Self::Other(msg.into())
|
||||
}
|
||||
|
||||
/// Get a short error code for logging/metrics.
|
||||
pub fn code(&self) -> &'static str {
|
||||
match self {
|
||||
Self::ConnectionLost => "CONNECTION_LOST",
|
||||
Self::LowBattery { .. } => "LOW_BATTERY",
|
||||
Self::TimeoutExceeded { .. } => "TIMEOUT",
|
||||
Self::TooManyFailures { .. } => "TOO_MANY_FAILURES",
|
||||
Self::StuckRecoveryFailed { .. } => "STUCK",
|
||||
Self::LocalizationDrift { .. } => "LOCALIZATION_DRIFT",
|
||||
Self::InsufficientExploration { .. } => "INSUFFICIENT_EXPLORATION",
|
||||
Self::Other(_) => "OTHER",
|
||||
}
|
||||
}
|
||||
|
||||
/// Check if this error is recoverable (exploration could potentially continue).
|
||||
pub fn is_recoverable(&self) -> bool {
|
||||
matches!(
|
||||
self,
|
||||
Self::StuckRecoveryFailed { .. } | Self::LocalizationDrift { .. }
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
impl fmt::Display for ExplorationError {
|
||||
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
|
||||
match self {
|
||||
Self::ConnectionLost => write!(f, "Robot connection lost"),
|
||||
Self::LowBattery { percent, threshold } => {
|
||||
write!(f, "Battery low: {}% (minimum: {}%)", percent, threshold)
|
||||
}
|
||||
Self::TimeoutExceeded {
|
||||
elapsed_secs,
|
||||
limit_secs,
|
||||
} => {
|
||||
write!(
|
||||
f,
|
||||
"Time limit exceeded: {:.1}s elapsed (limit: {:.1}s)",
|
||||
elapsed_secs, limit_secs
|
||||
)
|
||||
}
|
||||
Self::TooManyFailures { count, max } => {
|
||||
write!(f, "Too many consecutive failures: {} (max: {})", count, max)
|
||||
}
|
||||
Self::StuckRecoveryFailed { position, attempts } => {
|
||||
write!(
|
||||
f,
|
||||
"Stuck at ({:.2}, {:.2}) after {} recovery attempts",
|
||||
position.x, position.y, attempts
|
||||
)
|
||||
}
|
||||
Self::LocalizationDrift { consecutive_events } => {
|
||||
write!(
|
||||
f,
|
||||
"Localization drift detected ({} consecutive blocked events)",
|
||||
consecutive_events
|
||||
)
|
||||
}
|
||||
Self::InsufficientExploration {
|
||||
explored_area_m2,
|
||||
frontiers_explored,
|
||||
} => {
|
||||
write!(
|
||||
f,
|
||||
"Insufficient exploration: {:.2}m² explored, {} frontiers done",
|
||||
explored_area_m2, frontiers_explored
|
||||
)
|
||||
}
|
||||
Self::Other(msg) => write!(f, "{}", msg),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl std::error::Error for ExplorationError {}
|
||||
|
||||
/// Convert from String for backwards compatibility.
|
||||
impl From<String> for ExplorationError {
|
||||
fn from(s: String) -> Self {
|
||||
Self::Other(s)
|
||||
}
|
||||
}
|
||||
|
||||
impl From<&str> for ExplorationError {
|
||||
fn from(s: &str) -> Self {
|
||||
Self::Other(s.to_string())
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn test_error_display() {
|
||||
let err = ExplorationError::LowBattery {
|
||||
percent: 15,
|
||||
threshold: 20,
|
||||
};
|
||||
assert_eq!(err.to_string(), "Battery low: 15% (minimum: 20%)");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_error_code() {
|
||||
assert_eq!(ExplorationError::ConnectionLost.code(), "CONNECTION_LOST");
|
||||
assert_eq!(
|
||||
ExplorationError::TooManyFailures { count: 5, max: 5 }.code(),
|
||||
"TOO_MANY_FAILURES"
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_from_string() {
|
||||
let err: ExplorationError = "Custom error".into();
|
||||
assert!(matches!(err, ExplorationError::Other(_)));
|
||||
}
|
||||
}
|
||||
@@ -1,450 +0,0 @@
|
||||
//! VastuExplorer - High-level autonomous exploration.
|
||||
|
||||
use std::time::Instant;
|
||||
|
||||
use super::controller::{
|
||||
ExplorationCommand, ExplorationController, ExplorationEvent, ExplorationProgress,
|
||||
};
|
||||
use super::error::ExplorationError;
|
||||
use super::velocity::{
|
||||
VelocityConfig, compute_angular_velocity_to_heading, compute_velocity_to_target,
|
||||
};
|
||||
use crate::OccupancyGridMap;
|
||||
use crate::core::Pose2D;
|
||||
use crate::slam::CorrelativeMatcher;
|
||||
use crate::slam::loop_closure::{LoopClosureDetector, PoseGraph};
|
||||
|
||||
use super::config::ExplorerConfig;
|
||||
use super::motion_filter::MotionFilter;
|
||||
use super::source::SensorSource;
|
||||
|
||||
/// Result of a completed exploration.
|
||||
pub enum ExploreResult {
|
||||
/// Exploration completed successfully - all frontiers explored.
|
||||
Complete(OccupancyGridMap),
|
||||
/// Exploration failed or was interrupted.
|
||||
Failed {
|
||||
/// Partial map collected before failure.
|
||||
map: OccupancyGridMap,
|
||||
/// Error that caused the failure.
|
||||
error: ExplorationError,
|
||||
},
|
||||
}
|
||||
|
||||
impl ExploreResult {
|
||||
/// Check if exploration completed successfully.
|
||||
pub fn is_complete(&self) -> bool {
|
||||
matches!(self, Self::Complete(_))
|
||||
}
|
||||
|
||||
/// Get the map regardless of success/failure.
|
||||
pub fn into_map(self) -> OccupancyGridMap {
|
||||
match self {
|
||||
Self::Complete(map) => map,
|
||||
Self::Failed { map, .. } => map,
|
||||
}
|
||||
}
|
||||
|
||||
/// Get the error if exploration failed.
|
||||
pub fn error(&self) -> Option<&ExplorationError> {
|
||||
match self {
|
||||
Self::Complete(_) => None,
|
||||
Self::Failed { error, .. } => Some(error),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Status returned from a single exploration step.
|
||||
#[derive(Clone, Debug)]
|
||||
pub enum ExploreStatus {
|
||||
/// Still exploring.
|
||||
InProgress {
|
||||
/// Current exploration progress.
|
||||
progress: ExplorationProgress,
|
||||
},
|
||||
/// Exploration complete.
|
||||
Complete,
|
||||
/// Exploration failed.
|
||||
Failed {
|
||||
/// Error that caused the failure.
|
||||
error: ExplorationError,
|
||||
},
|
||||
}
|
||||
|
||||
/// High-level autonomous exploration controller.
|
||||
///
|
||||
/// Combines all components needed for autonomous map exploration:
|
||||
/// - Occupancy grid mapping
|
||||
/// - Scan-to-map matching for drift correction
|
||||
/// - Loop closure detection
|
||||
/// - Pose graph optimization
|
||||
/// - Frontier-based exploration
|
||||
///
|
||||
/// # Example
|
||||
///
|
||||
/// ```ignore
|
||||
/// let mut explorer = VastuExplorer::new(ExplorerConfig::default());
|
||||
/// let mut robot = MyRobotSource::new();
|
||||
///
|
||||
/// // Run exploration to completion
|
||||
/// match explorer.explore(&mut robot) {
|
||||
/// ExploreResult::Complete(map) => { /* save map */ }
|
||||
/// ExploreResult::Failed { map, reason } => { /* handle error */ }
|
||||
/// }
|
||||
/// ```
|
||||
pub struct VastuExplorer {
|
||||
/// Configuration.
|
||||
config: ExplorerConfig,
|
||||
/// Occupancy grid map.
|
||||
map: OccupancyGridMap,
|
||||
/// Exploration controller.
|
||||
controller: ExplorationController,
|
||||
/// Scan-to-map matcher.
|
||||
matcher: CorrelativeMatcher,
|
||||
/// Loop closure detector.
|
||||
loop_detector: LoopClosureDetector,
|
||||
/// Pose graph for optimization.
|
||||
pose_graph: PoseGraph,
|
||||
/// Motion filter for gating scan processing.
|
||||
motion_filter: MotionFilter,
|
||||
/// Velocity computation configuration.
|
||||
velocity_config: VelocityConfig,
|
||||
/// Last corrected pose.
|
||||
last_pose: Pose2D,
|
||||
/// Last odometry pose (for delta calculation).
|
||||
last_odom_pose: Option<Pose2D>,
|
||||
/// Start time.
|
||||
start_time: Option<Instant>,
|
||||
/// Whether exploration has started.
|
||||
started: bool,
|
||||
}
|
||||
|
||||
impl VastuExplorer {
|
||||
/// Create a new explorer with the given configuration.
|
||||
pub fn new(config: ExplorerConfig) -> Self {
|
||||
let map_config = config.to_map_config();
|
||||
let map = OccupancyGridMap::new(map_config);
|
||||
|
||||
let controller = ExplorationController::new(config.exploration.clone());
|
||||
let matcher = CorrelativeMatcher::new(config.matching.clone());
|
||||
let loop_detector = LoopClosureDetector::new(config.loop_closure.clone());
|
||||
let pose_graph = PoseGraph::new(config.pose_graph.clone());
|
||||
let motion_filter = MotionFilter::new(config.motion_filter.clone());
|
||||
let velocity_config = VelocityConfig::default();
|
||||
|
||||
Self {
|
||||
config,
|
||||
map,
|
||||
controller,
|
||||
matcher,
|
||||
loop_detector,
|
||||
pose_graph,
|
||||
motion_filter,
|
||||
velocity_config,
|
||||
last_pose: Pose2D::default(),
|
||||
last_odom_pose: None,
|
||||
start_time: None,
|
||||
started: false,
|
||||
}
|
||||
}
|
||||
|
||||
/// Create with default configuration.
|
||||
pub fn with_defaults() -> Self {
|
||||
Self::new(ExplorerConfig::default())
|
||||
}
|
||||
|
||||
/// Get the current map.
|
||||
pub fn map(&self) -> &OccupancyGridMap {
|
||||
&self.map
|
||||
}
|
||||
|
||||
/// Get exploration progress.
|
||||
pub fn progress(&self) -> ExplorationProgress {
|
||||
let mut progress = self.controller.progress();
|
||||
let map_stats = self.map.coverage_stats();
|
||||
progress.frontiers_explored = map_stats.frontier_count;
|
||||
progress
|
||||
}
|
||||
|
||||
/// Get the current corrected pose.
|
||||
pub fn current_pose(&self) -> Pose2D {
|
||||
self.last_pose
|
||||
}
|
||||
|
||||
/// Get elapsed time since exploration started.
|
||||
pub fn elapsed_secs(&self) -> f32 {
|
||||
self.start_time
|
||||
.map(|t| t.elapsed().as_secs_f32())
|
||||
.unwrap_or(0.0)
|
||||
}
|
||||
|
||||
/// Run exploration to completion (blocking).
|
||||
///
|
||||
/// This method loops until exploration is complete or fails.
|
||||
/// It handles all sensor integration, SLAM, and navigation internally.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `source` - Sensor source providing lidar, odometry, and accepting commands
|
||||
///
|
||||
/// # Returns
|
||||
/// `ExploreResult::Complete` with the finished map, or
|
||||
/// `ExploreResult::Failed` with a partial map and error.
|
||||
pub fn explore<S: SensorSource>(mut self, source: &mut S) -> ExploreResult {
|
||||
// Start exploration
|
||||
self.controller.start();
|
||||
self.start_time = Some(Instant::now());
|
||||
self.started = true;
|
||||
|
||||
// Control loop
|
||||
let step_duration = std::time::Duration::from_secs_f32(1.0 / self.config.update_rate_hz);
|
||||
|
||||
loop {
|
||||
let step_start = Instant::now();
|
||||
|
||||
// Check connection
|
||||
if !source.is_connected() {
|
||||
source.stop();
|
||||
return ExploreResult::Failed {
|
||||
map: self.map,
|
||||
error: ExplorationError::ConnectionLost,
|
||||
};
|
||||
}
|
||||
|
||||
// Check battery
|
||||
if let Some(battery) = source.battery_percent()
|
||||
&& battery < self.config.min_battery_percent
|
||||
{
|
||||
source.stop();
|
||||
return ExploreResult::Failed {
|
||||
map: self.map,
|
||||
error: ExplorationError::LowBattery {
|
||||
percent: battery,
|
||||
threshold: self.config.min_battery_percent,
|
||||
},
|
||||
};
|
||||
}
|
||||
|
||||
// Check time limit
|
||||
let elapsed = self.elapsed_secs();
|
||||
if self.config.max_time_secs > 0.0 && elapsed > self.config.max_time_secs {
|
||||
source.stop();
|
||||
return ExploreResult::Failed {
|
||||
map: self.map,
|
||||
error: ExplorationError::TimeoutExceeded {
|
||||
elapsed_secs: elapsed,
|
||||
limit_secs: self.config.max_time_secs,
|
||||
},
|
||||
};
|
||||
}
|
||||
|
||||
// Run one exploration step
|
||||
match self.step(source) {
|
||||
ExploreStatus::InProgress { .. } => {
|
||||
// Continue
|
||||
}
|
||||
ExploreStatus::Complete => {
|
||||
source.stop();
|
||||
return ExploreResult::Complete(self.map);
|
||||
}
|
||||
ExploreStatus::Failed { error } => {
|
||||
source.stop();
|
||||
return ExploreResult::Failed {
|
||||
map: self.map,
|
||||
error,
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
// Rate limiting
|
||||
let elapsed = step_start.elapsed();
|
||||
if elapsed < step_duration {
|
||||
std::thread::sleep(step_duration - elapsed);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Run a single exploration step (non-blocking).
|
||||
///
|
||||
/// Use this for custom control loops or when you need to
|
||||
/// interleave exploration with other tasks.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `source` - Sensor source
|
||||
///
|
||||
/// # Returns
|
||||
/// Current exploration status.
|
||||
pub fn step<S: SensorSource>(&mut self, source: &mut S) -> ExploreStatus {
|
||||
// Initialize if needed
|
||||
if !self.started {
|
||||
self.controller.start();
|
||||
self.start_time = Some(Instant::now());
|
||||
self.started = true;
|
||||
}
|
||||
|
||||
// Poll sensors
|
||||
source.poll();
|
||||
|
||||
// Get current odometry pose
|
||||
let odom_pose = source.get_pose();
|
||||
|
||||
// Calculate odometry delta
|
||||
let odom_delta = self.last_odom_pose.map(|last| {
|
||||
Pose2D::new(
|
||||
odom_pose.x - last.x,
|
||||
odom_pose.y - last.y,
|
||||
odom_pose.theta - last.theta,
|
||||
)
|
||||
});
|
||||
self.last_odom_pose = Some(odom_pose);
|
||||
|
||||
// Process lidar scan if available and motion filter allows
|
||||
if let Some(scan) = source.get_lidar_scan() {
|
||||
// Check motion filter - only process if sufficient motion occurred
|
||||
if self.motion_filter.should_process(odom_pose) {
|
||||
// Scan matching
|
||||
let match_result = self
|
||||
.matcher
|
||||
.match_scan(&scan, odom_pose, self.map.storage());
|
||||
|
||||
// Use matched pose if good, else use odometry
|
||||
let corrected_pose = if match_result.converged {
|
||||
match_result.pose
|
||||
} else {
|
||||
odom_pose
|
||||
};
|
||||
|
||||
// Integrate scan into map
|
||||
self.map.observe_lidar(&scan, corrected_pose);
|
||||
|
||||
// Update pose graph
|
||||
self.pose_graph.add_pose(corrected_pose, odom_delta);
|
||||
|
||||
// Check for loop closure
|
||||
if let Some(closure) = self.loop_detector.add_scan(&scan, corrected_pose) {
|
||||
self.pose_graph.add_loop_closure(closure);
|
||||
// Optimize when loop detected
|
||||
self.pose_graph.optimize();
|
||||
}
|
||||
|
||||
self.last_pose = corrected_pose;
|
||||
} else if let Some(delta) = odom_delta {
|
||||
// Scan filtered out - update pose from odometry delta only
|
||||
self.last_pose = Pose2D::new(
|
||||
self.last_pose.x + delta.x,
|
||||
self.last_pose.y + delta.y,
|
||||
self.last_pose.theta + delta.theta,
|
||||
);
|
||||
}
|
||||
} else if let Some(delta) = odom_delta {
|
||||
// No scan available - update pose from odometry delta
|
||||
// This prevents the controller from using stale positions
|
||||
self.last_pose = Pose2D::new(
|
||||
self.last_pose.x + delta.x,
|
||||
self.last_pose.y + delta.y,
|
||||
self.last_pose.theta + delta.theta,
|
||||
);
|
||||
}
|
||||
|
||||
// Handle cliff/bumper events
|
||||
if source.is_cliff_detected() {
|
||||
self.controller
|
||||
.handle_event(ExplorationEvent::ObstacleDetected);
|
||||
}
|
||||
if source.is_bumper_triggered() {
|
||||
self.controller
|
||||
.handle_event(ExplorationEvent::ObstacleDetected);
|
||||
}
|
||||
|
||||
// Update exploration controller
|
||||
let command = self.controller.update(self.last_pose, self.map.storage());
|
||||
|
||||
// Execute command
|
||||
match command {
|
||||
ExplorationCommand::MoveTo { target, max_speed } => {
|
||||
let (linear, angular) = compute_velocity_to_target(
|
||||
self.last_pose,
|
||||
target,
|
||||
max_speed,
|
||||
&self.velocity_config,
|
||||
);
|
||||
source.send_velocity(linear, angular);
|
||||
}
|
||||
ExplorationCommand::Rotate {
|
||||
target_heading,
|
||||
max_angular_speed,
|
||||
} => {
|
||||
let angular = compute_angular_velocity_to_heading(
|
||||
self.last_pose.theta,
|
||||
target_heading,
|
||||
max_angular_speed,
|
||||
&self.velocity_config,
|
||||
);
|
||||
source.send_velocity(0.0, angular);
|
||||
}
|
||||
ExplorationCommand::Stop => {
|
||||
source.stop();
|
||||
}
|
||||
ExplorationCommand::ExplorationComplete => {
|
||||
source.stop();
|
||||
return ExploreStatus::Complete;
|
||||
}
|
||||
ExplorationCommand::ExplorationFailed { reason } => {
|
||||
source.stop();
|
||||
return ExploreStatus::Failed {
|
||||
error: ExplorationError::from_message(reason),
|
||||
};
|
||||
}
|
||||
ExplorationCommand::None => {
|
||||
// Continue with current velocity or stop
|
||||
}
|
||||
}
|
||||
|
||||
ExploreStatus::InProgress {
|
||||
progress: self.controller.progress(),
|
||||
}
|
||||
}
|
||||
|
||||
/// Reset explorer for a new exploration run.
|
||||
pub fn reset(&mut self) {
|
||||
let map_config = self.config.to_map_config();
|
||||
self.map = OccupancyGridMap::new(map_config);
|
||||
self.controller = ExplorationController::new(self.config.exploration.clone());
|
||||
self.loop_detector.reset();
|
||||
self.pose_graph.reset();
|
||||
self.motion_filter.reset();
|
||||
self.last_pose = Pose2D::default();
|
||||
self.last_odom_pose = None;
|
||||
self.start_time = None;
|
||||
self.started = false;
|
||||
}
|
||||
|
||||
/// Get motion filter statistics.
|
||||
///
|
||||
/// Returns (passed_count, filtered_count, efficiency).
|
||||
pub fn motion_filter_stats(&self) -> (u64, u64, f32) {
|
||||
(
|
||||
self.motion_filter.passed_count(),
|
||||
self.motion_filter.filtered_count(),
|
||||
self.motion_filter.filter_efficiency(),
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use crate::core::normalize_angle;
|
||||
|
||||
#[test]
|
||||
fn test_explorer_creation() {
|
||||
let explorer = VastuExplorer::with_defaults();
|
||||
assert!(!explorer.started);
|
||||
assert_eq!(explorer.elapsed_secs(), 0.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_normalize_angle() {
|
||||
assert!((normalize_angle(0.0) - 0.0).abs() < 0.001);
|
||||
assert!((normalize_angle(std::f32::consts::PI * 2.0) - 0.0).abs() < 0.001);
|
||||
}
|
||||
}
|
||||
@@ -1,55 +0,0 @@
|
||||
//! High-level autonomous exploration API.
|
||||
//!
|
||||
//! This module provides a simple, unified API for autonomous map exploration.
|
||||
//! The `VastuExplorer` handles all the complexity of SLAM, scan matching,
|
||||
//! loop closure, and frontier-based exploration internally.
|
||||
//!
|
||||
//! # Example
|
||||
//!
|
||||
//! ```ignore
|
||||
//! use vastu_map::explore::{VastuExplorer, SensorSource, ExplorerConfig};
|
||||
//!
|
||||
//! // Implement SensorSource for your robot
|
||||
//! struct MyRobot { /* ... */ }
|
||||
//! impl SensorSource for MyRobot { /* ... */ }
|
||||
//!
|
||||
//! // Create explorer and run
|
||||
//! let config = ExplorerConfig::default();
|
||||
//! let mut explorer = VastuExplorer::new(config);
|
||||
//! let mut robot = MyRobot::new();
|
||||
//!
|
||||
//! match explorer.explore(&mut robot) {
|
||||
//! ExploreResult::Complete(map) => {
|
||||
//! // Save the completed map
|
||||
//! vastu_map::io::save_vastu(map.storage(), "map.vastu").unwrap();
|
||||
//! }
|
||||
//! ExploreResult::Failed { map, reason } => {
|
||||
//! println!("Exploration failed: {}", reason);
|
||||
//! }
|
||||
//! }
|
||||
//! ```
|
||||
|
||||
mod config;
|
||||
pub mod controller;
|
||||
mod error;
|
||||
mod explorer;
|
||||
mod motion_filter;
|
||||
mod source;
|
||||
mod submap_explorer;
|
||||
mod velocity;
|
||||
|
||||
pub use config::ExplorerConfig;
|
||||
pub use error::ExplorationError;
|
||||
pub use explorer::{ExploreResult, ExploreStatus, VastuExplorer};
|
||||
pub use motion_filter::{MotionFilter, MotionFilterConfig};
|
||||
pub use source::SensorSource;
|
||||
pub use submap_explorer::{SubmapExplorerConfig, VastuSubmapExplorer};
|
||||
pub use velocity::{
|
||||
VelocityConfig, compute_angular_velocity_to_heading, compute_velocity_to_target,
|
||||
};
|
||||
|
||||
// Re-export controller types for unified API
|
||||
pub use controller::{
|
||||
ExplorationCommand, ExplorationConfig, ExplorationController, ExplorationEvent,
|
||||
ExplorationProgress, ExplorationState, RecoveryAction,
|
||||
};
|
||||
@@ -1,349 +0,0 @@
|
||||
//! Motion filter for gating scan processing based on robot movement.
|
||||
//!
|
||||
//! The motion filter prevents redundant scan processing when the robot is stationary
|
||||
//! or moving slowly. A scan is only processed if sufficient time, distance, or rotation
|
||||
//! has occurred since the last processed scan.
|
||||
//!
|
||||
//! This improves CPU efficiency and ensures consistent submap sizes regardless of
|
||||
//! robot speed.
|
||||
|
||||
use std::time::Instant;
|
||||
|
||||
use serde::{Deserialize, Serialize};
|
||||
|
||||
use crate::core::{Pose2D, normalize_angle};
|
||||
|
||||
/// Configuration for the motion filter.
|
||||
///
|
||||
/// A scan passes the filter (should be processed) if ANY threshold is exceeded:
|
||||
/// - Time since last scan > max_time_secs
|
||||
/// - Distance traveled > max_distance
|
||||
/// - Rotation > max_angle
|
||||
#[derive(Clone, Debug, Serialize, Deserialize)]
|
||||
pub struct MotionFilterConfig {
|
||||
/// Maximum time between scan insertions (seconds).
|
||||
/// Ensures at least some updates even when stationary.
|
||||
/// Default: 5.0s (from Google Cartographer)
|
||||
#[serde(default = "default_max_time")]
|
||||
pub max_time_secs: f32,
|
||||
|
||||
/// Maximum distance before forcing scan insertion (meters).
|
||||
/// Default: 0.2m (20cm, from Google Cartographer)
|
||||
#[serde(default = "default_max_distance")]
|
||||
pub max_distance: f32,
|
||||
|
||||
/// Maximum rotation before forcing scan insertion (radians).
|
||||
/// Default: 0.17 rad (~10°, from Google Cartographer)
|
||||
#[serde(default = "default_max_angle")]
|
||||
pub max_angle: f32,
|
||||
|
||||
/// Whether the motion filter is enabled.
|
||||
/// When disabled, all scans pass through.
|
||||
/// Default: true
|
||||
#[serde(default = "default_enabled")]
|
||||
pub enabled: bool,
|
||||
}
|
||||
|
||||
fn default_max_time() -> f32 {
|
||||
5.0
|
||||
}
|
||||
|
||||
fn default_max_distance() -> f32 {
|
||||
0.2
|
||||
}
|
||||
|
||||
fn default_max_angle() -> f32 {
|
||||
0.17
|
||||
}
|
||||
|
||||
fn default_enabled() -> bool {
|
||||
true
|
||||
}
|
||||
|
||||
impl Default for MotionFilterConfig {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
max_time_secs: default_max_time(),
|
||||
max_distance: default_max_distance(),
|
||||
max_angle: default_max_angle(),
|
||||
enabled: default_enabled(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl MotionFilterConfig {
|
||||
/// Create a permissive filter that processes more scans.
|
||||
/// Useful for high-quality mapping at the cost of CPU.
|
||||
pub fn permissive() -> Self {
|
||||
Self {
|
||||
max_time_secs: 2.0,
|
||||
max_distance: 0.1,
|
||||
max_angle: 0.08,
|
||||
enabled: true,
|
||||
}
|
||||
}
|
||||
|
||||
/// Create a strict filter that processes fewer scans.
|
||||
/// Useful for CPU-constrained systems.
|
||||
pub fn strict() -> Self {
|
||||
Self {
|
||||
max_time_secs: 10.0,
|
||||
max_distance: 0.3,
|
||||
max_angle: 0.25,
|
||||
enabled: true,
|
||||
}
|
||||
}
|
||||
|
||||
/// Create a disabled filter (all scans pass through).
|
||||
pub fn disabled() -> Self {
|
||||
Self {
|
||||
enabled: false,
|
||||
..Default::default()
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Motion filter for gating scan processing.
|
||||
///
|
||||
/// The filter tracks the last processed pose and time, and determines
|
||||
/// whether a new scan should be processed based on the configured thresholds.
|
||||
pub struct MotionFilter {
|
||||
/// Pose when last scan was processed
|
||||
last_pose: Option<Pose2D>,
|
||||
/// Time when last scan was processed
|
||||
last_time: Option<Instant>,
|
||||
/// Configuration
|
||||
config: MotionFilterConfig,
|
||||
/// Count of scans that passed the filter
|
||||
passed_count: u64,
|
||||
/// Count of scans that were filtered out
|
||||
filtered_count: u64,
|
||||
}
|
||||
|
||||
impl MotionFilter {
|
||||
/// Create a new motion filter with the given configuration.
|
||||
pub fn new(config: MotionFilterConfig) -> Self {
|
||||
Self {
|
||||
last_pose: None,
|
||||
last_time: None,
|
||||
config,
|
||||
passed_count: 0,
|
||||
filtered_count: 0,
|
||||
}
|
||||
}
|
||||
|
||||
/// Check if a scan at the given pose should be processed.
|
||||
///
|
||||
/// Returns true if the scan should be processed, false if it should be skipped.
|
||||
/// Updates internal state when returning true.
|
||||
pub fn should_process(&mut self, pose: Pose2D) -> bool {
|
||||
// If disabled, always process
|
||||
if !self.config.enabled {
|
||||
self.passed_count += 1;
|
||||
return true;
|
||||
}
|
||||
|
||||
let now = Instant::now();
|
||||
|
||||
// First scan always passes
|
||||
let (Some(last_pose), Some(last_time)) = (self.last_pose, self.last_time) else {
|
||||
self.last_pose = Some(pose);
|
||||
self.last_time = Some(now);
|
||||
self.passed_count += 1;
|
||||
return true;
|
||||
};
|
||||
|
||||
// Check time threshold
|
||||
let dt = now.duration_since(last_time).as_secs_f32();
|
||||
if dt > self.config.max_time_secs {
|
||||
self.last_pose = Some(pose);
|
||||
self.last_time = Some(now);
|
||||
self.passed_count += 1;
|
||||
return true;
|
||||
}
|
||||
|
||||
// Check distance threshold
|
||||
let dx = pose.x - last_pose.x;
|
||||
let dy = pose.y - last_pose.y;
|
||||
let dist = (dx * dx + dy * dy).sqrt();
|
||||
if dist > self.config.max_distance {
|
||||
self.last_pose = Some(pose);
|
||||
self.last_time = Some(now);
|
||||
self.passed_count += 1;
|
||||
return true;
|
||||
}
|
||||
|
||||
// Check angle threshold
|
||||
let dtheta = normalize_angle(pose.theta - last_pose.theta).abs();
|
||||
if dtheta > self.config.max_angle {
|
||||
self.last_pose = Some(pose);
|
||||
self.last_time = Some(now);
|
||||
self.passed_count += 1;
|
||||
return true;
|
||||
}
|
||||
|
||||
// All thresholds within limits, filter this scan
|
||||
self.filtered_count += 1;
|
||||
false
|
||||
}
|
||||
|
||||
/// Reset the filter state.
|
||||
///
|
||||
/// Call this when starting a new exploration session.
|
||||
pub fn reset(&mut self) {
|
||||
self.last_pose = None;
|
||||
self.last_time = None;
|
||||
self.passed_count = 0;
|
||||
self.filtered_count = 0;
|
||||
}
|
||||
|
||||
/// Get the number of scans that passed the filter.
|
||||
pub fn passed_count(&self) -> u64 {
|
||||
self.passed_count
|
||||
}
|
||||
|
||||
/// Get the number of scans that were filtered out.
|
||||
pub fn filtered_count(&self) -> u64 {
|
||||
self.filtered_count
|
||||
}
|
||||
|
||||
/// Get the filter efficiency (fraction of scans filtered out).
|
||||
pub fn filter_efficiency(&self) -> f32 {
|
||||
let total = self.passed_count + self.filtered_count;
|
||||
if total == 0 {
|
||||
0.0
|
||||
} else {
|
||||
self.filtered_count as f32 / total as f32
|
||||
}
|
||||
}
|
||||
|
||||
/// Get the configuration.
|
||||
pub fn config(&self) -> &MotionFilterConfig {
|
||||
&self.config
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use std::thread::sleep;
|
||||
use std::time::Duration;
|
||||
|
||||
#[test]
|
||||
fn test_first_scan_always_passes() {
|
||||
let mut filter = MotionFilter::new(MotionFilterConfig::default());
|
||||
let pose = Pose2D::new(0.0, 0.0, 0.0);
|
||||
assert!(filter.should_process(pose));
|
||||
assert_eq!(filter.passed_count(), 1);
|
||||
assert_eq!(filter.filtered_count(), 0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_stationary_robot_filtered() {
|
||||
let mut filter = MotionFilter::new(MotionFilterConfig::default());
|
||||
let pose = Pose2D::new(1.0, 2.0, 0.5);
|
||||
|
||||
// First scan passes
|
||||
assert!(filter.should_process(pose));
|
||||
|
||||
// Same pose should be filtered
|
||||
assert!(!filter.should_process(pose));
|
||||
assert!(!filter.should_process(pose));
|
||||
|
||||
assert_eq!(filter.passed_count(), 1);
|
||||
assert_eq!(filter.filtered_count(), 2);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_distance_threshold() {
|
||||
let mut filter = MotionFilter::new(MotionFilterConfig::default());
|
||||
|
||||
// First scan
|
||||
assert!(filter.should_process(Pose2D::new(0.0, 0.0, 0.0)));
|
||||
|
||||
// Small movement (< 0.2m) - filtered
|
||||
assert!(!filter.should_process(Pose2D::new(0.1, 0.1, 0.0)));
|
||||
|
||||
// Large movement (> 0.2m) - passes
|
||||
assert!(filter.should_process(Pose2D::new(0.5, 0.0, 0.0)));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_angle_threshold() {
|
||||
let mut filter = MotionFilter::new(MotionFilterConfig::default());
|
||||
|
||||
// First scan
|
||||
assert!(filter.should_process(Pose2D::new(0.0, 0.0, 0.0)));
|
||||
|
||||
// Small rotation (< 0.17 rad) - filtered
|
||||
assert!(!filter.should_process(Pose2D::new(0.0, 0.0, 0.1)));
|
||||
|
||||
// Large rotation (> 0.17 rad) - passes
|
||||
assert!(filter.should_process(Pose2D::new(0.0, 0.0, 0.5)));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_time_threshold() {
|
||||
let config = MotionFilterConfig {
|
||||
max_time_secs: 0.05, // 50ms for fast test
|
||||
..Default::default()
|
||||
};
|
||||
let mut filter = MotionFilter::new(config);
|
||||
|
||||
// First scan
|
||||
assert!(filter.should_process(Pose2D::new(0.0, 0.0, 0.0)));
|
||||
|
||||
// Same pose immediately - filtered
|
||||
assert!(!filter.should_process(Pose2D::new(0.0, 0.0, 0.0)));
|
||||
|
||||
// Wait for time threshold
|
||||
sleep(Duration::from_millis(60));
|
||||
|
||||
// Same pose after timeout - passes
|
||||
assert!(filter.should_process(Pose2D::new(0.0, 0.0, 0.0)));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_disabled_filter() {
|
||||
let mut filter = MotionFilter::new(MotionFilterConfig::disabled());
|
||||
let pose = Pose2D::new(0.0, 0.0, 0.0);
|
||||
|
||||
// All scans should pass when disabled
|
||||
assert!(filter.should_process(pose));
|
||||
assert!(filter.should_process(pose));
|
||||
assert!(filter.should_process(pose));
|
||||
|
||||
assert_eq!(filter.passed_count(), 3);
|
||||
assert_eq!(filter.filtered_count(), 0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_reset() {
|
||||
let mut filter = MotionFilter::new(MotionFilterConfig::default());
|
||||
|
||||
filter.should_process(Pose2D::new(0.0, 0.0, 0.0));
|
||||
filter.should_process(Pose2D::new(0.0, 0.0, 0.0));
|
||||
|
||||
filter.reset();
|
||||
|
||||
assert_eq!(filter.passed_count(), 0);
|
||||
assert_eq!(filter.filtered_count(), 0);
|
||||
|
||||
// After reset, first scan passes again
|
||||
assert!(filter.should_process(Pose2D::new(0.0, 0.0, 0.0)));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_filter_efficiency() {
|
||||
let mut filter = MotionFilter::new(MotionFilterConfig::default());
|
||||
|
||||
filter.should_process(Pose2D::new(0.0, 0.0, 0.0)); // pass
|
||||
filter.should_process(Pose2D::new(0.0, 0.0, 0.0)); // filter
|
||||
filter.should_process(Pose2D::new(0.0, 0.0, 0.0)); // filter
|
||||
filter.should_process(Pose2D::new(1.0, 0.0, 0.0)); // pass (distance)
|
||||
|
||||
assert_eq!(filter.passed_count(), 2);
|
||||
assert_eq!(filter.filtered_count(), 2);
|
||||
assert!((filter.filter_efficiency() - 0.5).abs() < 0.001);
|
||||
}
|
||||
}
|
||||
@@ -1,101 +0,0 @@
|
||||
//! Sensor source trait for robot abstraction.
|
||||
|
||||
use crate::core::{LidarScan, Pose2D};
|
||||
|
||||
/// Trait for providing sensor data and accepting commands.
|
||||
///
|
||||
/// Implement this trait to connect VastuExplorer to your robot hardware
|
||||
/// or simulation environment.
|
||||
///
|
||||
/// # Example
|
||||
///
|
||||
/// ```ignore
|
||||
/// struct MyRobot {
|
||||
/// // Hardware connections
|
||||
/// }
|
||||
///
|
||||
/// impl SensorSource for MyRobot {
|
||||
/// fn get_pose(&self) -> Pose2D {
|
||||
/// // Read from encoders/odometry
|
||||
/// Pose2D::new(self.x, self.y, self.theta)
|
||||
/// }
|
||||
///
|
||||
/// fn get_lidar_scan(&self) -> Option<LidarScan> {
|
||||
/// // Read from lidar sensor
|
||||
/// Some(self.latest_scan.clone())
|
||||
/// }
|
||||
///
|
||||
/// fn send_velocity(&mut self, linear: f32, angular: f32) {
|
||||
/// // Send to motor controller
|
||||
/// self.motor.set_velocity(linear, angular);
|
||||
/// }
|
||||
///
|
||||
/// fn is_connected(&self) -> bool {
|
||||
/// self.connection.is_alive()
|
||||
/// }
|
||||
/// }
|
||||
/// ```
|
||||
pub trait SensorSource {
|
||||
/// Get current robot pose from odometry/encoders.
|
||||
///
|
||||
/// This is the initial pose estimate before scan matching.
|
||||
/// The pose should be in the world frame (x, y in meters, theta in radians).
|
||||
fn get_pose(&self) -> Pose2D;
|
||||
|
||||
/// Get the latest lidar scan.
|
||||
///
|
||||
/// Returns None if no new scan is available since the last call.
|
||||
/// The scan should contain ranges in meters and angles in radians.
|
||||
fn get_lidar_scan(&self) -> Option<LidarScan>;
|
||||
|
||||
/// Send velocity command to the robot.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `linear` - Forward velocity in m/s (positive = forward)
|
||||
/// * `angular` - Angular velocity in rad/s (positive = counter-clockwise)
|
||||
fn send_velocity(&mut self, linear: f32, angular: f32);
|
||||
|
||||
/// Check if the robot is still connected and operational.
|
||||
///
|
||||
/// Returns false if the connection is lost or the robot is in an error state.
|
||||
/// The explorer will stop if this returns false.
|
||||
fn is_connected(&self) -> bool;
|
||||
|
||||
/// Update sensor readings (optional polling).
|
||||
///
|
||||
/// Called at the beginning of each exploration step.
|
||||
/// Use this to poll sensors if they don't push data automatically.
|
||||
/// Default implementation does nothing.
|
||||
fn poll(&mut self) {}
|
||||
|
||||
/// Get cliff sensor state (optional).
|
||||
///
|
||||
/// Returns true if any cliff sensor is triggered.
|
||||
/// Default returns false (no cliff sensors).
|
||||
fn is_cliff_detected(&self) -> bool {
|
||||
false
|
||||
}
|
||||
|
||||
/// Get bumper state (optional).
|
||||
///
|
||||
/// Returns true if any bumper is triggered.
|
||||
/// Default returns false (no bumpers).
|
||||
fn is_bumper_triggered(&self) -> bool {
|
||||
false
|
||||
}
|
||||
|
||||
/// Stop the robot immediately (emergency or normal stop).
|
||||
///
|
||||
/// Default implementation sends zero velocity.
|
||||
fn stop(&mut self) {
|
||||
self.send_velocity(0.0, 0.0);
|
||||
}
|
||||
|
||||
/// Get current battery level as percentage (0-100).
|
||||
///
|
||||
/// Returns None if battery monitoring is not available.
|
||||
/// Default returns None.
|
||||
fn battery_percent(&self) -> Option<u8> {
|
||||
None
|
||||
}
|
||||
}
|
||||
@@ -1,570 +0,0 @@
|
||||
//! VastuSubmapExplorer - Submap-based autonomous exploration.
|
||||
//!
|
||||
//! This explorer uses the submap architecture for proper loop closure correction.
|
||||
//! Unlike `VastuExplorer` which uses a single global map, this version:
|
||||
//! - Stores raw scans in submaps for reversible map updates
|
||||
//! - Uses submap-level pose graph optimization
|
||||
//! - Can correctly apply loop closure corrections without ghost walls
|
||||
|
||||
use std::time::Instant;
|
||||
|
||||
use serde::{Deserialize, Serialize};
|
||||
|
||||
use super::controller::{
|
||||
ExplorationCommand, ExplorationController, ExplorationEvent, ExplorationProgress,
|
||||
};
|
||||
use super::error::ExplorationError;
|
||||
use super::motion_filter::MotionFilter;
|
||||
use super::velocity::{
|
||||
VelocityConfig, compute_angular_velocity_to_heading, compute_velocity_to_target,
|
||||
};
|
||||
use crate::core::{LidarScan, Pose2D};
|
||||
use crate::grid::GridStorage;
|
||||
use crate::slam::CorrelativeMatcherConfig;
|
||||
use crate::submap::{
|
||||
MultiSubmapMatchConfig, SubmapConfig, SubmapLoopClosure, SubmapManager, SubmapPoseGraph,
|
||||
};
|
||||
|
||||
use super::config::ExplorerConfig;
|
||||
use super::explorer::{ExploreResult, ExploreStatus};
|
||||
use super::source::SensorSource;
|
||||
|
||||
/// Configuration specific to submap-based exploration.
|
||||
#[derive(Clone, Debug, Serialize, Deserialize)]
|
||||
pub struct SubmapExplorerConfig {
|
||||
/// Base explorer configuration.
|
||||
#[serde(default)]
|
||||
pub base: ExplorerConfig,
|
||||
/// Submap configuration.
|
||||
#[serde(default)]
|
||||
pub submap: SubmapConfig,
|
||||
/// Multi-submap matching configuration.
|
||||
#[serde(default)]
|
||||
pub multi_match: MultiSubmapMatchConfig,
|
||||
/// Minimum score to accept a loop closure.
|
||||
#[serde(default = "default_loop_closure_min_score")]
|
||||
pub loop_closure_min_score: f32,
|
||||
/// Minimum submap gap for loop closure (submaps between).
|
||||
#[serde(default = "default_loop_closure_min_gap")]
|
||||
pub loop_closure_min_gap: usize,
|
||||
}
|
||||
|
||||
fn default_loop_closure_min_score() -> f32 {
|
||||
0.7
|
||||
}
|
||||
|
||||
fn default_loop_closure_min_gap() -> usize {
|
||||
3
|
||||
}
|
||||
|
||||
impl Default for SubmapExplorerConfig {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
base: ExplorerConfig::default(),
|
||||
submap: SubmapConfig::default(),
|
||||
multi_match: MultiSubmapMatchConfig::default(),
|
||||
loop_closure_min_score: default_loop_closure_min_score(),
|
||||
loop_closure_min_gap: default_loop_closure_min_gap(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl SubmapExplorerConfig {
|
||||
/// Create a fast configuration for quick testing.
|
||||
pub fn fast() -> Self {
|
||||
Self {
|
||||
base: ExplorerConfig::fast(),
|
||||
submap: SubmapConfig {
|
||||
scans_per_submap: 30,
|
||||
overlap_scans: 5,
|
||||
..Default::default()
|
||||
},
|
||||
..Default::default()
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// High-level submap-based exploration controller.
|
||||
///
|
||||
/// This explorer uses the submap architecture for improved SLAM accuracy,
|
||||
/// especially during loop closure. When a loop is detected, submap origins
|
||||
/// are corrected and grids are regenerated from stored scans.
|
||||
///
|
||||
/// # Key Differences from VastuExplorer
|
||||
///
|
||||
/// 1. **Submap Management**: Scans are organized into locally-consistent submaps
|
||||
/// 2. **Reversible Updates**: Raw scans are stored for regeneration after optimization
|
||||
/// 3. **Submap-level Pose Graph**: Fewer nodes (one per submap) for efficient optimization
|
||||
/// 4. **Multi-submap Matching**: Can match against active + overlapping finalized submaps
|
||||
pub struct VastuSubmapExplorer {
|
||||
/// Configuration.
|
||||
config: SubmapExplorerConfig,
|
||||
/// Submap manager (replaces OccupancyGridMap).
|
||||
manager: SubmapManager,
|
||||
/// Submap-level pose graph.
|
||||
pose_graph: SubmapPoseGraph,
|
||||
/// Exploration controller.
|
||||
controller: ExplorationController,
|
||||
/// Matcher configuration.
|
||||
matcher_config: CorrelativeMatcherConfig,
|
||||
/// Motion filter for gating scan processing.
|
||||
motion_filter: MotionFilter,
|
||||
/// Velocity computation configuration.
|
||||
velocity_config: VelocityConfig,
|
||||
/// Last corrected world pose.
|
||||
last_pose: Pose2D,
|
||||
/// Last odometry pose (for delta calculation).
|
||||
last_odom_pose: Option<Pose2D>,
|
||||
/// Last submap origin pose (for computing delta between submaps).
|
||||
last_submap_origin: Option<Pose2D>,
|
||||
/// Scan counter for timestamps.
|
||||
scan_count: u64,
|
||||
/// Last submap count (to detect new submap creation).
|
||||
last_submap_count: usize,
|
||||
/// Start time.
|
||||
start_time: Option<Instant>,
|
||||
/// Whether exploration has started.
|
||||
started: bool,
|
||||
}
|
||||
|
||||
impl VastuSubmapExplorer {
|
||||
/// Create a new submap explorer with the given configuration.
|
||||
pub fn new(config: SubmapExplorerConfig) -> Self {
|
||||
let map_config = config.base.to_map_config();
|
||||
let manager = SubmapManager::new(config.submap.clone(), &map_config);
|
||||
let pose_graph = SubmapPoseGraph::new(Default::default());
|
||||
let controller = ExplorationController::new(config.base.exploration.clone());
|
||||
let matcher_config = config.base.matching.clone();
|
||||
let motion_filter = MotionFilter::new(config.base.motion_filter.clone());
|
||||
let velocity_config = VelocityConfig::default();
|
||||
|
||||
Self {
|
||||
config,
|
||||
manager,
|
||||
pose_graph,
|
||||
controller,
|
||||
matcher_config,
|
||||
motion_filter,
|
||||
velocity_config,
|
||||
last_pose: Pose2D::default(),
|
||||
last_odom_pose: None,
|
||||
last_submap_origin: None,
|
||||
scan_count: 0,
|
||||
last_submap_count: 0,
|
||||
start_time: None,
|
||||
started: false,
|
||||
}
|
||||
}
|
||||
|
||||
/// Create with default configuration.
|
||||
pub fn with_defaults() -> Self {
|
||||
Self::new(SubmapExplorerConfig::default())
|
||||
}
|
||||
|
||||
/// Get a reference to the current global grid for visualization/planning.
|
||||
///
|
||||
/// This lazily regenerates the global grid if needed.
|
||||
pub fn global_grid(&mut self) -> &GridStorage {
|
||||
self.manager.global_grid()
|
||||
}
|
||||
|
||||
/// Get the submap manager for inspection.
|
||||
pub fn manager(&self) -> &SubmapManager {
|
||||
&self.manager
|
||||
}
|
||||
|
||||
/// Get exploration progress.
|
||||
pub fn progress(&self) -> ExplorationProgress {
|
||||
self.controller.progress()
|
||||
}
|
||||
|
||||
/// Get the current corrected pose.
|
||||
pub fn current_pose(&self) -> Pose2D {
|
||||
self.last_pose
|
||||
}
|
||||
|
||||
/// Get elapsed time since exploration started.
|
||||
pub fn elapsed_secs(&self) -> f32 {
|
||||
self.start_time
|
||||
.map(|t| t.elapsed().as_secs_f32())
|
||||
.unwrap_or(0.0)
|
||||
}
|
||||
|
||||
/// Get the number of submaps created.
|
||||
pub fn submap_count(&self) -> usize {
|
||||
self.manager.submaps().len()
|
||||
}
|
||||
|
||||
/// Get the number of scans processed.
|
||||
pub fn scan_count(&self) -> u64 {
|
||||
self.scan_count
|
||||
}
|
||||
|
||||
/// Has a loop closure been detected and applied?
|
||||
pub fn has_loop_closures(&self) -> bool {
|
||||
self.pose_graph.has_loops()
|
||||
}
|
||||
|
||||
/// Run exploration to completion (blocking).
|
||||
pub fn explore<S: SensorSource>(mut self, source: &mut S) -> ExploreResult {
|
||||
self.controller.start();
|
||||
self.start_time = Some(Instant::now());
|
||||
self.started = true;
|
||||
|
||||
let step_duration =
|
||||
std::time::Duration::from_secs_f32(1.0 / self.config.base.update_rate_hz);
|
||||
|
||||
loop {
|
||||
let step_start = Instant::now();
|
||||
|
||||
// Check connection
|
||||
if !source.is_connected() {
|
||||
source.stop();
|
||||
return ExploreResult::Failed {
|
||||
map: self.build_occupancy_grid_map(),
|
||||
error: ExplorationError::ConnectionLost,
|
||||
};
|
||||
}
|
||||
|
||||
// Check battery
|
||||
if let Some(battery) = source.battery_percent()
|
||||
&& battery < self.config.base.min_battery_percent
|
||||
{
|
||||
source.stop();
|
||||
return ExploreResult::Failed {
|
||||
map: self.build_occupancy_grid_map(),
|
||||
error: ExplorationError::LowBattery {
|
||||
percent: battery,
|
||||
threshold: self.config.base.min_battery_percent,
|
||||
},
|
||||
};
|
||||
}
|
||||
|
||||
// Check time limit
|
||||
if self.config.base.max_time_secs > 0.0
|
||||
&& self.elapsed_secs() > self.config.base.max_time_secs
|
||||
{
|
||||
source.stop();
|
||||
return ExploreResult::Failed {
|
||||
map: self.build_occupancy_grid_map(),
|
||||
error: ExplorationError::TimeoutExceeded {
|
||||
elapsed_secs: self.elapsed_secs(),
|
||||
limit_secs: self.config.base.max_time_secs,
|
||||
},
|
||||
};
|
||||
}
|
||||
|
||||
// Run one exploration step
|
||||
match self.step(source) {
|
||||
ExploreStatus::InProgress { .. } => {
|
||||
// Continue
|
||||
}
|
||||
ExploreStatus::Complete => {
|
||||
source.stop();
|
||||
return ExploreResult::Complete(self.build_occupancy_grid_map());
|
||||
}
|
||||
ExploreStatus::Failed { error } => {
|
||||
source.stop();
|
||||
return ExploreResult::Failed {
|
||||
map: self.build_occupancy_grid_map(),
|
||||
error,
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
// Rate limiting
|
||||
let elapsed = step_start.elapsed();
|
||||
if elapsed < step_duration {
|
||||
std::thread::sleep(step_duration - elapsed);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Run a single exploration step (non-blocking).
|
||||
pub fn step<S: SensorSource>(&mut self, source: &mut S) -> ExploreStatus {
|
||||
// Initialize if needed
|
||||
if !self.started {
|
||||
self.controller.start();
|
||||
self.start_time = Some(Instant::now());
|
||||
self.started = true;
|
||||
}
|
||||
|
||||
// Poll sensors
|
||||
source.poll();
|
||||
|
||||
// Get current odometry pose
|
||||
let odom_pose = source.get_pose();
|
||||
|
||||
// Calculate odometry delta
|
||||
let odom_delta = self.last_odom_pose.map(|last| {
|
||||
Pose2D::new(
|
||||
odom_pose.x - last.x,
|
||||
odom_pose.y - last.y,
|
||||
odom_pose.theta - last.theta,
|
||||
)
|
||||
});
|
||||
self.last_odom_pose = Some(odom_pose);
|
||||
|
||||
// Process lidar scan if available and motion filter allows
|
||||
if let Some(scan) = source.get_lidar_scan() {
|
||||
if self.motion_filter.should_process(odom_pose) {
|
||||
let corrected_pose = self.process_scan(&scan, odom_pose);
|
||||
self.last_pose = corrected_pose;
|
||||
} else if let Some(delta) = odom_delta {
|
||||
// Scan filtered out - update pose from odometry delta only
|
||||
self.last_pose = Pose2D::new(
|
||||
self.last_pose.x + delta.x,
|
||||
self.last_pose.y + delta.y,
|
||||
self.last_pose.theta + delta.theta,
|
||||
);
|
||||
}
|
||||
} else if let Some(delta) = odom_delta {
|
||||
// No scan - update from odometry delta
|
||||
self.last_pose = Pose2D::new(
|
||||
self.last_pose.x + delta.x,
|
||||
self.last_pose.y + delta.y,
|
||||
self.last_pose.theta + delta.theta,
|
||||
);
|
||||
}
|
||||
|
||||
// Handle cliff/bumper events
|
||||
if source.is_cliff_detected() || source.is_bumper_triggered() {
|
||||
self.controller
|
||||
.handle_event(ExplorationEvent::ObstacleDetected);
|
||||
}
|
||||
|
||||
// Get current global grid for planning
|
||||
let global_grid = self.manager.global_grid();
|
||||
|
||||
// Update exploration controller
|
||||
let command = self.controller.update(self.last_pose, global_grid);
|
||||
|
||||
// Execute command
|
||||
match command {
|
||||
ExplorationCommand::MoveTo { target, max_speed } => {
|
||||
let (linear, angular) = compute_velocity_to_target(
|
||||
self.last_pose,
|
||||
target,
|
||||
max_speed,
|
||||
&self.velocity_config,
|
||||
);
|
||||
source.send_velocity(linear, angular);
|
||||
}
|
||||
ExplorationCommand::Rotate {
|
||||
target_heading,
|
||||
max_angular_speed,
|
||||
} => {
|
||||
let angular = compute_angular_velocity_to_heading(
|
||||
self.last_pose.theta,
|
||||
target_heading,
|
||||
max_angular_speed,
|
||||
&self.velocity_config,
|
||||
);
|
||||
source.send_velocity(0.0, angular);
|
||||
}
|
||||
ExplorationCommand::Stop => {
|
||||
source.stop();
|
||||
}
|
||||
ExplorationCommand::ExplorationComplete => {
|
||||
source.stop();
|
||||
return ExploreStatus::Complete;
|
||||
}
|
||||
ExplorationCommand::ExplorationFailed { reason } => {
|
||||
source.stop();
|
||||
return ExploreStatus::Failed {
|
||||
error: ExplorationError::from_message(reason),
|
||||
};
|
||||
}
|
||||
ExplorationCommand::None => {
|
||||
// Continue with current velocity or stop
|
||||
}
|
||||
}
|
||||
|
||||
ExploreStatus::InProgress {
|
||||
progress: self.controller.progress(),
|
||||
}
|
||||
}
|
||||
|
||||
/// Process a lidar scan with submap-based SLAM.
|
||||
fn process_scan(&mut self, scan: &LidarScan, odom_pose: Pose2D) -> Pose2D {
|
||||
// Match against active + overlapping submaps
|
||||
let match_result = self.manager.match_scan_multi(
|
||||
scan,
|
||||
odom_pose,
|
||||
&self.matcher_config,
|
||||
&self.config.multi_match,
|
||||
);
|
||||
|
||||
// Use matched pose if converged
|
||||
let corrected_pose = if match_result.converged() {
|
||||
match_result.world_pose()
|
||||
} else {
|
||||
odom_pose
|
||||
};
|
||||
|
||||
// Track submap count before insert
|
||||
let submaps_before = self.manager.submaps().len();
|
||||
|
||||
// Insert scan into submap manager
|
||||
self.scan_count += 1;
|
||||
let _insert_result = self
|
||||
.manager
|
||||
.insert_scan(scan, corrected_pose, self.scan_count);
|
||||
|
||||
// Check if new submap was created
|
||||
let submaps_after = self.manager.submaps().len();
|
||||
if submaps_after > submaps_before {
|
||||
// New submap created - add to pose graph
|
||||
if let Some(active) = self.manager.active_submap() {
|
||||
// Compute delta from previous submap origin
|
||||
let odom_delta = self.last_submap_origin.map(|prev| {
|
||||
Pose2D::new(
|
||||
active.origin.x - prev.x,
|
||||
active.origin.y - prev.y,
|
||||
active.origin.theta - prev.theta,
|
||||
)
|
||||
});
|
||||
|
||||
self.pose_graph
|
||||
.add_submap(active.id, active.origin, odom_delta);
|
||||
self.last_submap_origin = Some(active.origin);
|
||||
}
|
||||
|
||||
// Check for loop closure candidates
|
||||
self.check_loop_closure(scan, corrected_pose);
|
||||
|
||||
self.last_submap_count = submaps_after;
|
||||
}
|
||||
|
||||
corrected_pose
|
||||
}
|
||||
|
||||
/// Check for loop closure against finalized submaps.
|
||||
fn check_loop_closure(&mut self, scan: &LidarScan, current_pose: Pose2D) {
|
||||
use crate::submap::SubmapId;
|
||||
|
||||
// Collect candidate info to avoid borrow issues
|
||||
let candidates: Vec<(SubmapId, Pose2D)> = self
|
||||
.manager
|
||||
.loop_closure_candidates(current_pose)
|
||||
.iter()
|
||||
.map(|s| (s.id, s.origin))
|
||||
.collect();
|
||||
|
||||
for (candidate_id, candidate_origin) in candidates {
|
||||
// Skip if already have a loop edge to this submap
|
||||
if self
|
||||
.pose_graph
|
||||
.loop_edges()
|
||||
.iter()
|
||||
.any(|e| e.from_id == candidate_id || e.to_id == candidate_id)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
// Get the candidate submap's grid for matching
|
||||
let Some(candidate) = self.manager.get_submap(candidate_id) else {
|
||||
continue;
|
||||
};
|
||||
|
||||
// Match against the candidate submap
|
||||
let matcher = crate::slam::CorrelativeMatcher::new(self.matcher_config.clone());
|
||||
let local_pose = candidate_origin.inverse().compose(¤t_pose);
|
||||
let match_result = matcher.match_scan(scan, local_pose, &candidate.grid);
|
||||
|
||||
if match_result.converged && match_result.score >= self.config.loop_closure_min_score {
|
||||
// Good match - add loop closure constraint
|
||||
let matched_world_pose = candidate_origin.compose(&match_result.pose);
|
||||
|
||||
if let Some(active) = self.manager.active_submap() {
|
||||
// Compute relative pose between submaps
|
||||
let relative_pose = candidate_origin.inverse().compose(&matched_world_pose);
|
||||
|
||||
let closure = SubmapLoopClosure {
|
||||
from_submap: active.id,
|
||||
to_submap: candidate_id,
|
||||
relative_pose,
|
||||
confidence: match_result.score,
|
||||
};
|
||||
|
||||
self.pose_graph.add_loop_closure(closure);
|
||||
|
||||
// Optimize pose graph
|
||||
let (_, corrections) = self.pose_graph.optimize();
|
||||
|
||||
// Apply corrections to submaps
|
||||
if !corrections.is_empty() {
|
||||
self.manager.apply_corrections(&corrections);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Convert to OccupancyGridMap for compatibility with ExploreResult.
|
||||
fn build_occupancy_grid_map(&mut self) -> crate::OccupancyGridMap {
|
||||
let map_config = self.config.base.to_map_config();
|
||||
let mut map = crate::OccupancyGridMap::new(map_config);
|
||||
|
||||
// Copy global grid contents
|
||||
let global = self.manager.global_grid();
|
||||
let dest_storage = map.storage_mut();
|
||||
|
||||
// Copy each cell from global grid to map storage
|
||||
for (coord, cell) in global.iter() {
|
||||
if let Some(dest_cell) = dest_storage.get_mut(coord) {
|
||||
*dest_cell.cell_type = cell.cell_type as u8;
|
||||
*dest_cell.observation_count = cell.observation_count;
|
||||
*dest_cell.swept = if cell.swept { 1 } else { 0 };
|
||||
}
|
||||
}
|
||||
|
||||
map
|
||||
}
|
||||
|
||||
/// Reset explorer for a new exploration run.
|
||||
pub fn reset(&mut self) {
|
||||
let map_config = self.config.base.to_map_config();
|
||||
self.manager = SubmapManager::new(self.config.submap.clone(), &map_config);
|
||||
self.pose_graph = SubmapPoseGraph::new(Default::default());
|
||||
self.controller = ExplorationController::new(self.config.base.exploration.clone());
|
||||
self.motion_filter.reset();
|
||||
self.last_pose = Pose2D::default();
|
||||
self.last_odom_pose = None;
|
||||
self.last_submap_origin = None;
|
||||
self.scan_count = 0;
|
||||
self.last_submap_count = 0;
|
||||
self.start_time = None;
|
||||
self.started = false;
|
||||
}
|
||||
|
||||
/// Get motion filter statistics.
|
||||
///
|
||||
/// Returns (passed_count, filtered_count, efficiency).
|
||||
pub fn motion_filter_stats(&self) -> (u64, u64, f32) {
|
||||
(
|
||||
self.motion_filter.passed_count(),
|
||||
self.motion_filter.filtered_count(),
|
||||
self.motion_filter.filter_efficiency(),
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn test_submap_explorer_creation() {
|
||||
let explorer = VastuSubmapExplorer::with_defaults();
|
||||
assert!(!explorer.started);
|
||||
assert_eq!(explorer.scan_count(), 0);
|
||||
assert_eq!(explorer.submap_count(), 0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_submap_explorer_config() {
|
||||
let config = SubmapExplorerConfig::fast();
|
||||
assert_eq!(config.submap.scans_per_submap, 30);
|
||||
}
|
||||
}
|
||||
@@ -1,167 +0,0 @@
|
||||
//! Velocity computation utilities for exploration.
|
||||
//!
|
||||
//! Shared functions for computing linear and angular velocities
|
||||
//! to navigate towards target positions.
|
||||
|
||||
use crate::core::{Pose2D, WorldPoint, normalize_angle};
|
||||
|
||||
/// Configuration for velocity computation.
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct VelocityConfig {
|
||||
/// Angle threshold (radians) - turn in place if angle error exceeds this.
|
||||
/// Default: 0.3 rad (~17 degrees)
|
||||
pub turn_threshold: f32,
|
||||
|
||||
/// Proportional gain for angular velocity.
|
||||
/// Default: 2.0
|
||||
pub angular_gain: f32,
|
||||
|
||||
/// Maximum angular velocity (rad/s).
|
||||
/// Default: 2.0
|
||||
pub max_angular_velocity: f32,
|
||||
|
||||
/// Distance threshold to consider target reached (meters).
|
||||
/// Default: 0.05m (5cm)
|
||||
pub reached_threshold: f32,
|
||||
|
||||
/// Angle threshold to consider heading reached (radians).
|
||||
/// Default: 0.05 rad (~3 degrees)
|
||||
pub heading_reached_threshold: f32,
|
||||
}
|
||||
|
||||
impl Default for VelocityConfig {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
turn_threshold: 0.3,
|
||||
angular_gain: 2.0,
|
||||
max_angular_velocity: 2.0,
|
||||
reached_threshold: 0.05,
|
||||
heading_reached_threshold: 0.05,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Compute velocity command to move towards a target position.
|
||||
///
|
||||
/// Returns (linear_velocity, angular_velocity) in (m/s, rad/s).
|
||||
///
|
||||
/// The behavior is:
|
||||
/// 1. If angle to target exceeds `turn_threshold`, rotate in place first
|
||||
/// 2. Otherwise, move forward with proportional angular correction
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `pose` - Current robot pose
|
||||
/// * `target` - Target position in world coordinates
|
||||
/// * `max_speed` - Maximum linear speed (m/s)
|
||||
/// * `config` - Velocity computation configuration
|
||||
pub fn compute_velocity_to_target(
|
||||
pose: Pose2D,
|
||||
target: WorldPoint,
|
||||
max_speed: f32,
|
||||
config: &VelocityConfig,
|
||||
) -> (f32, f32) {
|
||||
let dx = target.x - pose.x;
|
||||
let dy = target.y - pose.y;
|
||||
let distance = (dx * dx + dy * dy).sqrt();
|
||||
|
||||
// Target reached
|
||||
if distance < config.reached_threshold {
|
||||
return (0.0, 0.0);
|
||||
}
|
||||
|
||||
let target_angle = dy.atan2(dx);
|
||||
let angle_error = normalize_angle(target_angle - pose.theta);
|
||||
|
||||
// Turn towards target first if angle is large
|
||||
if angle_error.abs() > config.turn_threshold {
|
||||
let angular = angle_error.signum()
|
||||
* config
|
||||
.max_angular_velocity
|
||||
.min(angle_error.abs() * config.angular_gain);
|
||||
return (0.0, angular);
|
||||
}
|
||||
|
||||
// Move forward with proportional angular correction
|
||||
let linear = max_speed.min(distance);
|
||||
let angular = (angle_error * config.angular_gain)
|
||||
.clamp(-config.max_angular_velocity, config.max_angular_velocity);
|
||||
|
||||
(linear, angular)
|
||||
}
|
||||
|
||||
/// Compute angular velocity to rotate to a target heading.
|
||||
///
|
||||
/// Returns angular velocity in rad/s.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `current_heading` - Current heading (radians)
|
||||
/// * `target_heading` - Target heading (radians)
|
||||
/// * `max_speed` - Maximum angular speed (rad/s)
|
||||
/// * `config` - Velocity computation configuration
|
||||
pub fn compute_angular_velocity_to_heading(
|
||||
current_heading: f32,
|
||||
target_heading: f32,
|
||||
max_speed: f32,
|
||||
config: &VelocityConfig,
|
||||
) -> f32 {
|
||||
let error = normalize_angle(target_heading - current_heading);
|
||||
|
||||
if error.abs() < config.heading_reached_threshold {
|
||||
0.0
|
||||
} else {
|
||||
(error * config.angular_gain).clamp(-max_speed, max_speed)
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use std::f32::consts::PI;
|
||||
|
||||
#[test]
|
||||
fn test_target_reached() {
|
||||
let config = VelocityConfig::default();
|
||||
let pose = Pose2D::new(1.0, 1.0, 0.0);
|
||||
let target = WorldPoint::new(1.02, 1.02); // Within 5cm
|
||||
|
||||
let (linear, angular) = compute_velocity_to_target(pose, target, 0.5, &config);
|
||||
assert_eq!(linear, 0.0);
|
||||
assert_eq!(angular, 0.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_turn_first_when_angle_large() {
|
||||
let config = VelocityConfig::default();
|
||||
let pose = Pose2D::new(0.0, 0.0, 0.0); // Facing +X
|
||||
let target = WorldPoint::new(0.0, 1.0); // Target is +Y (90 degrees away)
|
||||
|
||||
let (linear, angular) = compute_velocity_to_target(pose, target, 0.5, &config);
|
||||
assert_eq!(linear, 0.0); // Should not move forward
|
||||
assert!(angular > 0.0); // Should turn left (positive)
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_move_forward_when_aligned() {
|
||||
let config = VelocityConfig::default();
|
||||
let pose = Pose2D::new(0.0, 0.0, 0.0); // Facing +X
|
||||
let target = WorldPoint::new(1.0, 0.0); // Target straight ahead
|
||||
|
||||
let (linear, angular) = compute_velocity_to_target(pose, target, 0.5, &config);
|
||||
assert!(linear > 0.0); // Should move forward
|
||||
assert!(angular.abs() < 0.01); // Minimal angular correction
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_heading_reached() {
|
||||
let config = VelocityConfig::default();
|
||||
let angular = compute_angular_velocity_to_heading(0.0, 0.02, 1.0, &config);
|
||||
assert_eq!(angular, 0.0); // Within threshold
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_rotate_to_heading() {
|
||||
let config = VelocityConfig::default();
|
||||
let angular = compute_angular_velocity_to_heading(0.0, PI / 2.0, 1.0, &config);
|
||||
assert!(angular > 0.0); // Should turn positive (counter-clockwise)
|
||||
}
|
||||
}
|
||||
@@ -1,19 +0,0 @@
|
||||
//! Occupancy grid implementation.
|
||||
//!
|
||||
//! This module provides the core occupancy grid storage and update mechanisms:
|
||||
//!
|
||||
//! - [`GridStorage`]: Core grid storage with coordinate conversion
|
||||
//! - [`raycaster`]: Bresenham ray casting for lidar updates
|
||||
//! - [`lidar_update`]: Process lidar scans to mark Floor/Wall cells
|
||||
//! - [`cliff_update`]: Process cliff sensors to mark Cliff cells
|
||||
//! - [`bumper_update`]: Process bumper collisions to mark Bump cells
|
||||
|
||||
pub mod bumper_update;
|
||||
pub mod cliff_update;
|
||||
mod config;
|
||||
pub mod lidar_update;
|
||||
pub mod raycaster;
|
||||
mod storage;
|
||||
|
||||
pub use config::{ConfigError, GridConfig, LogOddsConfig, MapConfig, SensorConfig};
|
||||
pub use storage::{CellCounts, CellMut, GridStorage};
|
||||
@@ -1,242 +0,0 @@
|
||||
//! Cached A* planner with pre-computed traversability.
|
||||
|
||||
use crate::core::{GridCoord, WorldPoint};
|
||||
use crate::grid::GridStorage;
|
||||
use crate::query::TraversabilityChecker;
|
||||
use log::{debug, trace};
|
||||
use std::collections::{BinaryHeap, HashMap, HashSet};
|
||||
|
||||
use super::types::{AStarConfig, AStarNode, PathFailure, PathResult};
|
||||
|
||||
/// A* planner with pre-computed traversability cache.
|
||||
///
|
||||
/// This is faster for multiple path queries on the same grid state,
|
||||
/// as it pre-computes traversability for all cells once.
|
||||
///
|
||||
/// # Example
|
||||
/// ```rust,ignore
|
||||
/// let cached = CachedAStarPlanner::new(storage, config);
|
||||
/// let path1 = cached.find_path(start1, goal1);
|
||||
/// let path2 = cached.find_path(start2, goal2); // Uses cached traversability
|
||||
/// ```
|
||||
pub struct CachedAStarPlanner<'a> {
|
||||
storage: &'a GridStorage,
|
||||
config: AStarConfig,
|
||||
/// Pre-computed traversability for each cell (true = safe to traverse)
|
||||
traversability_cache: Vec<bool>,
|
||||
}
|
||||
|
||||
impl<'a> CachedAStarPlanner<'a> {
|
||||
/// Create a new cached A* planner.
|
||||
///
|
||||
/// Pre-computes traversability for all cells based on the robot footprint.
|
||||
/// This has O(width × height × footprint_area) upfront cost but makes
|
||||
/// each pathfinding query O(1) for traversability checks.
|
||||
pub fn new(storage: &'a GridStorage, config: AStarConfig) -> Self {
|
||||
let checker = TraversabilityChecker::new(storage, config.footprint.clone());
|
||||
let width = storage.width();
|
||||
let height = storage.height();
|
||||
let exploration_mode = config.exploration_mode;
|
||||
|
||||
// Pre-compute traversability for all cells
|
||||
let mut cache = vec![false; width * height];
|
||||
for y in 0..height {
|
||||
for x in 0..width {
|
||||
let coord = GridCoord::new(x as i32, y as i32);
|
||||
let world = storage.grid_to_world(coord);
|
||||
cache[y * width + x] = checker.is_position_safe_mode(world, exploration_mode);
|
||||
}
|
||||
}
|
||||
|
||||
Self {
|
||||
storage,
|
||||
config,
|
||||
traversability_cache: cache,
|
||||
}
|
||||
}
|
||||
|
||||
/// Create with default configuration
|
||||
pub fn with_defaults(storage: &'a GridStorage) -> Self {
|
||||
Self::new(storage, AStarConfig::default())
|
||||
}
|
||||
|
||||
/// Check if a cell is traversable using the cache
|
||||
#[inline]
|
||||
fn is_traversable(&self, coord: GridCoord) -> bool {
|
||||
if !self.storage.is_valid_coord(coord) {
|
||||
return false;
|
||||
}
|
||||
let idx = coord.y as usize * self.storage.width() + coord.x as usize;
|
||||
self.traversability_cache[idx]
|
||||
}
|
||||
|
||||
/// Find a path from start to goal (grid coordinates)
|
||||
pub fn find_path(&self, start: GridCoord, goal: GridCoord) -> PathResult {
|
||||
trace!(
|
||||
"[CachedAStar] find_path: start=({},{}) goal=({},{})",
|
||||
start.x, start.y, goal.x, goal.y
|
||||
);
|
||||
|
||||
// Check bounds
|
||||
if !self.storage.is_valid_coord(start) || !self.storage.is_valid_coord(goal) {
|
||||
debug!("[CachedAStar] FAILED: OutOfBounds - start or goal outside grid");
|
||||
return PathResult::failed(PathFailure::OutOfBounds, 0);
|
||||
}
|
||||
|
||||
// Check start and goal traversability using cache
|
||||
if !self.is_traversable(start) {
|
||||
debug!("[CachedAStar] FAILED: StartBlocked");
|
||||
return PathResult::failed(PathFailure::StartBlocked, 0);
|
||||
}
|
||||
if !self.is_traversable(goal) {
|
||||
debug!("[CachedAStar] FAILED: GoalBlocked");
|
||||
return PathResult::failed(PathFailure::GoalBlocked, 0);
|
||||
}
|
||||
|
||||
// A* search
|
||||
let mut open_set = BinaryHeap::new();
|
||||
let mut closed_set = HashSet::new();
|
||||
let mut came_from: HashMap<GridCoord, GridCoord> = HashMap::new();
|
||||
let mut g_scores: HashMap<GridCoord, f32> = HashMap::new();
|
||||
|
||||
let h_start = self.heuristic(start, goal);
|
||||
open_set.push(AStarNode {
|
||||
coord: start,
|
||||
g_cost: 0.0,
|
||||
f_cost: h_start,
|
||||
});
|
||||
g_scores.insert(start, 0.0);
|
||||
|
||||
let mut nodes_expanded = 0;
|
||||
|
||||
while let Some(current) = open_set.pop() {
|
||||
nodes_expanded += 1;
|
||||
|
||||
if nodes_expanded > self.config.max_iterations {
|
||||
debug!(
|
||||
"[CachedAStar] FAILED: MaxIterationsExceeded ({} nodes)",
|
||||
nodes_expanded
|
||||
);
|
||||
return PathResult::failed(PathFailure::MaxIterationsExceeded, nodes_expanded);
|
||||
}
|
||||
|
||||
// Goal reached
|
||||
if current.coord == goal {
|
||||
return self.reconstruct_path(came_from, goal, current.g_cost, nodes_expanded);
|
||||
}
|
||||
|
||||
if closed_set.contains(¤t.coord) {
|
||||
continue;
|
||||
}
|
||||
closed_set.insert(current.coord);
|
||||
|
||||
// Explore neighbors
|
||||
let neighbors = if self.config.allow_diagonal {
|
||||
current.coord.neighbors_8().to_vec()
|
||||
} else {
|
||||
current.coord.neighbors_4().to_vec()
|
||||
};
|
||||
|
||||
for (i, neighbor) in neighbors.iter().enumerate() {
|
||||
if closed_set.contains(neighbor) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// Check traversability using cache (O(1) lookup)
|
||||
if !self.is_traversable(*neighbor) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// Calculate movement cost
|
||||
let is_diagonal = self.config.allow_diagonal && i >= 4;
|
||||
let move_cost = if is_diagonal {
|
||||
self.config.diagonal_cost
|
||||
} else {
|
||||
1.0
|
||||
};
|
||||
|
||||
let tentative_g = g_scores[¤t.coord] + move_cost;
|
||||
|
||||
let current_g = g_scores.get(neighbor).copied().unwrap_or(f32::INFINITY);
|
||||
if tentative_g < current_g {
|
||||
came_from.insert(*neighbor, current.coord);
|
||||
g_scores.insert(*neighbor, tentative_g);
|
||||
|
||||
let h = self.heuristic(*neighbor, goal);
|
||||
open_set.push(AStarNode {
|
||||
coord: *neighbor,
|
||||
g_cost: tentative_g,
|
||||
f_cost: tentative_g + h,
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
debug!(
|
||||
"[CachedAStar] FAILED: NoPath after expanding {} nodes",
|
||||
nodes_expanded
|
||||
);
|
||||
PathResult::failed(PathFailure::NoPath, nodes_expanded)
|
||||
}
|
||||
|
||||
/// Find a path from start to goal (world coordinates)
|
||||
pub fn find_path_world(&self, start: WorldPoint, goal: WorldPoint) -> PathResult {
|
||||
let start_grid = self.storage.world_to_grid(start);
|
||||
let goal_grid = self.storage.world_to_grid(goal);
|
||||
self.find_path(start_grid, goal_grid)
|
||||
}
|
||||
|
||||
/// Heuristic function (octile distance for 8-connected grid)
|
||||
fn heuristic(&self, from: GridCoord, to: GridCoord) -> f32 {
|
||||
let dx = (from.x - to.x).abs() as f32;
|
||||
let dy = (from.y - to.y).abs() as f32;
|
||||
|
||||
if self.config.allow_diagonal {
|
||||
let min = dx.min(dy);
|
||||
let max = dx.max(dy);
|
||||
min * self.config.diagonal_cost + (max - min)
|
||||
} else {
|
||||
dx + dy
|
||||
}
|
||||
}
|
||||
|
||||
/// Reconstruct the path from came_from map
|
||||
fn reconstruct_path(
|
||||
&self,
|
||||
came_from: HashMap<GridCoord, GridCoord>,
|
||||
goal: GridCoord,
|
||||
cost: f32,
|
||||
nodes_expanded: usize,
|
||||
) -> PathResult {
|
||||
let mut path_grid = Vec::new();
|
||||
let mut current = goal;
|
||||
|
||||
while let Some(&prev) = came_from.get(¤t) {
|
||||
path_grid.push(current);
|
||||
current = prev;
|
||||
}
|
||||
path_grid.push(current); // Add start
|
||||
path_grid.reverse();
|
||||
|
||||
let path_world: Vec<WorldPoint> = path_grid
|
||||
.iter()
|
||||
.map(|c| self.storage.grid_to_world(*c))
|
||||
.collect();
|
||||
|
||||
trace!(
|
||||
"[CachedAStar] SUCCESS: path length={} cells, cost={:.2}, nodes_expanded={}",
|
||||
path_grid.len(),
|
||||
cost,
|
||||
nodes_expanded
|
||||
);
|
||||
|
||||
PathResult {
|
||||
path_grid,
|
||||
path_world,
|
||||
cost,
|
||||
nodes_expanded,
|
||||
success: true,
|
||||
failure_reason: None,
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,235 +0,0 @@
|
||||
//! A* pathfinding algorithm.
|
||||
//!
|
||||
//! Implements A* search on the occupancy grid with support for:
|
||||
//! - Robot footprint collision checking
|
||||
//! - Safety margin around obstacles
|
||||
//! - 8-connected grid movement
|
||||
|
||||
mod cached;
|
||||
mod planner;
|
||||
mod types;
|
||||
|
||||
pub use cached::CachedAStarPlanner;
|
||||
pub use planner::AStarPlanner;
|
||||
pub use types::{AStarConfig, PathFailure, PathResult};
|
||||
|
||||
use crate::core::WorldPoint;
|
||||
use crate::grid::GridStorage;
|
||||
|
||||
/// Quick path finding with default configuration
|
||||
pub fn find_path(storage: &GridStorage, start: WorldPoint, goal: WorldPoint) -> PathResult {
|
||||
let planner = AStarPlanner::with_defaults(storage);
|
||||
planner.find_path_world(start, goal)
|
||||
}
|
||||
|
||||
/// Check if a path exists (faster than full path computation)
|
||||
pub fn path_exists(storage: &GridStorage, start: WorldPoint, goal: WorldPoint) -> bool {
|
||||
find_path(storage, start, goal).success
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use crate::core::{CellType, GridCoord};
|
||||
use crate::query::RobotFootprint;
|
||||
|
||||
fn create_test_storage() -> GridStorage {
|
||||
GridStorage::centered(50, 50, 0.1) // 5m x 5m at 10cm resolution
|
||||
}
|
||||
|
||||
fn fill_floor(storage: &mut GridStorage) {
|
||||
for x in 0..50 {
|
||||
for y in 0..50 {
|
||||
storage.set_type(GridCoord::new(x, y), CellType::Floor);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_simple_path() {
|
||||
let mut storage = create_test_storage();
|
||||
fill_floor(&mut storage);
|
||||
|
||||
// Small footprint for this test
|
||||
let config = AStarConfig::with_footprint(RobotFootprint::new(0.05, 0.01));
|
||||
let planner = AStarPlanner::new(&storage, config);
|
||||
|
||||
let start = GridCoord::new(10, 25);
|
||||
let goal = GridCoord::new(40, 25);
|
||||
|
||||
let result = planner.find_path(start, goal);
|
||||
|
||||
assert!(result.success);
|
||||
assert!(!result.path_grid.is_empty());
|
||||
assert_eq!(result.path_grid[0], start);
|
||||
assert_eq!(*result.path_grid.last().unwrap(), goal);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_path_around_obstacle() {
|
||||
let mut storage = create_test_storage();
|
||||
fill_floor(&mut storage);
|
||||
|
||||
// Add a wall across the middle
|
||||
for y in 15..35 {
|
||||
storage.set_type_with_priority(GridCoord::new(25, y), CellType::Wall);
|
||||
}
|
||||
|
||||
let config = AStarConfig::with_footprint(RobotFootprint::new(0.05, 0.01));
|
||||
let planner = AStarPlanner::new(&storage, config);
|
||||
|
||||
let start = GridCoord::new(10, 25);
|
||||
let goal = GridCoord::new(40, 25);
|
||||
|
||||
let result = planner.find_path(start, goal);
|
||||
|
||||
assert!(result.success);
|
||||
// Path should go around the wall
|
||||
assert!(result.path_grid.len() > 30);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_no_path() {
|
||||
let mut storage = create_test_storage();
|
||||
fill_floor(&mut storage);
|
||||
|
||||
// Add a complete wall barrier
|
||||
for y in 0..50 {
|
||||
storage.set_type_with_priority(GridCoord::new(25, y), CellType::Wall);
|
||||
}
|
||||
|
||||
let config = AStarConfig::with_footprint(RobotFootprint::new(0.05, 0.01));
|
||||
let planner = AStarPlanner::new(&storage, config);
|
||||
|
||||
let start = GridCoord::new(10, 25);
|
||||
let goal = GridCoord::new(40, 25);
|
||||
|
||||
let result = planner.find_path(start, goal);
|
||||
|
||||
assert!(!result.success);
|
||||
assert_eq!(result.failure_reason, Some(PathFailure::NoPath));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_start_blocked() {
|
||||
let mut storage = create_test_storage();
|
||||
fill_floor(&mut storage);
|
||||
|
||||
// Block start position
|
||||
storage.set_type_with_priority(GridCoord::new(10, 25), CellType::Wall);
|
||||
|
||||
let config = AStarConfig::with_footprint(RobotFootprint::new(0.05, 0.01));
|
||||
let planner = AStarPlanner::new(&storage, config);
|
||||
|
||||
let start = GridCoord::new(10, 25);
|
||||
let goal = GridCoord::new(40, 25);
|
||||
|
||||
let result = planner.find_path(start, goal);
|
||||
|
||||
assert!(!result.success);
|
||||
assert_eq!(result.failure_reason, Some(PathFailure::StartBlocked));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_goal_blocked() {
|
||||
let mut storage = create_test_storage();
|
||||
fill_floor(&mut storage);
|
||||
|
||||
// Block goal position
|
||||
storage.set_type_with_priority(GridCoord::new(40, 25), CellType::Wall);
|
||||
|
||||
let config = AStarConfig::with_footprint(RobotFootprint::new(0.05, 0.01));
|
||||
let planner = AStarPlanner::new(&storage, config);
|
||||
|
||||
let start = GridCoord::new(10, 25);
|
||||
let goal = GridCoord::new(40, 25);
|
||||
|
||||
let result = planner.find_path(start, goal);
|
||||
|
||||
assert!(!result.success);
|
||||
assert_eq!(result.failure_reason, Some(PathFailure::GoalBlocked));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_diagonal_path() {
|
||||
let mut storage = create_test_storage();
|
||||
fill_floor(&mut storage);
|
||||
|
||||
let config = AStarConfig {
|
||||
footprint: RobotFootprint::new(0.05, 0.01),
|
||||
allow_diagonal: true,
|
||||
..Default::default()
|
||||
};
|
||||
let planner = AStarPlanner::new(&storage, config);
|
||||
|
||||
let start = GridCoord::new(10, 10);
|
||||
let goal = GridCoord::new(40, 40);
|
||||
|
||||
let result = planner.find_path(start, goal);
|
||||
|
||||
assert!(result.success);
|
||||
// Diagonal path should be shorter than manhattan
|
||||
// For a 30x30 diagonal, manhattan would be 60, octile should be ~42
|
||||
assert!(result.path_grid.len() < 50);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_4_connected_path() {
|
||||
let mut storage = create_test_storage();
|
||||
fill_floor(&mut storage);
|
||||
|
||||
let config = AStarConfig {
|
||||
footprint: RobotFootprint::new(0.05, 0.01),
|
||||
allow_diagonal: false,
|
||||
..Default::default()
|
||||
};
|
||||
let planner = AStarPlanner::new(&storage, config);
|
||||
|
||||
let start = GridCoord::new(10, 10);
|
||||
let goal = GridCoord::new(40, 40);
|
||||
|
||||
let result = planner.find_path(start, goal);
|
||||
|
||||
assert!(result.success);
|
||||
// 4-connected path should follow manhattan distance
|
||||
// Path length should be approximately dx + dy = 60
|
||||
assert!(result.path_grid.len() >= 60);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_world_coordinates() {
|
||||
let mut storage = create_test_storage();
|
||||
fill_floor(&mut storage);
|
||||
|
||||
let config = AStarConfig::with_footprint(RobotFootprint::new(0.05, 0.01));
|
||||
let planner = AStarPlanner::new(&storage, config);
|
||||
|
||||
let start = WorldPoint::new(-1.0, 0.0);
|
||||
let goal = WorldPoint::new(1.0, 0.0);
|
||||
|
||||
let result = planner.find_path_world(start, goal);
|
||||
|
||||
assert!(result.success);
|
||||
assert!(result.path_world.len() > 0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_path_length_meters() {
|
||||
let mut storage = create_test_storage();
|
||||
fill_floor(&mut storage);
|
||||
|
||||
let config = AStarConfig::with_footprint(RobotFootprint::new(0.05, 0.01));
|
||||
let planner = AStarPlanner::new(&storage, config);
|
||||
|
||||
// Path of 2 meters (20 cells at 10cm resolution)
|
||||
let start = WorldPoint::new(-1.0, 0.0);
|
||||
let goal = WorldPoint::new(1.0, 0.0);
|
||||
|
||||
let result = planner.find_path_world(start, goal);
|
||||
|
||||
assert!(result.success);
|
||||
// Path length should be approximately 2.0 meters
|
||||
let length = result.length_meters();
|
||||
assert!(length > 1.5 && length < 2.5);
|
||||
}
|
||||
}
|
||||
@@ -1,225 +0,0 @@
|
||||
//! A* planner implementation.
|
||||
|
||||
use crate::core::{GridCoord, WorldPoint};
|
||||
use crate::grid::GridStorage;
|
||||
use crate::query::TraversabilityChecker;
|
||||
use log::{debug, trace};
|
||||
use std::collections::{BinaryHeap, HashMap, HashSet};
|
||||
|
||||
use super::types::{AStarConfig, AStarNode, PathFailure, PathResult};
|
||||
|
||||
/// A* pathfinder
|
||||
pub struct AStarPlanner<'a> {
|
||||
storage: &'a GridStorage,
|
||||
config: AStarConfig,
|
||||
}
|
||||
|
||||
impl<'a> AStarPlanner<'a> {
|
||||
/// Create a new A* planner
|
||||
pub fn new(storage: &'a GridStorage, config: AStarConfig) -> Self {
|
||||
Self { storage, config }
|
||||
}
|
||||
|
||||
/// Create with default configuration
|
||||
pub fn with_defaults(storage: &'a GridStorage) -> Self {
|
||||
Self::new(storage, AStarConfig::default())
|
||||
}
|
||||
|
||||
/// Find a path from start to goal (grid coordinates)
|
||||
pub fn find_path(&self, start: GridCoord, goal: GridCoord) -> PathResult {
|
||||
trace!(
|
||||
"[AStar] find_path: start=({},{}) goal=({},{})",
|
||||
start.x, start.y, goal.x, goal.y
|
||||
);
|
||||
|
||||
// Check bounds
|
||||
if !self.storage.is_valid_coord(start) || !self.storage.is_valid_coord(goal) {
|
||||
debug!("[AStar] FAILED: OutOfBounds - start or goal outside grid");
|
||||
return PathResult::failed(PathFailure::OutOfBounds, 0);
|
||||
}
|
||||
|
||||
// Check start and goal traversability using footprint
|
||||
let checker = TraversabilityChecker::new(self.storage, self.config.footprint.clone());
|
||||
|
||||
let start_world = self.storage.grid_to_world(start);
|
||||
let goal_world = self.storage.grid_to_world(goal);
|
||||
|
||||
if !checker.is_position_safe_mode(start_world, self.config.exploration_mode) {
|
||||
debug!(
|
||||
"[AStar] FAILED: StartBlocked at ({:.2},{:.2}), exploration_mode={}",
|
||||
start_world.x, start_world.y, self.config.exploration_mode
|
||||
);
|
||||
return PathResult::failed(PathFailure::StartBlocked, 0);
|
||||
}
|
||||
if !checker.is_position_safe_mode(goal_world, self.config.exploration_mode) {
|
||||
debug!(
|
||||
"[AStar] FAILED: GoalBlocked at ({:.2},{:.2}), exploration_mode={}",
|
||||
goal_world.x, goal_world.y, self.config.exploration_mode
|
||||
);
|
||||
return PathResult::failed(PathFailure::GoalBlocked, 0);
|
||||
}
|
||||
|
||||
// A* search
|
||||
let mut open_set = BinaryHeap::new();
|
||||
let mut closed_set = HashSet::new();
|
||||
let mut came_from: HashMap<GridCoord, GridCoord> = HashMap::new();
|
||||
let mut g_scores: HashMap<GridCoord, f32> = HashMap::new();
|
||||
|
||||
let h_start = self.heuristic(start, goal);
|
||||
open_set.push(AStarNode {
|
||||
coord: start,
|
||||
g_cost: 0.0,
|
||||
f_cost: h_start,
|
||||
});
|
||||
g_scores.insert(start, 0.0);
|
||||
|
||||
let mut nodes_expanded = 0;
|
||||
|
||||
while let Some(current) = open_set.pop() {
|
||||
nodes_expanded += 1;
|
||||
|
||||
if nodes_expanded > self.config.max_iterations {
|
||||
debug!(
|
||||
"[AStar] FAILED: MaxIterationsExceeded ({} nodes)",
|
||||
nodes_expanded
|
||||
);
|
||||
return PathResult::failed(PathFailure::MaxIterationsExceeded, nodes_expanded);
|
||||
}
|
||||
|
||||
// Goal reached
|
||||
if current.coord == goal {
|
||||
return self.reconstruct_path(came_from, goal, current.g_cost, nodes_expanded);
|
||||
}
|
||||
|
||||
if closed_set.contains(¤t.coord) {
|
||||
continue;
|
||||
}
|
||||
closed_set.insert(current.coord);
|
||||
|
||||
// Explore neighbors
|
||||
let neighbors = if self.config.allow_diagonal {
|
||||
current.coord.neighbors_8().to_vec()
|
||||
} else {
|
||||
current.coord.neighbors_4().to_vec()
|
||||
};
|
||||
|
||||
for (i, neighbor) in neighbors.iter().enumerate() {
|
||||
if closed_set.contains(neighbor) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if !self.storage.is_valid_coord(*neighbor) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// Check traversability with footprint
|
||||
let neighbor_world = self.storage.grid_to_world(*neighbor);
|
||||
if !checker.is_position_safe_mode(neighbor_world, self.config.exploration_mode) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// Calculate movement cost
|
||||
let is_diagonal = self.config.allow_diagonal && i >= 4;
|
||||
let move_cost = if is_diagonal {
|
||||
self.config.diagonal_cost
|
||||
} else {
|
||||
1.0
|
||||
};
|
||||
|
||||
// Add clearance cost if enabled
|
||||
let clearance_cost = if self.config.clearance_weight > 0.0 {
|
||||
let clearance = checker.clearance(neighbor_world, 1.0);
|
||||
// Penalize positions close to obstacles
|
||||
self.config.clearance_weight / (clearance + 0.1)
|
||||
} else {
|
||||
0.0
|
||||
};
|
||||
|
||||
let tentative_g = g_scores[¤t.coord] + move_cost + clearance_cost;
|
||||
|
||||
let current_g = g_scores.get(neighbor).copied().unwrap_or(f32::INFINITY);
|
||||
if tentative_g < current_g {
|
||||
came_from.insert(*neighbor, current.coord);
|
||||
g_scores.insert(*neighbor, tentative_g);
|
||||
|
||||
let h = self.heuristic(*neighbor, goal);
|
||||
open_set.push(AStarNode {
|
||||
coord: *neighbor,
|
||||
g_cost: tentative_g,
|
||||
f_cost: tentative_g + h,
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
debug!(
|
||||
"[AStar] FAILED: NoPath after expanding {} nodes",
|
||||
nodes_expanded
|
||||
);
|
||||
PathResult::failed(PathFailure::NoPath, nodes_expanded)
|
||||
}
|
||||
|
||||
/// Find a path from start to goal (world coordinates)
|
||||
pub fn find_path_world(&self, start: WorldPoint, goal: WorldPoint) -> PathResult {
|
||||
let start_grid = self.storage.world_to_grid(start);
|
||||
let goal_grid = self.storage.world_to_grid(goal);
|
||||
self.find_path(start_grid, goal_grid)
|
||||
}
|
||||
|
||||
/// Heuristic function (octile distance for 8-connected grid)
|
||||
fn heuristic(&self, from: GridCoord, to: GridCoord) -> f32 {
|
||||
let dx = (from.x - to.x).abs() as f32;
|
||||
let dy = (from.y - to.y).abs() as f32;
|
||||
|
||||
if self.config.allow_diagonal {
|
||||
// Octile distance
|
||||
let min = dx.min(dy);
|
||||
let max = dx.max(dy);
|
||||
min * self.config.diagonal_cost + (max - min)
|
||||
} else {
|
||||
// Manhattan distance
|
||||
dx + dy
|
||||
}
|
||||
}
|
||||
|
||||
/// Reconstruct the path from came_from map
|
||||
fn reconstruct_path(
|
||||
&self,
|
||||
came_from: HashMap<GridCoord, GridCoord>,
|
||||
goal: GridCoord,
|
||||
cost: f32,
|
||||
nodes_expanded: usize,
|
||||
) -> PathResult {
|
||||
let mut path_grid = Vec::new();
|
||||
let mut current = goal;
|
||||
|
||||
while let Some(&prev) = came_from.get(¤t) {
|
||||
path_grid.push(current);
|
||||
current = prev;
|
||||
}
|
||||
path_grid.push(current); // Add start
|
||||
path_grid.reverse();
|
||||
|
||||
// Convert to world coordinates
|
||||
let path_world: Vec<WorldPoint> = path_grid
|
||||
.iter()
|
||||
.map(|c| self.storage.grid_to_world(*c))
|
||||
.collect();
|
||||
|
||||
trace!(
|
||||
"[AStar] SUCCESS: path length={} cells, cost={:.2}, nodes_expanded={}",
|
||||
path_grid.len(),
|
||||
cost,
|
||||
nodes_expanded
|
||||
);
|
||||
|
||||
PathResult {
|
||||
path_grid,
|
||||
path_world,
|
||||
cost,
|
||||
nodes_expanded,
|
||||
success: true,
|
||||
failure_reason: None,
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,148 +0,0 @@
|
||||
//! A* pathfinding types.
|
||||
|
||||
use crate::core::{GridCoord, WorldPoint};
|
||||
use crate::query::RobotFootprint;
|
||||
use std::cmp::Ordering;
|
||||
|
||||
/// A node in the A* search
|
||||
#[derive(Clone, Debug)]
|
||||
pub(super) struct AStarNode {
|
||||
pub coord: GridCoord,
|
||||
pub g_cost: f32, // Cost from start
|
||||
pub f_cost: f32, // g_cost + heuristic
|
||||
}
|
||||
|
||||
impl Eq for AStarNode {}
|
||||
|
||||
impl PartialEq for AStarNode {
|
||||
fn eq(&self, other: &Self) -> bool {
|
||||
self.coord == other.coord
|
||||
}
|
||||
}
|
||||
|
||||
impl Ord for AStarNode {
|
||||
fn cmp(&self, other: &Self) -> Ordering {
|
||||
// Reverse ordering for min-heap behavior
|
||||
other
|
||||
.f_cost
|
||||
.partial_cmp(&self.f_cost)
|
||||
.unwrap_or(Ordering::Equal)
|
||||
}
|
||||
}
|
||||
|
||||
impl PartialOrd for AStarNode {
|
||||
fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
|
||||
Some(self.cmp(other))
|
||||
}
|
||||
}
|
||||
|
||||
/// A* pathfinding configuration
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct AStarConfig {
|
||||
/// Robot footprint for collision checking
|
||||
pub footprint: RobotFootprint,
|
||||
/// Allow diagonal movement (8-connected vs 4-connected)
|
||||
pub allow_diagonal: bool,
|
||||
/// Diagonal movement cost multiplier (sqrt(2) ≈ 1.414)
|
||||
pub diagonal_cost: f32,
|
||||
/// Maximum number of nodes to expand before giving up
|
||||
pub max_iterations: usize,
|
||||
/// Prefer paths with more clearance from obstacles
|
||||
pub clearance_weight: f32,
|
||||
/// Exploration mode: allow Unknown cells in footprint
|
||||
/// (assumes unknown areas are likely free)
|
||||
pub exploration_mode: bool,
|
||||
}
|
||||
|
||||
impl Default for AStarConfig {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
footprint: RobotFootprint::default(),
|
||||
allow_diagonal: true,
|
||||
diagonal_cost: std::f32::consts::SQRT_2,
|
||||
max_iterations: 100_000,
|
||||
clearance_weight: 0.0, // Disabled by default for performance
|
||||
exploration_mode: false,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl AStarConfig {
|
||||
/// Create with custom robot footprint
|
||||
pub fn with_footprint(footprint: RobotFootprint) -> Self {
|
||||
Self {
|
||||
footprint,
|
||||
..Default::default()
|
||||
}
|
||||
}
|
||||
|
||||
/// Enable clearance-weighted pathfinding
|
||||
pub fn with_clearance_weight(mut self, weight: f32) -> Self {
|
||||
self.clearance_weight = weight;
|
||||
self
|
||||
}
|
||||
}
|
||||
|
||||
/// Result of A* pathfinding
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct PathResult {
|
||||
/// Path as grid coordinates (empty if no path found)
|
||||
pub path_grid: Vec<GridCoord>,
|
||||
/// Path as world coordinates
|
||||
pub path_world: Vec<WorldPoint>,
|
||||
/// Total path cost
|
||||
pub cost: f32,
|
||||
/// Number of nodes expanded during search
|
||||
pub nodes_expanded: usize,
|
||||
/// Whether a path was found
|
||||
pub success: bool,
|
||||
/// Reason for failure (if any)
|
||||
pub failure_reason: Option<PathFailure>,
|
||||
}
|
||||
|
||||
impl PathResult {
|
||||
/// Create a failed result
|
||||
pub(super) fn failed(reason: PathFailure, nodes_expanded: usize) -> Self {
|
||||
Self {
|
||||
path_grid: Vec::new(),
|
||||
path_world: Vec::new(),
|
||||
cost: f32::INFINITY,
|
||||
nodes_expanded,
|
||||
success: false,
|
||||
failure_reason: Some(reason),
|
||||
}
|
||||
}
|
||||
|
||||
/// Path length in cells
|
||||
pub fn length_cells(&self) -> usize {
|
||||
self.path_grid.len()
|
||||
}
|
||||
|
||||
/// Path length in meters (approximate)
|
||||
pub fn length_meters(&self) -> f32 {
|
||||
if self.path_world.len() < 2 {
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
let mut total = 0.0;
|
||||
for i in 1..self.path_world.len() {
|
||||
total += self.path_world[i - 1].distance(&self.path_world[i]);
|
||||
}
|
||||
total
|
||||
}
|
||||
}
|
||||
|
||||
/// Reason for path failure
|
||||
#[derive(Clone, Debug, PartialEq, Eq)]
|
||||
pub enum PathFailure {
|
||||
/// Start position is not traversable
|
||||
StartBlocked,
|
||||
/// Goal position is not traversable
|
||||
GoalBlocked,
|
||||
/// No path exists between start and goal
|
||||
NoPath,
|
||||
/// Maximum iterations exceeded
|
||||
MaxIterationsExceeded,
|
||||
/// Start or goal is out of bounds
|
||||
OutOfBounds,
|
||||
}
|
||||
@@ -1,38 +0,0 @@
|
||||
//! Path planning algorithms.
|
||||
//!
|
||||
//! This module provides path planning on the occupancy grid:
|
||||
//!
|
||||
//! - **A* Search**: Find shortest collision-free paths
|
||||
//! - **Path Smoothing**: Make paths more natural for robot motion
|
||||
//!
|
||||
//! ## A* Pathfinding
|
||||
//!
|
||||
//! ```rust,ignore
|
||||
//! use vastu_map::pathfinding::{AStarPlanner, AStarConfig};
|
||||
//!
|
||||
//! let config = AStarConfig::default();
|
||||
//! let planner = AStarPlanner::new(map.storage(), config);
|
||||
//!
|
||||
//! let result = planner.find_path_world(start, goal);
|
||||
//! if result.success {
|
||||
//! println!("Path found with {} waypoints", result.path_world.len());
|
||||
//! }
|
||||
//! ```
|
||||
//!
|
||||
//! ## Path Smoothing
|
||||
//!
|
||||
//! ```rust,ignore
|
||||
//! use vastu_map::pathfinding::{PathSmoother, SmoothingConfig};
|
||||
//!
|
||||
//! let smoother = PathSmoother::with_defaults(map.storage());
|
||||
//! let smooth_path = smoother.smooth(&result.path_world);
|
||||
//! ```
|
||||
|
||||
pub mod astar;
|
||||
pub mod smoothing;
|
||||
|
||||
pub use astar::{
|
||||
AStarConfig, AStarPlanner, CachedAStarPlanner, PathFailure, PathResult, find_path, path_exists,
|
||||
};
|
||||
|
||||
pub use smoothing::{PathSmoother, SmoothingConfig, path_length, resample_path, simplify_path};
|
||||
@@ -1,400 +0,0 @@
|
||||
//! Path smoothing algorithms.
|
||||
//!
|
||||
//! Smooths A* paths for more natural robot movement:
|
||||
//! - Line-of-sight shortcutting
|
||||
//! - Bezier curve smoothing
|
||||
//! - Waypoint reduction
|
||||
|
||||
use crate::core::{GridCoord, WorldPoint};
|
||||
use crate::grid::GridStorage;
|
||||
use crate::query::{RobotFootprint, TraversabilityChecker};
|
||||
|
||||
/// Path smoothing configuration
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct SmoothingConfig {
|
||||
/// Robot footprint for collision checking
|
||||
pub footprint: RobotFootprint,
|
||||
/// Step size for line-of-sight checks (in meters)
|
||||
pub los_step_size: f32,
|
||||
/// Maximum iterations for iterative smoothing
|
||||
pub max_iterations: usize,
|
||||
}
|
||||
|
||||
impl Default for SmoothingConfig {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
footprint: RobotFootprint::default(),
|
||||
los_step_size: 0.05, // 5cm steps
|
||||
max_iterations: 100,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Path smoother
|
||||
pub struct PathSmoother<'a> {
|
||||
storage: &'a GridStorage,
|
||||
config: SmoothingConfig,
|
||||
}
|
||||
|
||||
impl<'a> PathSmoother<'a> {
|
||||
/// Create a new path smoother
|
||||
pub fn new(storage: &'a GridStorage, config: SmoothingConfig) -> Self {
|
||||
Self { storage, config }
|
||||
}
|
||||
|
||||
/// Create with default configuration
|
||||
pub fn with_defaults(storage: &'a GridStorage) -> Self {
|
||||
Self::new(storage, SmoothingConfig::default())
|
||||
}
|
||||
|
||||
/// Smooth a path using line-of-sight shortcuts
|
||||
///
|
||||
/// Removes unnecessary waypoints by checking if direct paths exist
|
||||
/// between non-adjacent waypoints.
|
||||
pub fn smooth_los(&self, path: &[WorldPoint]) -> Vec<WorldPoint> {
|
||||
if path.len() <= 2 {
|
||||
return path.to_vec();
|
||||
}
|
||||
|
||||
let checker = TraversabilityChecker::new(self.storage, self.config.footprint.clone());
|
||||
let mut smoothed = vec![path[0]];
|
||||
let mut i = 0;
|
||||
|
||||
while i < path.len() - 1 {
|
||||
// Find the furthest reachable point from current
|
||||
let mut furthest = i + 1;
|
||||
|
||||
for j in (i + 2)..path.len() {
|
||||
let check_result =
|
||||
checker.check_path(path[i], path[j], Some(self.config.los_step_size));
|
||||
|
||||
if check_result.is_clear {
|
||||
furthest = j;
|
||||
}
|
||||
}
|
||||
|
||||
smoothed.push(path[furthest]);
|
||||
i = furthest;
|
||||
}
|
||||
|
||||
smoothed
|
||||
}
|
||||
|
||||
/// Smooth using iterative midpoint displacement
|
||||
///
|
||||
/// Moves each waypoint towards the line connecting its neighbors
|
||||
/// while maintaining collision-free path.
|
||||
pub fn smooth_midpoint(&self, path: &[WorldPoint]) -> Vec<WorldPoint> {
|
||||
if path.len() <= 2 {
|
||||
return path.to_vec();
|
||||
}
|
||||
|
||||
let checker = TraversabilityChecker::new(self.storage, self.config.footprint.clone());
|
||||
let mut smoothed = path.to_vec();
|
||||
let alpha = 0.5; // Smoothing factor
|
||||
|
||||
for _ in 0..self.config.max_iterations {
|
||||
let mut changed = false;
|
||||
let mut new_path = smoothed.clone();
|
||||
|
||||
// Skip first and last points
|
||||
for i in 1..smoothed.len() - 1 {
|
||||
let prev = smoothed[i - 1];
|
||||
let curr = smoothed[i];
|
||||
let next = smoothed[i + 1];
|
||||
|
||||
// Target is midpoint of prev-next line
|
||||
let target = WorldPoint::new((prev.x + next.x) / 2.0, (prev.y + next.y) / 2.0);
|
||||
|
||||
// Move towards target
|
||||
let new_point = WorldPoint::new(
|
||||
curr.x + alpha * (target.x - curr.x),
|
||||
curr.y + alpha * (target.y - curr.y),
|
||||
);
|
||||
|
||||
// Check if new position is safe
|
||||
if checker.is_position_safe(new_point) {
|
||||
// Check paths to neighbors
|
||||
let to_prev =
|
||||
checker.check_path(new_point, prev, Some(self.config.los_step_size));
|
||||
let to_next =
|
||||
checker.check_path(new_point, next, Some(self.config.los_step_size));
|
||||
|
||||
if to_prev.is_clear
|
||||
&& to_next.is_clear
|
||||
&& ((new_point.x - curr.x).abs() > 0.001
|
||||
|| (new_point.y - curr.y).abs() > 0.001)
|
||||
{
|
||||
new_path[i] = new_point;
|
||||
changed = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
smoothed = new_path;
|
||||
if !changed {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
smoothed
|
||||
}
|
||||
|
||||
/// Full smoothing: LOS shortcuts + midpoint smoothing
|
||||
pub fn smooth(&self, path: &[WorldPoint]) -> Vec<WorldPoint> {
|
||||
let los_smoothed = self.smooth_los(path);
|
||||
self.smooth_midpoint(&los_smoothed)
|
||||
}
|
||||
|
||||
/// Convert grid path to world path with smoothing
|
||||
pub fn smooth_grid_path(&self, path: &[GridCoord]) -> Vec<WorldPoint> {
|
||||
let world_path: Vec<WorldPoint> = path
|
||||
.iter()
|
||||
.map(|c| self.storage.grid_to_world(*c))
|
||||
.collect();
|
||||
|
||||
self.smooth(&world_path)
|
||||
}
|
||||
}
|
||||
|
||||
/// Simplify a path by removing collinear points
|
||||
pub fn simplify_path(path: &[WorldPoint], tolerance: f32) -> Vec<WorldPoint> {
|
||||
if path.len() <= 2 {
|
||||
return path.to_vec();
|
||||
}
|
||||
|
||||
let mut result = vec![path[0]];
|
||||
|
||||
for i in 1..path.len() - 1 {
|
||||
let prev = result.last().unwrap();
|
||||
let curr = &path[i];
|
||||
let next = &path[i + 1];
|
||||
|
||||
// Check if current point is far enough from the line prev->next
|
||||
let distance = point_to_line_distance(curr, prev, next);
|
||||
if distance > tolerance {
|
||||
result.push(*curr);
|
||||
}
|
||||
}
|
||||
|
||||
result.push(*path.last().unwrap());
|
||||
result
|
||||
}
|
||||
|
||||
/// Calculate perpendicular distance from point to line
|
||||
fn point_to_line_distance(
|
||||
point: &WorldPoint,
|
||||
line_start: &WorldPoint,
|
||||
line_end: &WorldPoint,
|
||||
) -> f32 {
|
||||
let dx = line_end.x - line_start.x;
|
||||
let dy = line_end.y - line_start.y;
|
||||
let line_length_sq = dx * dx + dy * dy;
|
||||
|
||||
if line_length_sq < 0.0001 {
|
||||
// Line is a point
|
||||
return point.distance(line_start);
|
||||
}
|
||||
|
||||
// Project point onto line
|
||||
let t = ((point.x - line_start.x) * dx + (point.y - line_start.y) * dy) / line_length_sq;
|
||||
let t_clamped = t.clamp(0.0, 1.0);
|
||||
|
||||
let proj_x = line_start.x + t_clamped * dx;
|
||||
let proj_y = line_start.y + t_clamped * dy;
|
||||
|
||||
let distance_x = point.x - proj_x;
|
||||
let distance_y = point.y - proj_y;
|
||||
|
||||
(distance_x * distance_x + distance_y * distance_y).sqrt()
|
||||
}
|
||||
|
||||
/// Calculate total path length
|
||||
pub fn path_length(path: &[WorldPoint]) -> f32 {
|
||||
if path.len() < 2 {
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
path.windows(2).map(|w| w[0].distance(&w[1])).sum()
|
||||
}
|
||||
|
||||
/// Interpolate along path to get evenly spaced waypoints
|
||||
pub fn resample_path(path: &[WorldPoint], spacing: f32) -> Vec<WorldPoint> {
|
||||
if path.is_empty() {
|
||||
return Vec::new();
|
||||
}
|
||||
if path.len() == 1 {
|
||||
return path.to_vec();
|
||||
}
|
||||
|
||||
let total_length = path_length(path);
|
||||
if total_length < spacing {
|
||||
return vec![path[0], *path.last().unwrap()];
|
||||
}
|
||||
|
||||
let mut result = vec![path[0]];
|
||||
let mut accumulated = 0.0;
|
||||
let mut segment_idx = 0;
|
||||
let mut segment_progress = 0.0;
|
||||
|
||||
while accumulated < total_length - spacing * 0.5 {
|
||||
accumulated += spacing;
|
||||
|
||||
// Find the segment containing this distance
|
||||
let mut distance_along = 0.0;
|
||||
while segment_idx < path.len() - 1 {
|
||||
let segment_length = path[segment_idx].distance(&path[segment_idx + 1]);
|
||||
|
||||
if distance_along + segment_length >= accumulated {
|
||||
// Point is in this segment
|
||||
segment_progress = (accumulated - distance_along) / segment_length;
|
||||
break;
|
||||
}
|
||||
|
||||
distance_along += segment_length;
|
||||
segment_idx += 1;
|
||||
}
|
||||
|
||||
if segment_idx < path.len() - 1 {
|
||||
let p1 = &path[segment_idx];
|
||||
let p2 = &path[segment_idx + 1];
|
||||
let point = WorldPoint::new(
|
||||
p1.x + segment_progress * (p2.x - p1.x),
|
||||
p1.y + segment_progress * (p2.y - p1.y),
|
||||
);
|
||||
result.push(point);
|
||||
}
|
||||
}
|
||||
|
||||
// Always include the end point
|
||||
result.push(*path.last().unwrap());
|
||||
result
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use crate::core::CellType;
|
||||
|
||||
fn create_test_storage() -> GridStorage {
|
||||
GridStorage::centered(50, 50, 0.1) // 5m x 5m at 10cm resolution
|
||||
}
|
||||
|
||||
fn fill_floor(storage: &mut GridStorage) {
|
||||
for x in 0..50 {
|
||||
for y in 0..50 {
|
||||
storage.set_type(GridCoord::new(x, y), CellType::Floor);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_los_smoothing_straight_line() {
|
||||
let mut storage = create_test_storage();
|
||||
fill_floor(&mut storage);
|
||||
|
||||
let config = SmoothingConfig {
|
||||
footprint: RobotFootprint::new(0.05, 0.01),
|
||||
..Default::default()
|
||||
};
|
||||
let smoother = PathSmoother::new(&storage, config);
|
||||
|
||||
// Jagged path that should become straight
|
||||
let path = vec![
|
||||
WorldPoint::new(0.0, 0.0),
|
||||
WorldPoint::new(0.1, 0.01),
|
||||
WorldPoint::new(0.2, -0.01),
|
||||
WorldPoint::new(0.3, 0.02),
|
||||
WorldPoint::new(0.4, 0.0),
|
||||
WorldPoint::new(0.5, 0.0),
|
||||
];
|
||||
|
||||
let smoothed = smoother.smooth_los(&path);
|
||||
|
||||
// Should reduce to just start and end
|
||||
assert!(smoothed.len() <= 3);
|
||||
assert_eq!(smoothed[0], path[0]);
|
||||
assert_eq!(*smoothed.last().unwrap(), *path.last().unwrap());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_los_preserves_corners() {
|
||||
let mut storage = create_test_storage();
|
||||
fill_floor(&mut storage);
|
||||
|
||||
// Add wall
|
||||
for y in -10..10 {
|
||||
storage.set_type_with_priority(
|
||||
storage.world_to_grid(WorldPoint::new(0.5, y as f32 * 0.1)),
|
||||
CellType::Wall,
|
||||
);
|
||||
}
|
||||
|
||||
let config = SmoothingConfig {
|
||||
footprint: RobotFootprint::new(0.05, 0.01),
|
||||
..Default::default()
|
||||
};
|
||||
let smoother = PathSmoother::new(&storage, config);
|
||||
|
||||
// Path that goes around the wall
|
||||
let path = vec![
|
||||
WorldPoint::new(0.0, 0.0),
|
||||
WorldPoint::new(0.3, 0.0),
|
||||
WorldPoint::new(0.3, 1.2), // Go up
|
||||
WorldPoint::new(0.7, 1.2), // Go right (around wall)
|
||||
WorldPoint::new(0.7, 0.0), // Go down
|
||||
WorldPoint::new(1.0, 0.0),
|
||||
];
|
||||
|
||||
let smoothed = smoother.smooth_los(&path);
|
||||
|
||||
// Should preserve some waypoints to go around wall
|
||||
assert!(smoothed.len() >= 3);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_simplify_path() {
|
||||
// Path with a clear corner (not collinear)
|
||||
let path = vec![
|
||||
WorldPoint::new(0.0, 0.0),
|
||||
WorldPoint::new(0.1, 0.001), // Nearly collinear with start->end of segment 1
|
||||
WorldPoint::new(0.2, 0.0), // End of first segment
|
||||
WorldPoint::new(0.3, 0.1), // True corner - offset from line
|
||||
WorldPoint::new(0.3, 0.2),
|
||||
];
|
||||
|
||||
let simplified = simplify_path(&path, 0.01);
|
||||
|
||||
// Should have fewer points than original
|
||||
// start, corner at (0.2, 0), corner at (0.3, 0.1), end
|
||||
assert!(simplified.len() <= path.len());
|
||||
|
||||
// First and last points should always be preserved
|
||||
assert_eq!(simplified[0], path[0]);
|
||||
assert_eq!(*simplified.last().unwrap(), *path.last().unwrap());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_path_length() {
|
||||
let path = vec![
|
||||
WorldPoint::new(0.0, 0.0),
|
||||
WorldPoint::new(1.0, 0.0),
|
||||
WorldPoint::new(1.0, 1.0),
|
||||
];
|
||||
|
||||
let length = path_length(&path);
|
||||
assert!((length - 2.0).abs() < 0.01);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_resample_path() {
|
||||
let path = vec![WorldPoint::new(0.0, 0.0), WorldPoint::new(1.0, 0.0)];
|
||||
|
||||
let resampled = resample_path(&path, 0.25);
|
||||
|
||||
// Should have points at 0, 0.25, 0.5, 0.75, 1.0
|
||||
assert!(resampled.len() >= 4);
|
||||
assert_eq!(resampled[0], path[0]);
|
||||
assert_eq!(*resampled.last().unwrap(), *path.last().unwrap());
|
||||
}
|
||||
}
|
||||
@@ -1,781 +0,0 @@
|
||||
//! Frontier detection for exploration.
|
||||
//!
|
||||
//! A frontier is a boundary between known (Floor) and unknown cells.
|
||||
//! This module detects frontiers and clusters them for exploration targeting.
|
||||
|
||||
use crate::core::{CellType, GridCoord, WorldPoint};
|
||||
use crate::grid::GridStorage;
|
||||
use log::{debug, trace};
|
||||
use std::collections::{HashSet, VecDeque};
|
||||
use std::simd::{cmp::SimdPartialEq, u8x16};
|
||||
|
||||
/// A single frontier cell
|
||||
#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)]
|
||||
pub struct FrontierCell {
|
||||
/// Grid coordinate of the frontier cell
|
||||
pub coord: GridCoord,
|
||||
/// Number of adjacent unknown cells (1-8)
|
||||
pub unknown_neighbors: u8,
|
||||
}
|
||||
|
||||
impl FrontierCell {
|
||||
/// Create a new frontier cell
|
||||
pub fn new(coord: GridCoord, unknown_neighbors: u8) -> Self {
|
||||
Self {
|
||||
coord,
|
||||
unknown_neighbors,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// A cluster of connected frontier cells
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct Frontier {
|
||||
/// All cells in this frontier
|
||||
pub cells: Vec<FrontierCell>,
|
||||
/// Centroid in grid coordinates
|
||||
pub centroid_grid: GridCoord,
|
||||
/// Centroid in world coordinates
|
||||
pub centroid_world: WorldPoint,
|
||||
/// Approximate size (number of cells)
|
||||
pub size: usize,
|
||||
/// Sum of unknown neighbors (exploration potential)
|
||||
pub unknown_exposure: usize,
|
||||
}
|
||||
|
||||
impl Frontier {
|
||||
/// Create a new frontier from a set of cells
|
||||
pub fn from_cells(cells: Vec<FrontierCell>, storage: &GridStorage) -> Self {
|
||||
let size = cells.len();
|
||||
|
||||
// Calculate centroid
|
||||
let sum_x: i32 = cells.iter().map(|c| c.coord.x).sum();
|
||||
let sum_y: i32 = cells.iter().map(|c| c.coord.y).sum();
|
||||
let centroid_grid = GridCoord::new(sum_x / size as i32, sum_y / size as i32);
|
||||
let centroid_world = storage.grid_to_world(centroid_grid);
|
||||
|
||||
// Calculate total unknown exposure
|
||||
let unknown_exposure: usize = cells.iter().map(|c| c.unknown_neighbors as usize).sum();
|
||||
|
||||
Self {
|
||||
cells,
|
||||
centroid_grid,
|
||||
centroid_world,
|
||||
size,
|
||||
unknown_exposure,
|
||||
}
|
||||
}
|
||||
|
||||
/// Exploration score (higher = more valuable to explore)
|
||||
/// Considers both size and unknown exposure
|
||||
pub fn exploration_score(&self) -> f32 {
|
||||
// Balance between size and exposure
|
||||
// Larger frontiers with more unknown cells are more valuable
|
||||
self.size as f32 * 0.5 + self.unknown_exposure as f32 * 0.5
|
||||
}
|
||||
|
||||
/// Get the closest cell in this frontier to a given position
|
||||
pub fn closest_cell_to(&self, target: GridCoord) -> Option<&FrontierCell> {
|
||||
self.cells
|
||||
.iter()
|
||||
.min_by_key(|c| c.coord.chebyshev_distance(&target))
|
||||
}
|
||||
|
||||
/// Length of the frontier in cells (approximate)
|
||||
pub fn length(&self) -> usize {
|
||||
self.size
|
||||
}
|
||||
}
|
||||
|
||||
/// Frontier detector
|
||||
pub struct FrontierDetector {
|
||||
/// Minimum frontier size (smaller clusters are filtered out)
|
||||
pub min_frontier_size: usize,
|
||||
/// Use 8-connectivity for frontier detection (vs 4-connectivity)
|
||||
pub use_8_connectivity: bool,
|
||||
}
|
||||
|
||||
impl Default for FrontierDetector {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
min_frontier_size: 3,
|
||||
use_8_connectivity: true,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl FrontierDetector {
|
||||
/// Create a new frontier detector with default settings
|
||||
pub fn new() -> Self {
|
||||
Self::default()
|
||||
}
|
||||
|
||||
/// Create with custom minimum frontier size
|
||||
pub fn with_min_size(min_size: usize) -> Self {
|
||||
Self {
|
||||
min_frontier_size: min_size,
|
||||
..Default::default()
|
||||
}
|
||||
}
|
||||
|
||||
/// Detect all frontier cells in the grid (SIMD-optimized).
|
||||
///
|
||||
/// Uses SIMD to quickly filter Floor cells (16 at a time), then
|
||||
/// performs scalar neighbor counting for each candidate.
|
||||
pub fn detect_frontier_cells(&self, storage: &GridStorage) -> Vec<FrontierCell> {
|
||||
let width = storage.width();
|
||||
let height = storage.height();
|
||||
let cell_types = storage.cell_types_raw();
|
||||
|
||||
let floor_val = CellType::Floor as u8;
|
||||
let floor_vec = u8x16::splat(floor_val);
|
||||
|
||||
let mut frontiers = Vec::new();
|
||||
|
||||
// Process 16 cells at a time using SIMD
|
||||
let chunks = cell_types.chunks_exact(16);
|
||||
let remainder = chunks.remainder();
|
||||
|
||||
for (chunk_idx, chunk) in chunks.enumerate() {
|
||||
let data = u8x16::from_slice(chunk);
|
||||
let is_floor = data.simd_eq(floor_vec);
|
||||
let mask = is_floor.to_bitmask();
|
||||
|
||||
if mask == 0 {
|
||||
continue; // No Floor cells in this chunk - skip entirely
|
||||
}
|
||||
|
||||
// Process each Floor cell found in this chunk
|
||||
let base_idx = chunk_idx * 16;
|
||||
for bit in 0..16 {
|
||||
if (mask & (1 << bit)) != 0 {
|
||||
let idx = base_idx + bit;
|
||||
let x = (idx % width) as i32;
|
||||
let y = (idx / width) as i32;
|
||||
let coord = GridCoord::new(x, y);
|
||||
|
||||
// Count unknown neighbors (scalar - depends on grid topology)
|
||||
let unknown_count =
|
||||
self.count_unknown_neighbors(storage, coord, width as i32, height as i32);
|
||||
if unknown_count > 0 {
|
||||
frontiers.push(FrontierCell::new(coord, unknown_count));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Handle remainder (< 16 cells) with scalar fallback
|
||||
let base_idx = (cell_types.len() / 16) * 16;
|
||||
for (i, &cell_type) in remainder.iter().enumerate() {
|
||||
if cell_type == floor_val {
|
||||
let idx = base_idx + i;
|
||||
let x = (idx % width) as i32;
|
||||
let y = (idx / width) as i32;
|
||||
let coord = GridCoord::new(x, y);
|
||||
|
||||
let unknown_count =
|
||||
self.count_unknown_neighbors(storage, coord, width as i32, height as i32);
|
||||
if unknown_count > 0 {
|
||||
frontiers.push(FrontierCell::new(coord, unknown_count));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
trace!("[Frontier] Detected {} frontier cells", frontiers.len());
|
||||
frontiers
|
||||
}
|
||||
|
||||
/// Count unknown neighbors for a cell (helper for SIMD detection).
|
||||
#[inline]
|
||||
fn count_unknown_neighbors(
|
||||
&self,
|
||||
storage: &GridStorage,
|
||||
coord: GridCoord,
|
||||
width: i32,
|
||||
height: i32,
|
||||
) -> u8 {
|
||||
let neighbors = if self.use_8_connectivity {
|
||||
coord.neighbors_8()
|
||||
} else {
|
||||
let n4 = coord.neighbors_4();
|
||||
[n4[0], n4[1], n4[2], n4[3], n4[0], n4[1], n4[2], n4[3]]
|
||||
};
|
||||
|
||||
let limit = if self.use_8_connectivity { 8 } else { 4 };
|
||||
let mut count = 0u8;
|
||||
|
||||
for n in neighbors.iter().take(limit) {
|
||||
if n.x >= 0
|
||||
&& n.x < width
|
||||
&& n.y >= 0
|
||||
&& n.y < height
|
||||
&& storage.get_type(*n) == CellType::Unknown
|
||||
{
|
||||
count += 1;
|
||||
}
|
||||
}
|
||||
|
||||
count
|
||||
}
|
||||
|
||||
/// Detect and cluster frontiers
|
||||
pub fn detect_frontiers(&self, storage: &GridStorage) -> Vec<Frontier> {
|
||||
let frontier_cells = self.detect_frontier_cells(storage);
|
||||
|
||||
if frontier_cells.is_empty() {
|
||||
debug!("[Frontier] No frontier cells found (no Floor cells adjacent to Unknown)");
|
||||
return Vec::new();
|
||||
}
|
||||
|
||||
// Build a set for fast lookup
|
||||
let frontier_set: HashSet<GridCoord> = frontier_cells.iter().map(|f| f.coord).collect();
|
||||
|
||||
// Map coord to FrontierCell
|
||||
let cell_map: std::collections::HashMap<GridCoord, FrontierCell> =
|
||||
frontier_cells.iter().map(|f| (f.coord, *f)).collect();
|
||||
|
||||
// Cluster using BFS
|
||||
let mut visited = HashSet::new();
|
||||
let mut frontiers = Vec::new();
|
||||
let mut clusters_before_filter = 0;
|
||||
let mut filtered_clusters = 0;
|
||||
|
||||
for cell in &frontier_cells {
|
||||
if visited.contains(&cell.coord) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// BFS to find all connected frontier cells
|
||||
let mut cluster = Vec::new();
|
||||
let mut queue = VecDeque::new();
|
||||
queue.push_back(cell.coord);
|
||||
visited.insert(cell.coord);
|
||||
|
||||
while let Some(current) = queue.pop_front() {
|
||||
if let Some(&fc) = cell_map.get(¤t) {
|
||||
cluster.push(fc);
|
||||
}
|
||||
|
||||
// Check neighbors
|
||||
let neighbors = if self.use_8_connectivity {
|
||||
current.neighbors_8().to_vec()
|
||||
} else {
|
||||
current.neighbors_4().to_vec()
|
||||
};
|
||||
|
||||
for neighbor in neighbors {
|
||||
if !visited.contains(&neighbor) && frontier_set.contains(&neighbor) {
|
||||
visited.insert(neighbor);
|
||||
queue.push_back(neighbor);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
clusters_before_filter += 1;
|
||||
|
||||
// Only keep clusters above minimum size
|
||||
if cluster.len() >= self.min_frontier_size {
|
||||
frontiers.push(Frontier::from_cells(cluster, storage));
|
||||
} else {
|
||||
filtered_clusters += 1;
|
||||
trace!(
|
||||
"[Frontier] Filtered cluster with {} cells (min_size={})",
|
||||
cluster.len(),
|
||||
self.min_frontier_size
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
debug!(
|
||||
"[Frontier] Clustering: {} cells → {} clusters, {} valid (filtered {} < {} cells)",
|
||||
frontier_cells.len(),
|
||||
clusters_before_filter,
|
||||
frontiers.len(),
|
||||
filtered_clusters,
|
||||
self.min_frontier_size
|
||||
);
|
||||
|
||||
frontiers
|
||||
}
|
||||
|
||||
/// Get frontiers sorted by exploration score (highest first)
|
||||
pub fn detect_frontiers_sorted(&self, storage: &GridStorage) -> Vec<Frontier> {
|
||||
let mut frontiers = self.detect_frontiers(storage);
|
||||
frontiers.sort_by(|a, b| b.exploration_score().total_cmp(&a.exploration_score()));
|
||||
frontiers
|
||||
}
|
||||
|
||||
/// Find the best frontier to explore from a given position
|
||||
pub fn best_frontier_from(
|
||||
&self,
|
||||
storage: &GridStorage,
|
||||
robot_pos: GridCoord,
|
||||
) -> Option<Frontier> {
|
||||
let frontiers = self.detect_frontiers(storage);
|
||||
|
||||
if frontiers.is_empty() {
|
||||
return None;
|
||||
}
|
||||
|
||||
// Score combines exploration value and distance
|
||||
// Closer frontiers with higher exploration potential are preferred
|
||||
frontiers.into_iter().max_by(|a, b| {
|
||||
let score_a = self.score_frontier(a, robot_pos);
|
||||
let score_b = self.score_frontier(b, robot_pos);
|
||||
score_a.total_cmp(&score_b)
|
||||
})
|
||||
}
|
||||
|
||||
/// Score a frontier considering distance from robot
|
||||
fn score_frontier(&self, frontier: &Frontier, robot_pos: GridCoord) -> f32 {
|
||||
let distance = robot_pos.manhattan_distance(&frontier.centroid_grid) as f32;
|
||||
let exploration_score = frontier.exploration_score();
|
||||
|
||||
// Penalize distance, but not too heavily
|
||||
// We want to explore nearby frontiers first, but not ignore large distant ones
|
||||
if distance > 0.0 {
|
||||
exploration_score / (1.0 + distance * 0.1)
|
||||
} else {
|
||||
exploration_score * 2.0 // Bonus for being at the frontier
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Quick frontier detection without clustering (for performance)
|
||||
pub fn count_frontier_cells(storage: &GridStorage) -> usize {
|
||||
let detector = FrontierDetector::new();
|
||||
detector.detect_frontier_cells(storage).len()
|
||||
}
|
||||
|
||||
/// Check if any frontiers exist in the map
|
||||
pub fn has_frontiers(storage: &GridStorage) -> bool {
|
||||
let detector = FrontierDetector::new();
|
||||
!detector.detect_frontier_cells(storage).is_empty()
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use std::simd::{cmp::SimdPartialEq, u8x16};
|
||||
|
||||
fn create_test_storage() -> GridStorage {
|
||||
GridStorage::centered(20, 20, 0.1) // 2m x 2m at 10cm resolution
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// SIMD-OPTIMIZED TEST HELPERS
|
||||
// These are test-only implementations to verify SIMD correctness
|
||||
// =========================================================================
|
||||
|
||||
/// Detect all frontier cells using SIMD-accelerated Floor cell detection.
|
||||
fn detect_frontier_cells_simd(
|
||||
detector: &FrontierDetector,
|
||||
storage: &GridStorage,
|
||||
) -> Vec<FrontierCell> {
|
||||
let width = storage.width();
|
||||
let height = storage.height();
|
||||
let cell_types = storage.cell_types_raw();
|
||||
|
||||
let floor_val = CellType::Floor as u8;
|
||||
let floor_vec = u8x16::splat(floor_val);
|
||||
|
||||
let mut frontiers = Vec::new();
|
||||
|
||||
// Process 16 cells at a time using SIMD
|
||||
let chunks = cell_types.chunks_exact(16);
|
||||
let remainder = chunks.remainder();
|
||||
|
||||
for (chunk_idx, chunk) in chunks.enumerate() {
|
||||
let data = u8x16::from_slice(chunk);
|
||||
let is_floor = data.simd_eq(floor_vec);
|
||||
let mask = is_floor.to_bitmask();
|
||||
|
||||
if mask == 0 {
|
||||
continue; // No Floor cells in this chunk
|
||||
}
|
||||
|
||||
// Process each Floor cell found
|
||||
let base_idx = chunk_idx * 16;
|
||||
for bit in 0..16 {
|
||||
if (mask & (1 << bit)) != 0 {
|
||||
let idx = base_idx + bit;
|
||||
let x = (idx % width) as i32;
|
||||
let y = (idx / width) as i32;
|
||||
let coord = GridCoord::new(x, y);
|
||||
|
||||
// Count unknown neighbors (scalar)
|
||||
let unknown_count = count_unknown_neighbors(
|
||||
detector,
|
||||
storage,
|
||||
coord,
|
||||
width as i32,
|
||||
height as i32,
|
||||
);
|
||||
if unknown_count > 0 {
|
||||
frontiers.push(FrontierCell::new(coord, unknown_count));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Handle remainder (< 16 cells)
|
||||
let base_idx = (cell_types.len() / 16) * 16;
|
||||
for (i, &cell_type) in remainder.iter().enumerate() {
|
||||
if cell_type == floor_val {
|
||||
let idx = base_idx + i;
|
||||
let x = (idx % width) as i32;
|
||||
let y = (idx / width) as i32;
|
||||
let coord = GridCoord::new(x, y);
|
||||
|
||||
let unknown_count =
|
||||
count_unknown_neighbors(detector, storage, coord, width as i32, height as i32);
|
||||
if unknown_count > 0 {
|
||||
frontiers.push(FrontierCell::new(coord, unknown_count));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
frontiers
|
||||
}
|
||||
|
||||
/// Count unknown neighbors for a given cell
|
||||
fn count_unknown_neighbors(
|
||||
detector: &FrontierDetector,
|
||||
storage: &GridStorage,
|
||||
coord: GridCoord,
|
||||
width: i32,
|
||||
height: i32,
|
||||
) -> u8 {
|
||||
let neighbors = if detector.use_8_connectivity {
|
||||
coord.neighbors_8()
|
||||
} else {
|
||||
let n4 = coord.neighbors_4();
|
||||
[n4[0], n4[1], n4[2], n4[3], n4[0], n4[1], n4[2], n4[3]]
|
||||
};
|
||||
|
||||
let limit = if detector.use_8_connectivity { 8 } else { 4 };
|
||||
let mut count = 0u8;
|
||||
|
||||
for n in neighbors.iter().take(limit) {
|
||||
if n.x >= 0
|
||||
&& n.x < width
|
||||
&& n.y >= 0
|
||||
&& n.y < height
|
||||
&& storage.get_type(*n) == CellType::Unknown
|
||||
{
|
||||
count += 1;
|
||||
}
|
||||
}
|
||||
|
||||
count
|
||||
}
|
||||
|
||||
/// Detect and cluster frontiers using SIMD-optimized cell detection.
|
||||
fn detect_frontiers_simd(detector: &FrontierDetector, storage: &GridStorage) -> Vec<Frontier> {
|
||||
let frontier_cells = detect_frontier_cells_simd(detector, storage);
|
||||
|
||||
if frontier_cells.is_empty() {
|
||||
return Vec::new();
|
||||
}
|
||||
|
||||
cluster_frontier_cells(detector, frontier_cells, storage)
|
||||
}
|
||||
|
||||
/// Cluster frontier cells into connected frontiers.
|
||||
fn cluster_frontier_cells(
|
||||
detector: &FrontierDetector,
|
||||
frontier_cells: Vec<FrontierCell>,
|
||||
storage: &GridStorage,
|
||||
) -> Vec<Frontier> {
|
||||
// Build a set for fast lookup
|
||||
let frontier_set: HashSet<GridCoord> = frontier_cells.iter().map(|f| f.coord).collect();
|
||||
|
||||
// Map coord to FrontierCell
|
||||
let cell_map: std::collections::HashMap<GridCoord, FrontierCell> =
|
||||
frontier_cells.iter().map(|f| (f.coord, *f)).collect();
|
||||
|
||||
// Cluster using BFS
|
||||
let mut visited = HashSet::new();
|
||||
let mut frontiers = Vec::new();
|
||||
|
||||
for cell in &frontier_cells {
|
||||
if visited.contains(&cell.coord) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// BFS to find all connected frontier cells
|
||||
let mut cluster = Vec::new();
|
||||
let mut queue = VecDeque::new();
|
||||
queue.push_back(cell.coord);
|
||||
visited.insert(cell.coord);
|
||||
|
||||
while let Some(current) = queue.pop_front() {
|
||||
if let Some(&fc) = cell_map.get(¤t) {
|
||||
cluster.push(fc);
|
||||
}
|
||||
|
||||
// Check neighbors
|
||||
let neighbors = if detector.use_8_connectivity {
|
||||
current.neighbors_8().to_vec()
|
||||
} else {
|
||||
current.neighbors_4().to_vec()
|
||||
};
|
||||
|
||||
for neighbor in neighbors {
|
||||
if !visited.contains(&neighbor) && frontier_set.contains(&neighbor) {
|
||||
visited.insert(neighbor);
|
||||
queue.push_back(neighbor);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Only keep clusters above minimum size
|
||||
if cluster.len() >= detector.min_frontier_size {
|
||||
frontiers.push(Frontier::from_cells(cluster, storage));
|
||||
}
|
||||
}
|
||||
|
||||
frontiers
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_no_frontiers_on_empty_grid() {
|
||||
let storage = create_test_storage();
|
||||
let detector = FrontierDetector::new();
|
||||
|
||||
let frontiers = detector.detect_frontiers(&storage);
|
||||
assert!(frontiers.is_empty());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_frontier_detection_basic() {
|
||||
let mut storage = create_test_storage();
|
||||
|
||||
// Create a small floor area
|
||||
for x in 8..12 {
|
||||
for y in 8..12 {
|
||||
storage.set_type(GridCoord::new(x, y), CellType::Floor);
|
||||
}
|
||||
}
|
||||
|
||||
let detector = FrontierDetector::new();
|
||||
let cells = detector.detect_frontier_cells(&storage);
|
||||
|
||||
// All edge cells should be frontiers
|
||||
assert!(cells.len() > 0);
|
||||
|
||||
// Center cell should NOT be a frontier
|
||||
let center = GridCoord::new(10, 10);
|
||||
assert!(!cells.iter().any(|f| f.coord == center));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_frontier_clustering() {
|
||||
let mut storage = create_test_storage();
|
||||
|
||||
// Create two separate floor regions
|
||||
// Region 1
|
||||
for x in 2..5 {
|
||||
for y in 2..5 {
|
||||
storage.set_type(GridCoord::new(x, y), CellType::Floor);
|
||||
}
|
||||
}
|
||||
// Region 2 (separated by unknown)
|
||||
for x in 15..18 {
|
||||
for y in 15..18 {
|
||||
storage.set_type(GridCoord::new(x, y), CellType::Floor);
|
||||
}
|
||||
}
|
||||
|
||||
let detector = FrontierDetector::new();
|
||||
let frontiers = detector.detect_frontiers(&storage);
|
||||
|
||||
// Should have 2 frontier clusters
|
||||
assert_eq!(frontiers.len(), 2);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_frontier_centroid() {
|
||||
let mut storage = create_test_storage();
|
||||
|
||||
// Create a symmetric floor region
|
||||
for x in 5..15 {
|
||||
for y in 5..15 {
|
||||
storage.set_type(GridCoord::new(x, y), CellType::Floor);
|
||||
}
|
||||
}
|
||||
|
||||
let detector = FrontierDetector::new();
|
||||
let frontiers = detector.detect_frontiers(&storage);
|
||||
|
||||
assert_eq!(frontiers.len(), 1);
|
||||
|
||||
// Centroid should be roughly in the middle of the frontier ring
|
||||
let frontier = &frontiers[0];
|
||||
// The frontier is on the edge, so centroid should be near edges
|
||||
assert!(frontier.centroid_grid.x >= 5 && frontier.centroid_grid.x <= 14);
|
||||
assert!(frontier.centroid_grid.y >= 5 && frontier.centroid_grid.y <= 14);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_min_frontier_size() {
|
||||
let mut storage = create_test_storage();
|
||||
|
||||
// Create a very small floor area (just 1 cell)
|
||||
storage.set_type(GridCoord::new(10, 10), CellType::Floor);
|
||||
|
||||
let detector = FrontierDetector::with_min_size(5);
|
||||
let frontiers = detector.detect_frontiers(&storage);
|
||||
|
||||
// Single cell frontier should be filtered out
|
||||
assert!(frontiers.is_empty());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_wall_blocks_frontier() {
|
||||
let mut storage = create_test_storage();
|
||||
|
||||
// Floor surrounded by walls on one side
|
||||
for x in 8..12 {
|
||||
for y in 8..12 {
|
||||
storage.set_type(GridCoord::new(x, y), CellType::Floor);
|
||||
}
|
||||
}
|
||||
// Add wall on the right
|
||||
for y in 7..13 {
|
||||
storage.set_type(GridCoord::new(12, y), CellType::Wall);
|
||||
}
|
||||
|
||||
let detector = FrontierDetector::new();
|
||||
let cells = detector.detect_frontier_cells(&storage);
|
||||
|
||||
// Cells next to wall should not have frontiers on that side
|
||||
let right_edge = GridCoord::new(11, 10);
|
||||
let frontier_cell = cells.iter().find(|f| f.coord == right_edge);
|
||||
|
||||
// The right edge cell should have fewer unknown neighbors
|
||||
// (wall blocks the right side from being unknown)
|
||||
if let Some(fc) = frontier_cell {
|
||||
// Should be < 8 unknown neighbors due to wall
|
||||
assert!(fc.unknown_neighbors < 8);
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_exploration_score() {
|
||||
let mut storage = create_test_storage();
|
||||
|
||||
// Large floor area
|
||||
for x in 2..18 {
|
||||
for y in 2..18 {
|
||||
storage.set_type(GridCoord::new(x, y), CellType::Floor);
|
||||
}
|
||||
}
|
||||
|
||||
let detector = FrontierDetector::new();
|
||||
let frontiers = detector.detect_frontiers(&storage);
|
||||
|
||||
assert!(!frontiers.is_empty());
|
||||
|
||||
// Score should be positive
|
||||
assert!(frontiers[0].exploration_score() > 0.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_best_frontier_from() {
|
||||
let mut storage = create_test_storage();
|
||||
|
||||
// Create floor area
|
||||
for x in 5..15 {
|
||||
for y in 5..15 {
|
||||
storage.set_type(GridCoord::new(x, y), CellType::Floor);
|
||||
}
|
||||
}
|
||||
|
||||
let detector = FrontierDetector::new();
|
||||
let robot_pos = GridCoord::new(10, 10);
|
||||
|
||||
let best = detector.best_frontier_from(&storage, robot_pos);
|
||||
assert!(best.is_some());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_simd_frontier_matches_scalar() {
|
||||
let mut storage = create_test_storage();
|
||||
|
||||
// Create floor area
|
||||
for x in 5..15 {
|
||||
for y in 5..15 {
|
||||
storage.set_type(GridCoord::new(x, y), CellType::Floor);
|
||||
}
|
||||
}
|
||||
|
||||
let detector = FrontierDetector::new();
|
||||
|
||||
// Get scalar frontier cells
|
||||
let scalar_cells = detector.detect_frontier_cells(&storage);
|
||||
|
||||
// Get SIMD frontier cells
|
||||
let simd_cells = detect_frontier_cells_simd(&detector, &storage);
|
||||
|
||||
// Should have same count
|
||||
assert_eq!(
|
||||
scalar_cells.len(),
|
||||
simd_cells.len(),
|
||||
"Cell count mismatch: scalar={}, simd={}",
|
||||
scalar_cells.len(),
|
||||
simd_cells.len()
|
||||
);
|
||||
|
||||
// Verify same cells are found (order may differ)
|
||||
let scalar_set: std::collections::HashSet<_> = scalar_cells
|
||||
.iter()
|
||||
.map(|c| (c.coord, c.unknown_neighbors))
|
||||
.collect();
|
||||
let simd_set: std::collections::HashSet<_> = simd_cells
|
||||
.iter()
|
||||
.map(|c| (c.coord, c.unknown_neighbors))
|
||||
.collect();
|
||||
|
||||
assert_eq!(scalar_set, simd_set, "Different frontier cells detected");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_simd_frontier_clustering_matches_scalar() {
|
||||
let mut storage = create_test_storage();
|
||||
|
||||
// Create two separate floor regions
|
||||
for x in 2..5 {
|
||||
for y in 2..5 {
|
||||
storage.set_type(GridCoord::new(x, y), CellType::Floor);
|
||||
}
|
||||
}
|
||||
for x in 15..18 {
|
||||
for y in 15..18 {
|
||||
storage.set_type(GridCoord::new(x, y), CellType::Floor);
|
||||
}
|
||||
}
|
||||
|
||||
let detector = FrontierDetector::new();
|
||||
|
||||
let scalar_frontiers = detector.detect_frontiers(&storage);
|
||||
let simd_frontiers = detect_frontiers_simd(&detector, &storage);
|
||||
|
||||
// Should have same number of clusters
|
||||
assert_eq!(
|
||||
scalar_frontiers.len(),
|
||||
simd_frontiers.len(),
|
||||
"Frontier count mismatch: scalar={}, simd={}",
|
||||
scalar_frontiers.len(),
|
||||
simd_frontiers.len()
|
||||
);
|
||||
|
||||
// Each cluster should have same size
|
||||
let mut scalar_sizes: Vec<_> = scalar_frontiers.iter().map(|f| f.size).collect();
|
||||
let mut simd_sizes: Vec<_> = simd_frontiers.iter().map(|f| f.size).collect();
|
||||
scalar_sizes.sort();
|
||||
simd_sizes.sort();
|
||||
|
||||
assert_eq!(scalar_sizes, simd_sizes, "Cluster sizes differ");
|
||||
}
|
||||
}
|
||||
@@ -1,47 +0,0 @@
|
||||
//! Map query operations.
|
||||
//!
|
||||
//! This module provides query operations on the occupancy grid:
|
||||
//!
|
||||
//! - **Frontier detection**: Find boundaries between known and unknown areas
|
||||
//! - **Traversability**: Check if positions and paths are safe for the robot
|
||||
//!
|
||||
//! ## Frontier Detection
|
||||
//!
|
||||
//! Frontiers are floor cells adjacent to unknown cells. They represent the
|
||||
//! boundary of explored space and are used as targets for exploration.
|
||||
//!
|
||||
//! ```rust,ignore
|
||||
//! use vastu_map::query::{FrontierDetector, Frontier};
|
||||
//!
|
||||
//! let detector = FrontierDetector::new();
|
||||
//! let frontiers = detector.detect_frontiers(map.storage());
|
||||
//!
|
||||
//! for frontier in frontiers {
|
||||
//! println!("Frontier at {:?} with {} cells",
|
||||
//! frontier.centroid_world, frontier.size);
|
||||
//! }
|
||||
//! ```
|
||||
//!
|
||||
//! ## Traversability
|
||||
//!
|
||||
//! Check if positions and paths are safe for robot navigation.
|
||||
//!
|
||||
//! ```rust,ignore
|
||||
//! use vastu_map::query::{TraversabilityChecker, RobotFootprint};
|
||||
//!
|
||||
//! let footprint = RobotFootprint::new(0.17, 0.05);
|
||||
//! let checker = TraversabilityChecker::new(map.storage(), footprint);
|
||||
//!
|
||||
//! if checker.is_position_safe(target_position) {
|
||||
//! // Safe to navigate to
|
||||
//! }
|
||||
//! ```
|
||||
|
||||
pub mod frontier;
|
||||
pub mod traversability;
|
||||
|
||||
pub use frontier::{Frontier, FrontierCell, FrontierDetector, count_frontier_cells, has_frontiers};
|
||||
|
||||
pub use traversability::{
|
||||
PathCheckResult, RobotFootprint, TraversabilityChecker, is_safe, is_traversable,
|
||||
};
|
||||
@@ -1,400 +0,0 @@
|
||||
//! Traversability checker implementation.
|
||||
|
||||
use std::simd::{cmp::SimdPartialOrd, u8x16};
|
||||
|
||||
use crate::core::{CellType, GridCoord, WorldPoint};
|
||||
use crate::grid::GridStorage;
|
||||
|
||||
use super::footprint::RobotFootprint;
|
||||
use super::path::PathCheckResult;
|
||||
|
||||
/// Traversability checker for path planning and navigation.
|
||||
pub struct TraversabilityChecker<'a> {
|
||||
storage: &'a GridStorage,
|
||||
footprint: RobotFootprint,
|
||||
}
|
||||
|
||||
impl<'a> TraversabilityChecker<'a> {
|
||||
/// Create a new traversability checker.
|
||||
pub fn new(storage: &'a GridStorage, footprint: RobotFootprint) -> Self {
|
||||
Self { storage, footprint }
|
||||
}
|
||||
|
||||
/// Create with default robot footprint.
|
||||
pub fn with_default_footprint(storage: &'a GridStorage) -> Self {
|
||||
Self::new(storage, RobotFootprint::default())
|
||||
}
|
||||
|
||||
/// Get the robot footprint.
|
||||
pub fn footprint(&self) -> &RobotFootprint {
|
||||
&self.footprint
|
||||
}
|
||||
|
||||
/// Get the storage.
|
||||
pub fn storage(&self) -> &GridStorage {
|
||||
self.storage
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// Single cell traversability
|
||||
// =========================================================================
|
||||
|
||||
/// Check if a single cell is traversable.
|
||||
pub fn is_cell_traversable(&self, coord: GridCoord) -> bool {
|
||||
if !self.storage.is_valid_coord(coord) {
|
||||
return false;
|
||||
}
|
||||
self.storage.get_type(coord).is_traversable()
|
||||
}
|
||||
|
||||
/// Check if a world point is traversable (single cell).
|
||||
pub fn is_point_traversable(&self, point: WorldPoint) -> bool {
|
||||
let coord = self.storage.world_to_grid(point);
|
||||
self.is_cell_traversable(coord)
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// Position safety (considering footprint)
|
||||
// =========================================================================
|
||||
|
||||
/// Check if a position is safe for the robot (considering footprint).
|
||||
pub fn is_position_safe(&self, position: WorldPoint) -> bool {
|
||||
self.is_position_safe_mode(position, false)
|
||||
}
|
||||
|
||||
/// Check if a position is safe, with option to allow Unknown cells (SIMD-optimized).
|
||||
///
|
||||
/// In exploration mode, Unknown cells are allowed in the footprint
|
||||
/// (they're assumed to be likely free). Only actual obstacles block.
|
||||
///
|
||||
/// Uses SIMD to quickly check the bounding box for obstacles before
|
||||
/// performing the circular footprint check.
|
||||
pub fn is_position_safe_mode(&self, position: WorldPoint, allow_unknown: bool) -> bool {
|
||||
let center = self.storage.world_to_grid(position);
|
||||
let resolution = self.storage.resolution();
|
||||
let cells_radius = (self.footprint.total_radius() / resolution).ceil() as i32;
|
||||
|
||||
// Fast path: SIMD check for obstacles in bounding box
|
||||
if !self.is_rect_obstacle_free(center, cells_radius, cells_radius) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// If allow_unknown is true, we're done (no obstacles found via SIMD)
|
||||
if allow_unknown {
|
||||
// Still need to check bounds validity
|
||||
return self.is_footprint_in_bounds(center, cells_radius, position);
|
||||
}
|
||||
|
||||
// Strict mode: Need to check for Unknown cells in circular footprint
|
||||
self.is_footprint_traversable(center, cells_radius, position)
|
||||
}
|
||||
|
||||
/// Check if a position is known (all cells within footprint are known).
|
||||
pub fn is_position_known(&self, position: WorldPoint) -> bool {
|
||||
let center = self.storage.world_to_grid(position);
|
||||
let resolution = self.storage.resolution();
|
||||
let cells_radius = (self.footprint.total_radius() / resolution).ceil() as i32;
|
||||
|
||||
for dy in -cells_radius..=cells_radius {
|
||||
for dx in -cells_radius..=cells_radius {
|
||||
let coord = center + GridCoord::new(dx, dy);
|
||||
|
||||
if !self.storage.is_valid_coord(coord) {
|
||||
return false;
|
||||
}
|
||||
|
||||
let cell_world = self.storage.grid_to_world(coord);
|
||||
let dist = cell_world.distance(&position);
|
||||
|
||||
if dist <= self.footprint.total_radius() && !self.storage.get_type(coord).is_known()
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
true
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// SIMD-optimized obstacle checking
|
||||
// =========================================================================
|
||||
|
||||
/// Check if a rectangular region contains any obstacle cells (SIMD-optimized).
|
||||
///
|
||||
/// Obstacles are Wall, Cliff, or Bump (cell_type >= 2).
|
||||
/// Returns true if no obstacles found.
|
||||
#[inline]
|
||||
pub(super) fn is_rect_obstacle_free(
|
||||
&self,
|
||||
center: GridCoord,
|
||||
half_width: i32,
|
||||
half_height: i32,
|
||||
) -> bool {
|
||||
let width = self.storage.width() as i32;
|
||||
let height = self.storage.height() as i32;
|
||||
|
||||
let min_x = (center.x - half_width).max(0) as usize;
|
||||
let max_x = ((center.x + half_width).min(width - 1) as usize).min(self.storage.width() - 1);
|
||||
let min_y = (center.y - half_height).max(0) as usize;
|
||||
let max_y =
|
||||
((center.y + half_height).min(height - 1) as usize).min(self.storage.height() - 1);
|
||||
|
||||
if min_x > max_x || min_y > max_y {
|
||||
return false; // Out of bounds
|
||||
}
|
||||
|
||||
let cell_types = self.storage.cell_types_raw();
|
||||
let storage_width = self.storage.width();
|
||||
|
||||
// Wall, Cliff, Bump are >= 2 (Floor=1, Unknown=0)
|
||||
let wall_val = CellType::Wall as u8;
|
||||
let wall_vec = u8x16::splat(wall_val);
|
||||
|
||||
for y in min_y..=max_y {
|
||||
let row_start = y * storage_width + min_x;
|
||||
let row_end = y * storage_width + max_x + 1;
|
||||
let row_slice = &cell_types[row_start..row_end];
|
||||
|
||||
// Process 16 cells at a time
|
||||
let chunks = row_slice.chunks_exact(16);
|
||||
let remainder = chunks.remainder();
|
||||
|
||||
for chunk in chunks {
|
||||
let data = u8x16::from_slice(chunk);
|
||||
// Check if any cell >= Wall (2)
|
||||
let is_obstacle = data.simd_ge(wall_vec);
|
||||
if is_obstacle.to_bitmask() != 0 {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// Scalar remainder
|
||||
for &cell_type in remainder {
|
||||
if cell_type >= wall_val {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
true
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// Footprint checking helpers
|
||||
// =========================================================================
|
||||
|
||||
/// Check if all cells in the circular footprint are within bounds.
|
||||
#[inline]
|
||||
fn is_footprint_in_bounds(
|
||||
&self,
|
||||
center: GridCoord,
|
||||
cells_radius: i32,
|
||||
position: WorldPoint,
|
||||
) -> bool {
|
||||
for dy in -cells_radius..=cells_radius {
|
||||
for dx in -cells_radius..=cells_radius {
|
||||
let coord = center + GridCoord::new(dx, dy);
|
||||
|
||||
if !self.storage.is_valid_coord(coord) {
|
||||
// Check if this cell is within the circular footprint
|
||||
let cell_world = self.storage.grid_to_world(coord);
|
||||
let dist = cell_world.distance(&position);
|
||||
if dist <= self.footprint.total_radius() {
|
||||
return false; // Out of bounds within footprint
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
true
|
||||
}
|
||||
|
||||
/// Check if all cells in the circular footprint are traversable (Floor).
|
||||
#[inline]
|
||||
fn is_footprint_traversable(
|
||||
&self,
|
||||
center: GridCoord,
|
||||
cells_radius: i32,
|
||||
position: WorldPoint,
|
||||
) -> bool {
|
||||
for dy in -cells_radius..=cells_radius {
|
||||
for dx in -cells_radius..=cells_radius {
|
||||
let coord = center + GridCoord::new(dx, dy);
|
||||
|
||||
if !self.storage.is_valid_coord(coord) {
|
||||
// Check if this cell is within the circular footprint
|
||||
let cell_world = self.storage.grid_to_world(coord);
|
||||
let dist = cell_world.distance(&position);
|
||||
if dist <= self.footprint.total_radius() {
|
||||
return false; // Out of bounds within footprint
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
// Check if within circular footprint
|
||||
let cell_world = self.storage.grid_to_world(coord);
|
||||
let dist = cell_world.distance(&position);
|
||||
|
||||
if dist <= self.footprint.total_radius() {
|
||||
let cell_type = self.storage.get_type(coord);
|
||||
if !cell_type.is_traversable() {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
true
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// Path checking
|
||||
// =========================================================================
|
||||
|
||||
/// Check if a straight-line path is traversable.
|
||||
/// Returns the furthest traversable point and whether the full path is clear.
|
||||
pub fn check_path(
|
||||
&self,
|
||||
start: WorldPoint,
|
||||
end: WorldPoint,
|
||||
step_size: Option<f32>,
|
||||
) -> PathCheckResult {
|
||||
let step = step_size.unwrap_or(self.storage.resolution() * 0.5);
|
||||
let total_dist = start.distance(&end);
|
||||
|
||||
if total_dist < step {
|
||||
// Very short path - just check endpoints
|
||||
let start_safe = self.is_position_safe(start);
|
||||
let end_safe = self.is_position_safe(end);
|
||||
|
||||
return PathCheckResult {
|
||||
is_clear: start_safe && end_safe,
|
||||
blocked_at: if !start_safe {
|
||||
Some(start)
|
||||
} else if !end_safe {
|
||||
Some(end)
|
||||
} else {
|
||||
None
|
||||
},
|
||||
furthest_safe: if start_safe { Some(end) } else { None },
|
||||
distance_traversable: if start_safe && end_safe {
|
||||
total_dist
|
||||
} else {
|
||||
0.0
|
||||
},
|
||||
};
|
||||
}
|
||||
|
||||
let dir_x = (end.x - start.x) / total_dist;
|
||||
let dir_y = (end.y - start.y) / total_dist;
|
||||
|
||||
let mut furthest_safe = start;
|
||||
let mut distance_traversable = 0.0;
|
||||
|
||||
// Check start position first
|
||||
if !self.is_position_safe(start) {
|
||||
return PathCheckResult {
|
||||
is_clear: false,
|
||||
blocked_at: Some(start),
|
||||
furthest_safe: None,
|
||||
distance_traversable: 0.0,
|
||||
};
|
||||
}
|
||||
|
||||
let mut dist = step;
|
||||
while dist < total_dist {
|
||||
let point = WorldPoint::new(start.x + dir_x * dist, start.y + dir_y * dist);
|
||||
|
||||
if self.is_position_safe(point) {
|
||||
furthest_safe = point;
|
||||
distance_traversable = dist;
|
||||
} else {
|
||||
return PathCheckResult {
|
||||
is_clear: false,
|
||||
blocked_at: Some(point),
|
||||
furthest_safe: Some(furthest_safe),
|
||||
distance_traversable,
|
||||
};
|
||||
}
|
||||
|
||||
dist += step;
|
||||
}
|
||||
|
||||
// Check endpoint
|
||||
if self.is_position_safe(end) {
|
||||
PathCheckResult {
|
||||
is_clear: true,
|
||||
blocked_at: None,
|
||||
furthest_safe: Some(end),
|
||||
distance_traversable: total_dist,
|
||||
}
|
||||
} else {
|
||||
PathCheckResult {
|
||||
is_clear: false,
|
||||
blocked_at: Some(end),
|
||||
furthest_safe: Some(furthest_safe),
|
||||
distance_traversable,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Check if movement from current position in a direction is safe.
|
||||
pub fn check_direction(
|
||||
&self,
|
||||
position: WorldPoint,
|
||||
angle: f32,
|
||||
distance: f32,
|
||||
) -> PathCheckResult {
|
||||
let end = WorldPoint::new(
|
||||
position.x + distance * angle.cos(),
|
||||
position.y + distance * angle.sin(),
|
||||
);
|
||||
self.check_path(position, end, None)
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// Obstacle queries
|
||||
// =========================================================================
|
||||
|
||||
/// Find the nearest obstacle from a position.
|
||||
pub fn nearest_obstacle(&self, position: WorldPoint, max_distance: f32) -> Option<WorldPoint> {
|
||||
let center = self.storage.world_to_grid(position);
|
||||
let resolution = self.storage.resolution();
|
||||
let max_cells = (max_distance / resolution).ceil() as i32;
|
||||
|
||||
let mut nearest: Option<(WorldPoint, f32)> = None;
|
||||
|
||||
for dy in -max_cells..=max_cells {
|
||||
for dx in -max_cells..=max_cells {
|
||||
let coord = center + GridCoord::new(dx, dy);
|
||||
|
||||
if !self.storage.is_valid_coord(coord) {
|
||||
continue;
|
||||
}
|
||||
|
||||
let cell_type = self.storage.get_type(coord);
|
||||
if cell_type.is_obstacle() {
|
||||
let obstacle_pos = self.storage.grid_to_world(coord);
|
||||
let dist = position.distance(&obstacle_pos);
|
||||
|
||||
if dist <= max_distance {
|
||||
match nearest {
|
||||
None => nearest = Some((obstacle_pos, dist)),
|
||||
Some((_, d)) if dist < d => nearest = Some((obstacle_pos, dist)),
|
||||
_ => {}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
nearest.map(|(p, _)| p)
|
||||
}
|
||||
|
||||
/// Get clearance (distance to nearest obstacle) at a position.
|
||||
pub fn clearance(&self, position: WorldPoint, max_distance: f32) -> f32 {
|
||||
match self.nearest_obstacle(position, max_distance) {
|
||||
Some(obstacle) => position.distance(&obstacle),
|
||||
None => max_distance,
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,36 +0,0 @@
|
||||
//! Robot footprint definition for collision checking.
|
||||
|
||||
use serde::{Deserialize, Serialize};
|
||||
|
||||
/// Robot footprint for collision checking.
|
||||
#[derive(Clone, Debug, Serialize, Deserialize)]
|
||||
pub struct RobotFootprint {
|
||||
/// Robot radius in meters.
|
||||
pub radius: f32,
|
||||
/// Safety margin in meters.
|
||||
pub safety_margin: f32,
|
||||
}
|
||||
|
||||
impl Default for RobotFootprint {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
radius: 0.17, // CRL-200S robot radius
|
||||
safety_margin: 0.05, // 5cm safety margin
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl RobotFootprint {
|
||||
/// Create a new robot footprint.
|
||||
pub fn new(radius: f32, safety_margin: f32) -> Self {
|
||||
Self {
|
||||
radius,
|
||||
safety_margin,
|
||||
}
|
||||
}
|
||||
|
||||
/// Total radius including safety margin.
|
||||
pub fn total_radius(&self) -> f32 {
|
||||
self.radius + self.safety_margin
|
||||
}
|
||||
}
|
||||
@@ -1,508 +0,0 @@
|
||||
//! Traversability queries for path planning and navigation.
|
||||
//!
|
||||
//! Provides methods to check if positions and paths are safe for robot traversal.
|
||||
|
||||
mod checker;
|
||||
mod footprint;
|
||||
mod path;
|
||||
|
||||
pub use checker::TraversabilityChecker;
|
||||
pub use footprint::RobotFootprint;
|
||||
pub use path::PathCheckResult;
|
||||
|
||||
use crate::core::WorldPoint;
|
||||
use crate::grid::GridStorage;
|
||||
|
||||
/// Quick check if a point is traversable.
|
||||
pub fn is_traversable(storage: &GridStorage, point: WorldPoint) -> bool {
|
||||
let checker = TraversabilityChecker::with_default_footprint(storage);
|
||||
checker.is_point_traversable(point)
|
||||
}
|
||||
|
||||
/// Quick check if a point is safe for the robot.
|
||||
pub fn is_safe(storage: &GridStorage, point: WorldPoint) -> bool {
|
||||
let checker = TraversabilityChecker::with_default_footprint(storage);
|
||||
checker.is_position_safe(point)
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use crate::core::{CellType, GridCoord};
|
||||
use std::simd::{cmp::SimdPartialOrd, u8x16};
|
||||
|
||||
fn create_test_storage() -> GridStorage {
|
||||
GridStorage::centered(100, 100, 0.05) // 5m x 5m at 5cm resolution
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// SIMD-OPTIMIZED TEST HELPERS
|
||||
// These are test-only implementations to verify SIMD correctness
|
||||
// =========================================================================
|
||||
|
||||
/// Check if a rectangular region contains any obstacle cells (SIMD-optimized).
|
||||
fn is_rect_obstacle_free(
|
||||
checker: &TraversabilityChecker,
|
||||
center: GridCoord,
|
||||
half_width: i32,
|
||||
half_height: i32,
|
||||
) -> bool {
|
||||
let width = checker.storage().width() as i32;
|
||||
let height = checker.storage().height() as i32;
|
||||
|
||||
let min_x = (center.x - half_width).max(0) as usize;
|
||||
let max_x =
|
||||
((center.x + half_width).min(width - 1) as usize).min(checker.storage().width() - 1);
|
||||
let min_y = (center.y - half_height).max(0) as usize;
|
||||
let max_y =
|
||||
((center.y + half_height).min(height - 1) as usize).min(checker.storage().height() - 1);
|
||||
|
||||
if min_x > max_x || min_y > max_y {
|
||||
return false; // Out of bounds
|
||||
}
|
||||
|
||||
let cell_types = checker.storage().cell_types_raw();
|
||||
let storage_width = checker.storage().width();
|
||||
|
||||
let wall_val = CellType::Wall as u8;
|
||||
let wall_vec = u8x16::splat(wall_val);
|
||||
|
||||
for y in min_y..=max_y {
|
||||
let row_start = y * storage_width + min_x;
|
||||
let row_end = y * storage_width + max_x + 1;
|
||||
let row_slice = &cell_types[row_start..row_end];
|
||||
|
||||
// Process 16 cells at a time
|
||||
let chunks = row_slice.chunks_exact(16);
|
||||
let remainder = chunks.remainder();
|
||||
|
||||
for chunk in chunks {
|
||||
let data = u8x16::from_slice(chunk);
|
||||
let is_obstacle = data.simd_ge(wall_vec);
|
||||
if is_obstacle.to_bitmask() != 0 {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
for &cell_type in remainder {
|
||||
if cell_type >= wall_val {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
true
|
||||
}
|
||||
|
||||
/// Check if a position is safe using SIMD for bounding box pre-check.
|
||||
fn is_position_safe_simd(checker: &TraversabilityChecker, position: WorldPoint) -> bool {
|
||||
let center = checker.storage().world_to_grid(position);
|
||||
let resolution = checker.storage().resolution();
|
||||
let cells_radius = (checker.footprint().total_radius() / resolution).ceil() as i32;
|
||||
|
||||
if !is_rect_obstacle_free(checker, center, cells_radius, cells_radius) {
|
||||
return false;
|
||||
}
|
||||
|
||||
is_position_safe_check_unknown(checker, position, center, cells_radius)
|
||||
}
|
||||
|
||||
/// Check for Unknown cells within the footprint (helper for SIMD version)
|
||||
fn is_position_safe_check_unknown(
|
||||
checker: &TraversabilityChecker,
|
||||
position: WorldPoint,
|
||||
center: GridCoord,
|
||||
cells_radius: i32,
|
||||
) -> bool {
|
||||
for dy in -cells_radius..=cells_radius {
|
||||
for dx in -cells_radius..=cells_radius {
|
||||
let coord = center + GridCoord::new(dx, dy);
|
||||
|
||||
if !checker.storage().is_valid_coord(coord) {
|
||||
return false;
|
||||
}
|
||||
|
||||
let cell_world = checker.storage().grid_to_world(coord);
|
||||
let dist = cell_world.distance(&position);
|
||||
|
||||
if dist <= checker.footprint().total_radius() {
|
||||
let cell_type = checker.storage().get_type(coord);
|
||||
if !cell_type.is_traversable() {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
true
|
||||
}
|
||||
|
||||
/// Find nearest obstacle using SIMD for batch distance calculations.
|
||||
fn nearest_obstacle_simd(
|
||||
checker: &TraversabilityChecker,
|
||||
position: WorldPoint,
|
||||
max_distance: f32,
|
||||
) -> Option<WorldPoint> {
|
||||
let center = checker.storage().world_to_grid(position);
|
||||
let resolution = checker.storage().resolution();
|
||||
let max_cells = (max_distance / resolution).ceil() as i32;
|
||||
|
||||
let width = checker.storage().width() as i32;
|
||||
let height = checker.storage().height() as i32;
|
||||
|
||||
let cell_types = checker.storage().cell_types_raw();
|
||||
let storage_width = checker.storage().width();
|
||||
|
||||
let wall_val = CellType::Wall as u8;
|
||||
let wall_vec = u8x16::splat(wall_val);
|
||||
|
||||
let mut nearest: Option<(WorldPoint, f32)> = None;
|
||||
|
||||
let min_y = (center.y - max_cells).max(0) as usize;
|
||||
let max_y =
|
||||
((center.y + max_cells).min(height - 1) as usize).min(checker.storage().height() - 1);
|
||||
let min_x = (center.x - max_cells).max(0) as usize;
|
||||
let max_x =
|
||||
((center.x + max_cells).min(width - 1) as usize).min(checker.storage().width() - 1);
|
||||
|
||||
for y in min_y..=max_y {
|
||||
let row_start = y * storage_width + min_x;
|
||||
let row_end = y * storage_width + max_x + 1;
|
||||
let row_slice = &cell_types[row_start..row_end];
|
||||
|
||||
let chunks = row_slice.chunks_exact(16);
|
||||
let remainder = chunks.remainder();
|
||||
|
||||
for (chunk_idx, chunk) in chunks.enumerate() {
|
||||
let data = u8x16::from_slice(chunk);
|
||||
let is_obstacle = data.simd_ge(wall_vec);
|
||||
let mask = is_obstacle.to_bitmask();
|
||||
|
||||
if mask != 0 {
|
||||
let base_x = min_x + chunk_idx * 16;
|
||||
for bit in 0..16 {
|
||||
if (mask & (1 << bit)) != 0 {
|
||||
let x = base_x + bit;
|
||||
let coord = GridCoord::new(x as i32, y as i32);
|
||||
let obstacle_pos = checker.storage().grid_to_world(coord);
|
||||
let dist = position.distance(&obstacle_pos);
|
||||
|
||||
if dist <= max_distance {
|
||||
match nearest {
|
||||
None => nearest = Some((obstacle_pos, dist)),
|
||||
Some((_, d)) if dist < d => {
|
||||
nearest = Some((obstacle_pos, dist))
|
||||
}
|
||||
_ => {}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
let remainder_start = min_x + (row_slice.len() / 16) * 16;
|
||||
for (i, &cell_type) in remainder.iter().enumerate() {
|
||||
if cell_type >= wall_val {
|
||||
let x = remainder_start + i;
|
||||
let coord = GridCoord::new(x as i32, y as i32);
|
||||
let obstacle_pos = checker.storage().grid_to_world(coord);
|
||||
let dist = position.distance(&obstacle_pos);
|
||||
|
||||
if dist <= max_distance {
|
||||
match nearest {
|
||||
None => nearest = Some((obstacle_pos, dist)),
|
||||
Some((_, d)) if dist < d => nearest = Some((obstacle_pos, dist)),
|
||||
_ => {}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
nearest.map(|(p, _)| p)
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_traversable_unknown() {
|
||||
let storage = create_test_storage();
|
||||
let checker = TraversabilityChecker::with_default_footprint(&storage);
|
||||
|
||||
// Unknown cells are not traversable
|
||||
assert!(!checker.is_point_traversable(WorldPoint::new(0.0, 0.0)));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_traversable_floor() {
|
||||
let mut storage = create_test_storage();
|
||||
|
||||
// Mark center as floor
|
||||
let center = storage.world_to_grid(WorldPoint::new(0.0, 0.0));
|
||||
storage.set_type(center, CellType::Floor);
|
||||
|
||||
let checker = TraversabilityChecker::with_default_footprint(&storage);
|
||||
|
||||
// Floor is traversable
|
||||
assert!(checker.is_cell_traversable(center));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_position_safe_requires_footprint() {
|
||||
let mut storage = create_test_storage();
|
||||
let center = WorldPoint::new(0.0, 0.0);
|
||||
|
||||
// Mark a large floor area
|
||||
for dx in -10..=10 {
|
||||
for dy in -10..=10 {
|
||||
let coord = storage.world_to_grid(center) + GridCoord::new(dx, dy);
|
||||
storage.set_type(coord, CellType::Floor);
|
||||
}
|
||||
}
|
||||
|
||||
let footprint = RobotFootprint::new(0.1, 0.05);
|
||||
let checker = TraversabilityChecker::new(&storage, footprint);
|
||||
|
||||
// Position should be safe with floor all around
|
||||
assert!(checker.is_position_safe(center));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_position_unsafe_near_wall() {
|
||||
let mut storage = create_test_storage();
|
||||
let center = WorldPoint::new(0.0, 0.0);
|
||||
|
||||
// Mark floor area
|
||||
let center_coord = storage.world_to_grid(center);
|
||||
for dx in -10..=10 {
|
||||
for dy in -10..=10 {
|
||||
let coord = center_coord + GridCoord::new(dx, dy);
|
||||
storage.set_type(coord, CellType::Floor);
|
||||
}
|
||||
}
|
||||
|
||||
// Put a wall very close (within robot radius)
|
||||
let wall_coord = center_coord + GridCoord::new(2, 0); // ~10cm away
|
||||
storage.set_type_with_priority(wall_coord, CellType::Wall);
|
||||
|
||||
let footprint = RobotFootprint::new(0.17, 0.05); // ~22cm total radius
|
||||
let checker = TraversabilityChecker::new(&storage, footprint);
|
||||
|
||||
// Position should be unsafe due to wall within footprint
|
||||
assert!(!checker.is_position_safe(center));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_path_check_clear() {
|
||||
let mut storage = create_test_storage();
|
||||
|
||||
// Create a floor corridor
|
||||
for x in -20..=20 {
|
||||
for y in -5..=5 {
|
||||
let coord = storage.world_to_grid(WorldPoint::ZERO) + GridCoord::new(x, y);
|
||||
storage.set_type(coord, CellType::Floor);
|
||||
}
|
||||
}
|
||||
|
||||
let footprint = RobotFootprint::new(0.1, 0.02);
|
||||
let checker = TraversabilityChecker::new(&storage, footprint);
|
||||
|
||||
let start = WorldPoint::new(-0.5, 0.0);
|
||||
let end = WorldPoint::new(0.5, 0.0);
|
||||
|
||||
let result = checker.check_path(start, end, None);
|
||||
|
||||
assert!(result.is_clear);
|
||||
assert!(result.blocked_at.is_none());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_path_check_blocked() {
|
||||
let mut storage = create_test_storage();
|
||||
|
||||
// Create floor
|
||||
for x in -20..=20 {
|
||||
for y in -5..=5 {
|
||||
let coord = storage.world_to_grid(WorldPoint::ZERO) + GridCoord::new(x, y);
|
||||
storage.set_type(coord, CellType::Floor);
|
||||
}
|
||||
}
|
||||
|
||||
// Add wall in the middle
|
||||
let wall_coord = storage.world_to_grid(WorldPoint::new(0.0, 0.0));
|
||||
storage.set_type_with_priority(wall_coord, CellType::Wall);
|
||||
|
||||
let footprint = RobotFootprint::new(0.1, 0.02);
|
||||
let checker = TraversabilityChecker::new(&storage, footprint);
|
||||
|
||||
let start = WorldPoint::new(-0.5, 0.0);
|
||||
let end = WorldPoint::new(0.5, 0.0);
|
||||
|
||||
let result = checker.check_path(start, end, None);
|
||||
|
||||
assert!(!result.is_clear);
|
||||
assert!(result.blocked_at.is_some());
|
||||
assert!(result.furthest_safe.is_some());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_clearance() {
|
||||
let mut storage = create_test_storage();
|
||||
|
||||
// Create floor
|
||||
for x in -20..=20 {
|
||||
for y in -10..=10 {
|
||||
let coord = storage.world_to_grid(WorldPoint::ZERO) + GridCoord::new(x, y);
|
||||
storage.set_type(coord, CellType::Floor);
|
||||
}
|
||||
}
|
||||
|
||||
// Add wall
|
||||
let wall_pos = WorldPoint::new(0.3, 0.0);
|
||||
let wall_coord = storage.world_to_grid(wall_pos);
|
||||
storage.set_type_with_priority(wall_coord, CellType::Wall);
|
||||
|
||||
let checker = TraversabilityChecker::with_default_footprint(&storage);
|
||||
let robot_pos = WorldPoint::new(0.0, 0.0);
|
||||
|
||||
let clearance = checker.clearance(robot_pos, 1.0);
|
||||
|
||||
// Clearance should be approximately the distance to the wall
|
||||
assert!(clearance > 0.0);
|
||||
assert!(clearance < 0.5);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_nearest_obstacle() {
|
||||
let mut storage = create_test_storage();
|
||||
|
||||
// Create floor
|
||||
for x in -20..=20 {
|
||||
for y in -20..=20 {
|
||||
let coord = storage.world_to_grid(WorldPoint::ZERO) + GridCoord::new(x, y);
|
||||
storage.set_type(coord, CellType::Floor);
|
||||
}
|
||||
}
|
||||
|
||||
// Add walls
|
||||
let wall1_pos = WorldPoint::new(0.2, 0.0);
|
||||
let wall2_pos = WorldPoint::new(-0.5, 0.0);
|
||||
storage.set_type_with_priority(storage.world_to_grid(wall1_pos), CellType::Wall);
|
||||
storage.set_type_with_priority(storage.world_to_grid(wall2_pos), CellType::Wall);
|
||||
|
||||
let checker = TraversabilityChecker::with_default_footprint(&storage);
|
||||
let robot_pos = WorldPoint::new(0.0, 0.0);
|
||||
|
||||
let nearest = checker.nearest_obstacle(robot_pos, 1.0);
|
||||
|
||||
assert!(nearest.is_some());
|
||||
// Nearest should be the closer wall
|
||||
let obstacle = nearest.unwrap();
|
||||
assert!(obstacle.distance(&robot_pos) < 0.3);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_simd_position_safe_matches_scalar() {
|
||||
let mut storage = create_test_storage();
|
||||
let center = WorldPoint::new(0.0, 0.0);
|
||||
|
||||
// Mark a large floor area
|
||||
for dx in -10..=10 {
|
||||
for dy in -10..=10 {
|
||||
let coord = storage.world_to_grid(center) + GridCoord::new(dx, dy);
|
||||
storage.set_type(coord, CellType::Floor);
|
||||
}
|
||||
}
|
||||
|
||||
let footprint = RobotFootprint::new(0.1, 0.05);
|
||||
let checker = TraversabilityChecker::new(&storage, footprint);
|
||||
|
||||
// Test positions
|
||||
let positions = [
|
||||
WorldPoint::new(0.0, 0.0),
|
||||
WorldPoint::new(0.1, 0.1),
|
||||
WorldPoint::new(-0.1, 0.1),
|
||||
];
|
||||
|
||||
for pos in positions {
|
||||
let scalar = checker.is_position_safe(pos);
|
||||
let simd = is_position_safe_simd(&checker, pos);
|
||||
assert_eq!(scalar, simd, "Mismatch at {:?}", pos);
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_simd_rect_obstacle_free() {
|
||||
let mut storage = create_test_storage();
|
||||
let center = storage.world_to_grid(WorldPoint::ZERO);
|
||||
|
||||
// Mark floor area
|
||||
for dx in -10..=10 {
|
||||
for dy in -10..=10 {
|
||||
let coord = center + GridCoord::new(dx, dy);
|
||||
storage.set_type(coord, CellType::Floor);
|
||||
}
|
||||
}
|
||||
|
||||
// First check: should be obstacle-free
|
||||
{
|
||||
let checker = TraversabilityChecker::with_default_footprint(&storage);
|
||||
assert!(is_rect_obstacle_free(&checker, center, 5, 5));
|
||||
}
|
||||
|
||||
// Add a wall
|
||||
storage.set_type_with_priority(center + GridCoord::new(2, 2), CellType::Wall);
|
||||
|
||||
// Second check: should now detect obstacle
|
||||
{
|
||||
let checker = TraversabilityChecker::with_default_footprint(&storage);
|
||||
assert!(!is_rect_obstacle_free(&checker, center, 5, 5));
|
||||
|
||||
// But smaller rect not including wall should be clear
|
||||
assert!(is_rect_obstacle_free(
|
||||
&checker,
|
||||
center + GridCoord::new(-5, 0),
|
||||
2,
|
||||
2
|
||||
));
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_simd_nearest_obstacle_matches_scalar() {
|
||||
let mut storage = create_test_storage();
|
||||
|
||||
// Create floor
|
||||
for x in -20..=20 {
|
||||
for y in -20..=20 {
|
||||
let coord = storage.world_to_grid(WorldPoint::ZERO) + GridCoord::new(x, y);
|
||||
storage.set_type(coord, CellType::Floor);
|
||||
}
|
||||
}
|
||||
|
||||
// Add walls
|
||||
let wall1_pos = WorldPoint::new(0.2, 0.0);
|
||||
let wall2_pos = WorldPoint::new(-0.5, 0.0);
|
||||
storage.set_type_with_priority(storage.world_to_grid(wall1_pos), CellType::Wall);
|
||||
storage.set_type_with_priority(storage.world_to_grid(wall2_pos), CellType::Wall);
|
||||
|
||||
let checker = TraversabilityChecker::with_default_footprint(&storage);
|
||||
let robot_pos = WorldPoint::new(0.0, 0.0);
|
||||
|
||||
let scalar_nearest = checker.nearest_obstacle(robot_pos, 1.0);
|
||||
let simd_nearest = nearest_obstacle_simd(&checker, robot_pos, 1.0);
|
||||
|
||||
assert_eq!(scalar_nearest.is_some(), simd_nearest.is_some());
|
||||
|
||||
if let (Some(s), Some(simd)) = (scalar_nearest, simd_nearest) {
|
||||
// Should find the same nearest obstacle (within grid resolution tolerance)
|
||||
let scalar_dist = s.distance(&robot_pos);
|
||||
let simd_dist = simd.distance(&robot_pos);
|
||||
assert!(
|
||||
(scalar_dist - simd_dist).abs() < storage.resolution(),
|
||||
"Distance mismatch: scalar={}, simd={}",
|
||||
scalar_dist,
|
||||
simd_dist
|
||||
);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,26 +0,0 @@
|
||||
//! Path traversability checking result.
|
||||
|
||||
use crate::core::WorldPoint;
|
||||
|
||||
/// Result of path traversability check.
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct PathCheckResult {
|
||||
/// Is the entire path clear?
|
||||
pub is_clear: bool,
|
||||
/// Position where the path is blocked (if blocked).
|
||||
pub blocked_at: Option<WorldPoint>,
|
||||
/// Furthest safe position along the path.
|
||||
pub furthest_safe: Option<WorldPoint>,
|
||||
/// Distance that is traversable.
|
||||
pub distance_traversable: f32,
|
||||
}
|
||||
|
||||
impl PathCheckResult {
|
||||
/// Percentage of path that is traversable (0.0 to 1.0).
|
||||
pub fn traversable_ratio(&self, total_length: f32) -> f32 {
|
||||
if total_length <= 0.0 {
|
||||
return 0.0;
|
||||
}
|
||||
(self.distance_traversable / total_length).min(1.0)
|
||||
}
|
||||
}
|
||||
@@ -1,47 +0,0 @@
|
||||
//! SLAM (Simultaneous Localization and Mapping) module.
|
||||
//!
|
||||
//! This module provides scan-to-map matching and loop closure capabilities
|
||||
//! for correcting encoder drift during autonomous exploration.
|
||||
//!
|
||||
//! # Components
|
||||
//!
|
||||
//! - **Correlative Matcher**: Brute-force search for best scan alignment
|
||||
//! - **Loop Closure**: Detects revisited locations using LiDAR-IRIS descriptors
|
||||
//! - **Pose Graph**: Stores and optimizes pose constraints
|
||||
//!
|
||||
//! # Usage
|
||||
//!
|
||||
//! ```ignore
|
||||
//! use vastu_map::slam::{CorrelativeMatcher, CorrelativeMatcherConfig};
|
||||
//!
|
||||
//! let config = CorrelativeMatcherConfig::default();
|
||||
//! let matcher = CorrelativeMatcher::new(config);
|
||||
//!
|
||||
//! // Match scan against current map
|
||||
//! let result = matcher.match_scan(&scan, encoder_pose, &storage);
|
||||
//! if result.converged {
|
||||
//! // Use refined pose
|
||||
//! let corrected_pose = result.pose;
|
||||
//! }
|
||||
//! ```
|
||||
|
||||
mod background_optimizer;
|
||||
mod branch_bound;
|
||||
mod config;
|
||||
mod matcher;
|
||||
mod types;
|
||||
|
||||
pub mod loop_closure;
|
||||
|
||||
pub use background_optimizer::{
|
||||
BackgroundOptimizer, BackgroundOptimizerConfig, OptimizationResult, OptimizerTrigger,
|
||||
PoseCorrection,
|
||||
};
|
||||
pub use branch_bound::{
|
||||
BranchBoundConfig, BranchBoundResult, NUM_LEVELS, PrecomputedGrids, branch_and_bound_match,
|
||||
branch_and_bound_match_scan, branch_and_bound_match_simd,
|
||||
};
|
||||
pub use config::CorrelativeMatcherConfig;
|
||||
pub use loop_closure::{LoopClosureConfig, LoopClosureDetector, PoseGraphConfig};
|
||||
pub use matcher::{CorrelativeMatcher, ScratchBuffers};
|
||||
pub use types::{MatchQuality, ScanMatchResult};
|
||||
@@ -1,8 +1,8 @@
|
||||
[package]
|
||||
name = "vastu-map"
|
||||
version = "0.2.0"
|
||||
name = "vastu-slam"
|
||||
version = "0.3.0"
|
||||
edition = "2024"
|
||||
description = "Occupancy grid mapping library for robot exploration and navigation"
|
||||
description = "2D SLAM library with occupancy grid mapping, scan matching, and loop closure"
|
||||
license = "MIT"
|
||||
|
||||
# Note: This crate requires nightly Rust for portable_simd feature
|
||||
@@ -15,15 +15,7 @@ serde_yaml = "0.9" # YAML config files
|
||||
[dev-dependencies]
|
||||
env_logger = "0.11" # Test logging
|
||||
approx = "0.5" # Approximate comparisons (tests only)
|
||||
# Simulation dependencies (examples only)
|
||||
sangam-io = { path = "../sangam-io", features = ["mock"] }
|
||||
image = { version = "0.25", default-features = false, features = ["png", "pnm"] }
|
||||
clap = { version = "4.5", features = ["derive"] }
|
||||
rand = "0.9"
|
||||
|
||||
[[example]]
|
||||
name = "explore_mock"
|
||||
path = "examples/explore_mock.rs"
|
||||
|
||||
[profile.release]
|
||||
opt-level = 3
|
||||
@@ -0,0 +1,423 @@
|
||||
# VastuSLAM
|
||||
|
||||
A high-performance 2D SLAM library for robot vacuum cleaners, featuring multi-sensor occupancy grid mapping, scan matching, and loop closure.
|
||||
|
||||
## Features
|
||||
|
||||
- **Multi-Sensor Occupancy Grid**: Semantic cell types (Floor, Wall, Cliff, Bump) with Bayesian probability updates
|
||||
- **Scan Matching**: Correlative and branch-and-bound scan-to-map alignment with Gauss-Newton refinement
|
||||
- **Submap Architecture**: Cartographer-inspired reversible map updates for loop closure correction
|
||||
- **Loop Closure**: LiDAR-IRIS descriptors with pose graph optimization
|
||||
- **SIMD Optimized**: Explicit `std::simd` operations with `f32x4` for portable SIMD (ARM NEON, x86 SSE/AVX)
|
||||
- **SoA Data Layout**: Structure-of-Arrays for cache-friendly, SIMD-efficient memory access
|
||||
|
||||
## Quick Start
|
||||
|
||||
```rust
|
||||
use vastu_slam::{OccupancyGridMap, MapConfig, Pose2D, LidarScan};
|
||||
|
||||
// Create map with default configuration (800×800 cells, 2.5cm resolution = 20m×20m)
|
||||
let config = MapConfig::default();
|
||||
let mut map = OccupancyGridMap::new(config);
|
||||
|
||||
// Process lidar scan
|
||||
let pose = Pose2D::new(0.0, 0.0, 0.0);
|
||||
let scan = LidarScan::new(ranges, angles, 0.15, 8.0);
|
||||
let result = map.observe_lidar(&scan, pose);
|
||||
|
||||
println!("Updated {} cells", result.cells_updated);
|
||||
```
|
||||
|
||||
## System Architecture
|
||||
|
||||
```
|
||||
┌─────────────────────────────────────────────────────────────────────────────────┐
|
||||
│ VastuSLAM Pipeline │
|
||||
├─────────────────────────────────────────────────────────────────────────────────┤
|
||||
│ │
|
||||
│ Sensors SLAM Core Output │
|
||||
│ ─────── ───────── ────── │
|
||||
│ │
|
||||
│ ┌─────────┐ ┌─────────────────────────────────────────────┐ ┌─────────┐ │
|
||||
│ │ LiDAR │────▶│ Scan Matching │──▶│ Pose │ │
|
||||
│ │ 360° │ │ ┌──────────────────────────────────────┐ │ │ Output │ │
|
||||
│ └─────────┘ │ │ Coarse: Correlative / Branch-Bound │ │ └─────────┘ │
|
||||
│ │ │ Fine: Gauss-Newton + Distance Field │ │ │
|
||||
│ ┌─────────┐ │ └──────────────────────────────────────┘ │ ┌─────────┐ │
|
||||
│ │ Cliff │────▶│ │ │──▶│ Map │ │
|
||||
│ │ Sensors │ │ ▼ │ │ .vastu │ │
|
||||
│ └─────────┘ │ ┌──────────────────────────────────────┐ │ │ .pgm │ │
|
||||
│ │ │ Occupancy Grid │ │ └─────────┘ │
|
||||
│ ┌─────────┐ │ │ • Log-odds Bayesian updates │ │ │
|
||||
│ │ Bumper │────▶│ │ • Priority: Bump > Cliff > Wall │ │ ┌─────────┐ │
|
||||
│ │ Sensors │ │ │ • Bresenham ray casting │ │──▶│ Paths │ │
|
||||
│ └─────────┘ │ └──────────────────────────────────────┘ │ │ A* │ │
|
||||
│ │ │ │ └─────────┘ │
|
||||
│ ┌─────────┐ │ ▼ │ │
|
||||
│ │ Encoder │────▶│ ┌──────────────────────────────────────┐ │ │
|
||||
│ │ Odom │ │ │ Submap Manager │ │ │
|
||||
│ └─────────┘ │ │ • Local grid per submap │ │ │
|
||||
│ │ │ • Stored scans for regeneration │ │ │
|
||||
│ │ │ • Origin-only optimization │ │ │
|
||||
│ │ └──────────────────────────────────────┘ │ │
|
||||
│ │ │ │ │
|
||||
│ │ ▼ │ │
|
||||
│ │ ┌──────────────────────────────────────┐ │ │
|
||||
│ │ │ Loop Closure │ │ │
|
||||
│ │ │ • LiDAR-IRIS descriptors │ │ │
|
||||
│ │ │ • Pose graph optimization │ │ │
|
||||
│ │ │ • Submap origin correction │ │ │
|
||||
│ │ └──────────────────────────────────────┘ │ │
|
||||
│ └─────────────────────────────────────────────┘ │
|
||||
│ │
|
||||
└─────────────────────────────────────────────────────────────────────────────────┘
|
||||
```
|
||||
|
||||
## Core Algorithms
|
||||
|
||||
### 1. Occupancy Grid with Log-Odds
|
||||
|
||||
VastuSLAM uses a Bayesian occupancy grid with log-odds representation for numerically stable probability updates:
|
||||
|
||||
```
|
||||
Log-Odds Update:
|
||||
L(cell | observation) = L(cell) + L(sensor_model)
|
||||
|
||||
Where:
|
||||
• L_hit = +20 (observation confirms obstacle)
|
||||
• L_miss = -4 (observation confirms free space)
|
||||
• L_occupied_threshold = +50 → classify as Wall
|
||||
• L_free_threshold = -50 → classify as Floor
|
||||
```
|
||||
|
||||
The asymmetric weights (20 vs -4) mean a single hit strongly indicates an obstacle, while multiple passes are needed to clear a false positive.
|
||||
|
||||
### 2. Multi-Resolution Scan Matching
|
||||
|
||||
Scan matching corrects encoder drift by aligning LiDAR scans to the existing map:
|
||||
|
||||
```
|
||||
Pipeline:
|
||||
┌───────────────────────────────────────────────────────────────────┐
|
||||
│ Step 1: COARSE SEARCH │
|
||||
│ • Window: ±30cm × ±30cm × ±8.6° │
|
||||
│ • Resolution: 4cm linear, 2.3° angular │
|
||||
│ • Method: Brute-force or branch-and-bound │
|
||||
│ • Scoring: Gaussian distance field (smooth gradients) │
|
||||
├───────────────────────────────────────────────────────────────────┤
|
||||
│ Step 2: FINE SEARCH │
|
||||
│ • Window: ±6cm × ±6cm × ±3.4° │
|
||||
│ • Resolution: 2cm linear, 1.1° angular │
|
||||
│ • Prior-penalized scoring │
|
||||
├───────────────────────────────────────────────────────────────────┤
|
||||
│ Step 3: GAUSS-NEWTON REFINEMENT │
|
||||
│ • Adaptive Levenberg-Marquardt │
|
||||
│ • Sub-millimeter accuracy │
|
||||
│ • Convergence: cost change < 1e-6 │
|
||||
└───────────────────────────────────────────────────────────────────┘
|
||||
```
|
||||
|
||||
**Gaussian Scoring** provides smooth gradients for optimization:
|
||||
```
|
||||
Score(point) = exp(-d² / 2σ²)
|
||||
|
||||
Where d = distance to nearest wall, σ = 0.10m (typical)
|
||||
```
|
||||
|
||||
### 3. Submap Architecture
|
||||
|
||||
Inspired by Google Cartographer, VastuSLAM uses submaps for reversible map updates:
|
||||
|
||||
```
|
||||
┌───────────────────────────────────────────────────────────────┐
|
||||
│ World Frame │
|
||||
│ │
|
||||
│ Submap 0 Submap 1 Submap 2 │
|
||||
│ ┌─────────┐ ┌─────────┐ ┌─────────┐ │
|
||||
│ │ Local │ ──T01──▶ │ Local │ ──T12──▶ │ Local │ │
|
||||
│ │ Grid │ │ Grid │ │ Grid │ │
|
||||
│ │ [scans] │ │ [scans] │ │ [scans] │ │
|
||||
│ └────┬────┘ └────┬────┘ └────┬────┘ │
|
||||
│ ▼ ▼ ▼ │
|
||||
│ origin_0 origin_1 origin_2 │
|
||||
│ (fixed) (adjustable) (adjustable) │
|
||||
└───────────────────────────────────────────────────────────────┘
|
||||
|
||||
Key Insight: Only submap origins change during optimization.
|
||||
Local grids regenerate from stored scans.
|
||||
```
|
||||
|
||||
**Submap Lifecycle:**
|
||||
1. **Active**: Receives scans, builds local grid
|
||||
2. **Filling**: Overlap period (scans go to both old and new submap)
|
||||
3. **Finalized**: No more updates, eligible for loop closure
|
||||
|
||||
### 4. Loop Closure Detection
|
||||
|
||||
Uses LiDAR-IRIS descriptors for efficient place recognition:
|
||||
|
||||
```
|
||||
LiDAR-IRIS Pipeline:
|
||||
1. Project scan → radial signature (range histogram by angle)
|
||||
2. Extract features via Fourier transform
|
||||
3. Create binary descriptor (rotation-invariant)
|
||||
4. Compare using Hamming distance (fast bit operations)
|
||||
5. Geometric validation with scan matching
|
||||
6. Pose graph optimization if validated
|
||||
```
|
||||
|
||||
### 5. Cell Type Priority
|
||||
|
||||
Multiple sensors update the same cells. Priority ensures correct classification:
|
||||
|
||||
```
|
||||
Priority (highest to lowest):
|
||||
┌─────────────────────────────────────────────────────┐
|
||||
│ 4. Bump (invisible obstacle - glass, chair legs) │ ← Bumper sensor
|
||||
├─────────────────────────────────────────────────────┤
|
||||
│ 3. Cliff (floor drop-off - stairs, ledges) │ ← Cliff sensor
|
||||
├─────────────────────────────────────────────────────┤
|
||||
│ 2. Wall (visible obstacle) │ ← LiDAR endpoint
|
||||
├─────────────────────────────────────────────────────┤
|
||||
│ 1. Floor (traversable) │ ← LiDAR ray
|
||||
├─────────────────────────────────────────────────────┤
|
||||
│ 0. Unknown (not observed) │ ← Initial state
|
||||
└─────────────────────────────────────────────────────┘
|
||||
```
|
||||
|
||||
## Module Documentation
|
||||
|
||||
| Module | Description | Documentation |
|
||||
|--------|-------------|---------------|
|
||||
| [`core`](src/core/) | Fundamental types: Pose2D, WorldPoint, GridCoord, LidarScan, Cell | [README](src/core/README.md) |
|
||||
| [`grid`](src/grid/) | Occupancy grid storage with SoA layout, ray casting, sensor updates | [README](src/grid/README.md) |
|
||||
| [`matching`](src/matching/) | Scan matching (correlative, branch-bound), loop closure, pose graph | [README](src/matching/README.md) |
|
||||
| [`submap`](src/submap/) | Submap manager, pose graph, multi-submap matching | [README](src/submap/README.md) |
|
||||
| [`config`](src/config/) | YAML configuration loading with sensible defaults | [README](src/config/README.md) |
|
||||
| [`io`](src/io/) | Persistence: native .vastu format, ROS .pgm export, SVG visualization | [README](src/io/README.md) |
|
||||
| [`modes`](src/modes/) | Operation modes: localization-only (no map updates) | [README](src/modes/README.md) |
|
||||
|
||||
## Coordinate System
|
||||
|
||||
All coordinates follow the ROS REP-103 convention:
|
||||
|
||||
```
|
||||
+X (Forward)
|
||||
▲
|
||||
│
|
||||
│
|
||||
+Y (Left) ◄─────┼───── -Y (Right)
|
||||
│
|
||||
│
|
||||
▼
|
||||
-X (Backward)
|
||||
|
||||
Rotation: Counter-clockwise positive (θ = 0 points to +X)
|
||||
```
|
||||
|
||||
## Configuration
|
||||
|
||||
### YAML Configuration
|
||||
|
||||
```yaml
|
||||
# config.yaml
|
||||
grid:
|
||||
resolution: 0.025 # 2.5cm cells
|
||||
initial_width: 800 # 20m × 20m
|
||||
initial_height: 800
|
||||
|
||||
sensor:
|
||||
robot:
|
||||
radius: 0.17 # Robot radius for collision
|
||||
lidar:
|
||||
offset_x: -0.110 # LiDAR position relative to robot center
|
||||
max_range: 8.0
|
||||
|
||||
slam:
|
||||
correlative:
|
||||
enabled: true
|
||||
search_x: 0.30 # ±30cm search window
|
||||
search_y: 0.30
|
||||
search_theta: 0.15 # ±8.6° search window
|
||||
multi_resolution: true
|
||||
use_gaussian_scoring: true
|
||||
|
||||
loop_closure:
|
||||
enabled: true
|
||||
min_score_threshold: 0.6
|
||||
```
|
||||
|
||||
### Programmatic Configuration
|
||||
|
||||
```rust
|
||||
use vastu_slam::config::VastuConfig;
|
||||
use std::path::Path;
|
||||
|
||||
// Load from YAML file
|
||||
let config = VastuConfig::load(Path::new("config.yaml"))?;
|
||||
|
||||
// Or use defaults
|
||||
let config = VastuConfig::default();
|
||||
|
||||
// Convert to runtime configs
|
||||
let map_config = config.to_map_config();
|
||||
let matcher_config = config.correlative_matcher_config();
|
||||
```
|
||||
|
||||
## Memory Layout
|
||||
|
||||
VastuSLAM uses Structure-of-Arrays (SoA) for SIMD efficiency:
|
||||
|
||||
```
|
||||
Traditional AoS (Array of Structs):
|
||||
┌──────────────────────────────────────────────────┐
|
||||
│ Cell0{type,conf,swept} │ Cell1{...} │ Cell2{...} │ ...
|
||||
└──────────────────────────────────────────────────┘
|
||||
↑ Cache line may span multiple cells, mixed data types
|
||||
|
||||
VastuSLAM SoA (Struct of Arrays):
|
||||
┌───────────────────────────────────────┐
|
||||
│ types: [t0, t1, t2, t3, t4, t5, ...] │ ← f32x4 SIMD loads
|
||||
├───────────────────────────────────────┤
|
||||
│ confs: [c0, c1, c2, c3, c4, c5, ...] │ ← Contiguous memory
|
||||
├───────────────────────────────────────┤
|
||||
│ swept: [s0, s1, s2, s3, s4, s5, ...] │ ← Cache-friendly
|
||||
└───────────────────────────────────────┘
|
||||
```
|
||||
|
||||
## Performance Characteristics
|
||||
|
||||
| Metric | Typical Value | Notes |
|
||||
|--------|---------------|-------|
|
||||
| Binary size | ~100KB | Stripped, statically linked |
|
||||
| Memory (800×800) | ~4MB | SoA grid storage |
|
||||
| Scan matching | 5-15ms | ARM Cortex-A7 @ 1GHz |
|
||||
| Map save/load | <10ms | Native .vastu format |
|
||||
| Resolution | 2.5cm | Default, configurable |
|
||||
|
||||
## Building
|
||||
|
||||
### Development
|
||||
|
||||
```bash
|
||||
# Requires nightly Rust for portable_simd
|
||||
rustup override set nightly
|
||||
|
||||
# Build and test
|
||||
cargo build --release
|
||||
cargo test
|
||||
```
|
||||
|
||||
### Cross-Compilation (ARM)
|
||||
|
||||
```bash
|
||||
rustup target add armv7-unknown-linux-musleabihf
|
||||
cargo build --release --target armv7-unknown-linux-musleabihf
|
||||
```
|
||||
|
||||
### Documentation
|
||||
|
||||
```bash
|
||||
cargo +nightly doc --no-deps --open
|
||||
```
|
||||
|
||||
## Usage Examples
|
||||
|
||||
### Basic Mapping
|
||||
|
||||
```rust
|
||||
use vastu_slam::{OccupancyGridMap, MapConfig, Pose2D, LidarScan, SensorObservation};
|
||||
|
||||
let mut map = OccupancyGridMap::new(MapConfig::default());
|
||||
|
||||
// Process complete sensor observation
|
||||
let observation = SensorObservation {
|
||||
pose: Pose2D::new(0.0, 0.0, 0.0),
|
||||
lidar: Some(scan),
|
||||
cliffs: cliff_sensors,
|
||||
bumpers: bumper_sensors,
|
||||
timestamp_us: 0,
|
||||
};
|
||||
|
||||
let result = map.observe(&observation);
|
||||
println!("Floor: {}, Wall: {}, Cliff: {}, Bump: {}",
|
||||
result.cells_floor, result.cells_wall,
|
||||
result.cells_cliff, result.cells_bump);
|
||||
```
|
||||
|
||||
### Scan Matching with Drift Correction
|
||||
|
||||
```rust
|
||||
use vastu_slam::{OccupancyGridMap, CorrelativeMatcherConfig};
|
||||
|
||||
let mut map = OccupancyGridMap::new(MapConfig::default());
|
||||
let matcher_config = CorrelativeMatcherConfig::default();
|
||||
|
||||
// Match scan and integrate at corrected pose
|
||||
let (result, corrected_pose, match_result) =
|
||||
map.observe_lidar_with_matching(&scan, encoder_pose, &matcher_config);
|
||||
|
||||
if match_result.converged {
|
||||
println!("Corrected pose: ({:.3}, {:.3})",
|
||||
corrected_pose.x, corrected_pose.y);
|
||||
}
|
||||
```
|
||||
|
||||
### Localization on Known Map
|
||||
|
||||
```rust
|
||||
use vastu_slam::modes::{Localizer, LocalizerConfig};
|
||||
use vastu_slam::io::load_vastu;
|
||||
|
||||
// Load pre-built map
|
||||
let map = load_vastu(Path::new("map.vastu"))?;
|
||||
|
||||
// Create localizer
|
||||
let mut localizer = Localizer::new(map, LocalizerConfig::default());
|
||||
localizer.set_initial_pose(dock_pose);
|
||||
|
||||
// Localization loop
|
||||
let result = localizer.localize(&scan, Some(odom_delta));
|
||||
if result.converged {
|
||||
println!("Robot at: ({:.2}, {:.2})", result.pose.x, result.pose.y);
|
||||
}
|
||||
```
|
||||
|
||||
### Submap-Based SLAM
|
||||
|
||||
```rust
|
||||
use vastu_slam::submap::{SubmapManager, SubmapConfig};
|
||||
|
||||
let mut manager = SubmapManager::new(SubmapConfig::default(), &map_config);
|
||||
|
||||
// Insert scans (manager handles submap transitions)
|
||||
let result = manager.insert_scan(&scan, pose, timestamp_us);
|
||||
if result.new_submap_created {
|
||||
println!("Started new submap");
|
||||
}
|
||||
|
||||
// Get composed global grid
|
||||
let global_grid = manager.global_grid();
|
||||
```
|
||||
|
||||
## File Formats
|
||||
|
||||
| Format | Extension | Purpose | Preserves |
|
||||
|--------|-----------|---------|-----------|
|
||||
| Native | `.vastu` | Full persistence | All cell data (type, confidence, swept) |
|
||||
| ROS Map | `.pgm` + `.yaml` | ROS integration | Occupancy only (Free/Occupied/Unknown) |
|
||||
| SVG | `.svg` | Visualization | Visual map + trajectory overlay |
|
||||
|
||||
## Thread Safety
|
||||
|
||||
| Type | Thread Safety | Typical Usage |
|
||||
|------|---------------|---------------|
|
||||
| `OccupancyGridMap` | `Send` | Single-threaded map owner |
|
||||
| `GridStorage` | `Send` | Single-threaded access |
|
||||
| `CorrelativeMatcher` | `Send + Sync` | Shared across threads |
|
||||
| `SubmapManager` | `Send` | Single-threaded manager |
|
||||
| `Localizer` | `Send` | Single-threaded localizer |
|
||||
|
||||
## License
|
||||
|
||||
MIT
|
||||
@@ -0,0 +1,310 @@
|
||||
# Config Module
|
||||
|
||||
Unified configuration loading for VastuSLAM from YAML files.
|
||||
|
||||
## Module Structure
|
||||
|
||||
```
|
||||
config/
|
||||
├── mod.rs # Module exports
|
||||
├── vastu.rs # VastuConfig - main configuration struct
|
||||
├── grid.rs # GridSection - grid dimensions and origin
|
||||
├── sensor.rs # SensorSection - robot geometry and sensors
|
||||
├── slam.rs # SlamSection - SLAM algorithm settings
|
||||
├── persistence.rs # PersistenceSection - output settings
|
||||
├── defaults.rs # Default value functions for serde
|
||||
└── error.rs # ConfigLoadError enum
|
||||
```
|
||||
|
||||
## Configuration Hierarchy
|
||||
|
||||
```
|
||||
VastuConfig
|
||||
├── grid: GridSection
|
||||
│ ├── resolution (0.025m)
|
||||
│ ├── initial_width (800 cells)
|
||||
│ ├── initial_height (800 cells)
|
||||
│ ├── origin_mode ("center" | "corner")
|
||||
│ └── auto_expand (true)
|
||||
│
|
||||
├── sensor: SensorSection
|
||||
│ ├── lidar: LidarSettings
|
||||
│ │ ├── offset_x (-0.110m)
|
||||
│ │ ├── offset_y (0.0m)
|
||||
│ │ ├── min_range (0.15m)
|
||||
│ │ └── max_range (8.0m)
|
||||
│ │
|
||||
│ ├── robot: RobotSettings
|
||||
│ │ └── radius (0.17m)
|
||||
│ │
|
||||
│ ├── cliff_sensors: CliffSensorSettings
|
||||
│ │ ├── left_side [0.12, 0.10]
|
||||
│ │ ├── left_front [0.15, 0.05]
|
||||
│ │ ├── right_front [0.15, -0.05]
|
||||
│ │ └── right_side [0.12, -0.10]
|
||||
│ │
|
||||
│ └── bumper: BumperSettings
|
||||
│ ├── left_angle (0.5 rad)
|
||||
│ └── right_angle (-0.5 rad)
|
||||
│
|
||||
├── slam: SlamSection
|
||||
│ ├── log_odds: LogOddsConfig
|
||||
│ ├── correlative: CorrelativeMatcherConfig
|
||||
│ ├── loop_closure: LoopClosureConfig
|
||||
│ ├── pose_graph: PoseGraphConfig
|
||||
│ └── background_optimizer: BackgroundOptimizerConfig
|
||||
│
|
||||
└── persistence: PersistenceSection
|
||||
├── output_format ("both")
|
||||
├── output_dir ("./output")
|
||||
└── auto_save_interval (30s)
|
||||
```
|
||||
|
||||
## Usage Examples
|
||||
|
||||
### Loading Configuration
|
||||
|
||||
```rust
|
||||
use vastu_slam::config::VastuConfig;
|
||||
use std::path::Path;
|
||||
|
||||
// Load from specific file
|
||||
let config = VastuConfig::load(Path::new("config.yaml"))?;
|
||||
|
||||
// Load from default path (configs/config.yaml)
|
||||
let config = VastuConfig::load_default()?;
|
||||
|
||||
// Parse from YAML string
|
||||
let yaml = r#"
|
||||
grid:
|
||||
resolution: 0.025
|
||||
initial_width: 1000
|
||||
sensor:
|
||||
robot:
|
||||
radius: 0.17
|
||||
"#;
|
||||
let config = VastuConfig::from_yaml(yaml)?;
|
||||
|
||||
// Use defaults (no file needed)
|
||||
let config = VastuConfig::default();
|
||||
```
|
||||
|
||||
### Accessing Settings
|
||||
|
||||
```rust
|
||||
let config = VastuConfig::default();
|
||||
|
||||
// Direct access to sections
|
||||
let resolution = config.grid.resolution;
|
||||
let robot_radius = config.sensor.robot.radius;
|
||||
let lidar_offset = config.sensor.lidar.offset_x;
|
||||
|
||||
// Convenient methods
|
||||
let radius = config.robot_radius();
|
||||
|
||||
// Convert to runtime configs
|
||||
let map_config = config.to_map_config();
|
||||
let matcher_config = config.correlative_matcher_config();
|
||||
let loop_config = config.loop_closure_config();
|
||||
```
|
||||
|
||||
## YAML Configuration File
|
||||
|
||||
### Complete Example
|
||||
|
||||
```yaml
|
||||
# config.yaml - VastuSLAM Configuration
|
||||
|
||||
# Grid settings
|
||||
grid:
|
||||
resolution: 0.025 # Cell size in meters (2.5cm)
|
||||
initial_width: 800 # Initial grid width in cells (20m)
|
||||
initial_height: 800 # Initial grid height in cells (20m)
|
||||
origin_mode: center # "center" or "corner"
|
||||
auto_expand: true # Grow grid as needed
|
||||
|
||||
# Sensor settings
|
||||
sensor:
|
||||
lidar:
|
||||
offset_x: -0.110 # LiDAR offset from robot center (m)
|
||||
offset_y: 0.0 # LiDAR lateral offset (m)
|
||||
min_range: 0.15 # Minimum valid range (m)
|
||||
max_range: 8.0 # Maximum valid range (m)
|
||||
|
||||
robot:
|
||||
radius: 0.17 # Robot radius for collision checking (m)
|
||||
|
||||
cliff_sensors: # Positions in robot frame [x, y]
|
||||
left_side: [0.12, 0.10]
|
||||
left_front: [0.15, 0.05]
|
||||
right_front: [0.15, -0.05]
|
||||
right_side: [0.12, -0.10]
|
||||
|
||||
bumper:
|
||||
left_angle: 0.5 # Left bumper angle from forward (rad)
|
||||
right_angle: -0.5 # Right bumper angle from forward (rad)
|
||||
|
||||
# SLAM settings (Cartographer-style)
|
||||
slam:
|
||||
log_odds:
|
||||
l_hit: 20 # Log-odds for obstacle hit
|
||||
l_miss: -4 # Log-odds for free space
|
||||
l_occupied_threshold: 50 # Wall classification threshold
|
||||
l_free_threshold: -50 # Floor classification threshold
|
||||
|
||||
correlative:
|
||||
enabled: true
|
||||
search_x: 0.30 # X search window (m)
|
||||
search_y: 0.30 # Y search window (m)
|
||||
search_theta: 0.15 # Theta search window (rad)
|
||||
linear_resolution: 0.02 # Search step (m)
|
||||
angular_resolution: 0.02 # Search step (rad)
|
||||
multi_resolution: true
|
||||
use_gaussian_scoring: true
|
||||
gaussian_sigma: 0.10
|
||||
|
||||
loop_closure:
|
||||
enabled: true
|
||||
min_score_threshold: 0.6
|
||||
min_distance_threshold: 5.0
|
||||
descriptor_threshold: 100
|
||||
|
||||
pose_graph:
|
||||
max_iterations: 100
|
||||
convergence_threshold: 1e-5
|
||||
odometry_weight: 10.0
|
||||
loop_weight: 50.0
|
||||
|
||||
background_optimizer:
|
||||
max_iterations: 50
|
||||
use_background_thread: false
|
||||
|
||||
# Output settings
|
||||
persistence:
|
||||
output_format: both # "vastu", "pgm", or "both"
|
||||
output_dir: ./output
|
||||
auto_save_interval: 30 # Seconds (0 = disabled)
|
||||
```
|
||||
|
||||
### Minimal Example
|
||||
|
||||
```yaml
|
||||
# minimal.yaml - Only override what you need
|
||||
grid:
|
||||
resolution: 0.05 # 5cm cells for faster processing
|
||||
|
||||
sensor:
|
||||
robot:
|
||||
radius: 0.20 # Larger robot
|
||||
```
|
||||
|
||||
All missing values use sensible defaults.
|
||||
|
||||
## Configuration Sections
|
||||
|
||||
### GridSection
|
||||
|
||||
Controls the occupancy grid dimensions:
|
||||
|
||||
| Parameter | Default | Description |
|
||||
|-----------|---------|-------------|
|
||||
| `resolution` | 0.025 | Cell size in meters (2.5cm) |
|
||||
| `initial_width` | 800 | Grid width in cells (20m) |
|
||||
| `initial_height` | 800 | Grid height in cells (20m) |
|
||||
| `origin_mode` | "center" | Grid origin placement |
|
||||
| `auto_expand` | true | Grow grid as needed |
|
||||
|
||||
**Origin Modes:**
|
||||
- `"center"`: Robot starts at grid center
|
||||
- `"corner"`: Robot starts at grid corner (origin_x, origin_y)
|
||||
|
||||
### SensorSection
|
||||
|
||||
Robot geometry and sensor positions:
|
||||
|
||||
| Subsection | Parameter | Default | Description |
|
||||
|------------|-----------|---------|-------------|
|
||||
| lidar | offset_x | -0.110 | LiDAR X offset from center (m) |
|
||||
| lidar | offset_y | 0.0 | LiDAR Y offset from center (m) |
|
||||
| lidar | min_range | 0.15 | Minimum valid range (m) |
|
||||
| lidar | max_range | 8.0 | Maximum valid range (m) |
|
||||
| robot | radius | 0.17 | Robot radius (m) |
|
||||
|
||||
**CRL-200S Default Positions:**
|
||||
```
|
||||
Front
|
||||
LF ─────── RF
|
||||
╲ ↑ ╱
|
||||
╲ │ ╱
|
||||
LS ──┬──┼──┬── RS Cliff sensors
|
||||
│ │
|
||||
L ─┴─────┴─ R Bumpers (left: +28°, right: -28°)
|
||||
│
|
||||
LIDAR (-11cm from center)
|
||||
```
|
||||
|
||||
### SlamSection
|
||||
|
||||
SLAM algorithm parameters:
|
||||
|
||||
| Subsection | Description |
|
||||
|------------|-------------|
|
||||
| `log_odds` | Bayesian occupancy update parameters |
|
||||
| `correlative` | Scan matcher search window and resolution |
|
||||
| `loop_closure` | Loop detection thresholds |
|
||||
| `pose_graph` | Graph optimization settings |
|
||||
| `background_optimizer` | Async optimization settings |
|
||||
|
||||
### PersistenceSection
|
||||
|
||||
Output settings:
|
||||
|
||||
| Parameter | Default | Description |
|
||||
|-----------|---------|-------------|
|
||||
| `output_format` | "both" | "vastu", "pgm", or "both" |
|
||||
| `output_dir` | "./output" | Output directory path |
|
||||
| `auto_save_interval` | 30 | Auto-save interval in seconds |
|
||||
|
||||
## Conversion to Runtime Configs
|
||||
|
||||
The YAML configuration is converted to typed runtime configs:
|
||||
|
||||
```rust
|
||||
let config = VastuConfig::load(path)?;
|
||||
|
||||
// For OccupancyGridMap
|
||||
let map_config: MapConfig = config.to_map_config();
|
||||
|
||||
// For CorrelativeMatcher
|
||||
let matcher: CorrelativeMatcherConfig = config.correlative_matcher_config();
|
||||
|
||||
// For LoopClosureDetector
|
||||
let loop_config: LoopClosureConfig = config.loop_closure_config();
|
||||
|
||||
// For PoseGraph
|
||||
let graph_config: PoseGraphConfig = config.pose_graph_config();
|
||||
|
||||
// For BackgroundOptimizer
|
||||
let opt_config: BackgroundOptimizerConfig = config.background_optimizer_config();
|
||||
```
|
||||
|
||||
## Error Handling
|
||||
|
||||
```rust
|
||||
use vastu_slam::config::{VastuConfig, ConfigLoadError};
|
||||
|
||||
match VastuConfig::load(path) {
|
||||
Ok(config) => { /* use config */ }
|
||||
Err(ConfigLoadError::Io(msg)) => {
|
||||
eprintln!("Failed to read file: {}", msg);
|
||||
}
|
||||
Err(ConfigLoadError::Parse(msg)) => {
|
||||
eprintln!("Invalid YAML: {}", msg);
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
## Thread Safety
|
||||
|
||||
`VastuConfig` and all section types are `Clone + Send + Sync`.
|
||||
Configuration is typically loaded once at startup and shared immutably.
|
||||
@@ -0,0 +1,49 @@
|
||||
//! Default value functions for serde deserialization.
|
||||
|
||||
pub fn resolution() -> f32 {
|
||||
0.025
|
||||
}
|
||||
|
||||
pub fn grid_size() -> usize {
|
||||
800
|
||||
}
|
||||
|
||||
pub fn origin_mode() -> String {
|
||||
"center".to_string()
|
||||
}
|
||||
|
||||
pub fn enabled() -> bool {
|
||||
true
|
||||
}
|
||||
|
||||
pub fn lidar_offset_x() -> f32 {
|
||||
-0.110
|
||||
}
|
||||
|
||||
pub fn min_range() -> f32 {
|
||||
0.15
|
||||
}
|
||||
|
||||
pub fn max_range() -> f32 {
|
||||
8.0
|
||||
}
|
||||
|
||||
pub fn robot_radius() -> f32 {
|
||||
0.17
|
||||
}
|
||||
|
||||
pub fn left_bumper_angle() -> f32 {
|
||||
0.5
|
||||
}
|
||||
|
||||
pub fn right_bumper_angle() -> f32 {
|
||||
-0.5
|
||||
}
|
||||
|
||||
pub fn output_format() -> String {
|
||||
"both".to_string()
|
||||
}
|
||||
|
||||
pub fn output_dir() -> String {
|
||||
"./output".to_string()
|
||||
}
|
||||
@@ -0,0 +1,68 @@
|
||||
//! Unified configuration loading for VastuSLAM.
|
||||
//!
|
||||
//! Loads all configuration from a single YAML file with sensible defaults.
|
||||
//!
|
||||
//! ## Quick Start
|
||||
//!
|
||||
//! ```rust,ignore
|
||||
//! use vastu_slam::config::VastuConfig;
|
||||
//!
|
||||
//! // Load from default path (configs/config.yaml)
|
||||
//! let config = VastuConfig::load_default()?;
|
||||
//!
|
||||
//! // Or use built-in defaults (no file needed)
|
||||
//! let config = VastuConfig::default();
|
||||
//!
|
||||
//! // Convert to runtime configs
|
||||
//! let map_config = config.to_map_config();
|
||||
//! let matcher_config = config.correlative_matcher_config();
|
||||
//! ```
|
||||
//!
|
||||
//! ## Configuration Sections
|
||||
//!
|
||||
//! | Section | Description |
|
||||
//! |---------|-------------|
|
||||
//! | [`GridSection`] | Grid dimensions, resolution, origin |
|
||||
//! | [`SensorSection`] | Robot geometry, LiDAR, cliff, bumper |
|
||||
//! | [`SlamSection`] | Matcher, loop closure, pose graph settings |
|
||||
//! | [`PersistenceSection`] | Output format and directory |
|
||||
//!
|
||||
//! ## Example YAML
|
||||
//!
|
||||
//! ```yaml
|
||||
//! grid:
|
||||
//! resolution: 0.025 # 2.5cm cells
|
||||
//! initial_width: 800 # 20m
|
||||
//!
|
||||
//! sensor:
|
||||
//! robot:
|
||||
//! radius: 0.17
|
||||
//! lidar:
|
||||
//! offset_x: -0.110
|
||||
//! max_range: 8.0
|
||||
//!
|
||||
//! slam:
|
||||
//! correlative:
|
||||
//! enabled: true
|
||||
//! multi_resolution: true
|
||||
//! ```
|
||||
|
||||
mod defaults;
|
||||
mod error;
|
||||
mod grid;
|
||||
mod persistence;
|
||||
mod sensor;
|
||||
mod slam;
|
||||
mod vastu;
|
||||
|
||||
// Re-export main types
|
||||
pub use error::ConfigLoadError;
|
||||
pub use vastu::VastuConfig;
|
||||
|
||||
// Re-export section types
|
||||
pub use grid::GridSection;
|
||||
pub use persistence::PersistenceSection;
|
||||
pub use sensor::{
|
||||
BumperSettings, CliffSensorSettings, LidarSettings, RobotSettings, SensorSection,
|
||||
};
|
||||
pub use slam::SlamSection;
|
||||
@@ -3,7 +3,7 @@
|
||||
use serde::{Deserialize, Serialize};
|
||||
|
||||
use crate::grid::LogOddsConfig;
|
||||
use crate::slam::{
|
||||
use crate::matching::{
|
||||
BackgroundOptimizerConfig, CorrelativeMatcherConfig, LoopClosureConfig, PoseGraphConfig,
|
||||
};
|
||||
|
||||
@@ -0,0 +1,122 @@
|
||||
//! Main VastuConfig and conversion methods.
|
||||
|
||||
use std::path::Path;
|
||||
|
||||
use serde::{Deserialize, Serialize};
|
||||
|
||||
use crate::grid::MapConfig;
|
||||
use crate::matching::{
|
||||
BackgroundOptimizerConfig, CorrelativeMatcherConfig, LoopClosureConfig, PoseGraphConfig,
|
||||
};
|
||||
|
||||
use super::error::ConfigLoadError;
|
||||
use super::grid::GridSection;
|
||||
use super::persistence::PersistenceSection;
|
||||
use super::sensor::SensorSection;
|
||||
use super::slam::SlamSection;
|
||||
|
||||
/// Full VastuSLAM configuration loaded from YAML
|
||||
#[derive(Clone, Debug, Serialize, Deserialize, Default)]
|
||||
pub struct VastuConfig {
|
||||
/// Grid settings
|
||||
#[serde(default)]
|
||||
pub grid: GridSection,
|
||||
|
||||
/// Sensor settings
|
||||
#[serde(default)]
|
||||
pub sensor: SensorSection,
|
||||
|
||||
/// SLAM settings (Cartographer-style defaults)
|
||||
#[serde(default)]
|
||||
pub slam: SlamSection,
|
||||
|
||||
/// Persistence settings
|
||||
#[serde(default)]
|
||||
pub persistence: PersistenceSection,
|
||||
}
|
||||
|
||||
impl VastuConfig {
|
||||
/// Load configuration from a YAML file
|
||||
pub fn load(path: &Path) -> Result<Self, ConfigLoadError> {
|
||||
let contents =
|
||||
std::fs::read_to_string(path).map_err(|e| ConfigLoadError::Io(e.to_string()))?;
|
||||
Self::from_yaml(&contents)
|
||||
}
|
||||
|
||||
/// Load from default config path (configs/config.yaml)
|
||||
pub fn load_default() -> Result<Self, ConfigLoadError> {
|
||||
let path = Path::new("configs/config.yaml");
|
||||
if path.exists() {
|
||||
Self::load(path)
|
||||
} else {
|
||||
Ok(Self::default())
|
||||
}
|
||||
}
|
||||
|
||||
/// Parse from YAML string
|
||||
pub fn from_yaml(yaml: &str) -> Result<Self, ConfigLoadError> {
|
||||
serde_yaml::from_str(yaml).map_err(|e| ConfigLoadError::Parse(e.to_string()))
|
||||
}
|
||||
|
||||
/// Convert to MapConfig for OccupancyGridMap
|
||||
pub fn to_map_config(&self) -> MapConfig {
|
||||
MapConfig {
|
||||
grid: self.grid.to_grid_config(),
|
||||
sensor: self.sensor.to_sensor_config(),
|
||||
log_odds: self.slam.log_odds.clone(),
|
||||
}
|
||||
}
|
||||
|
||||
/// Get the correlative matcher config
|
||||
pub fn correlative_matcher_config(&self) -> CorrelativeMatcherConfig {
|
||||
self.slam.correlative.clone()
|
||||
}
|
||||
|
||||
/// Get the loop closure config
|
||||
pub fn loop_closure_config(&self) -> LoopClosureConfig {
|
||||
self.slam.loop_closure.clone()
|
||||
}
|
||||
|
||||
/// Get the pose graph config
|
||||
pub fn pose_graph_config(&self) -> PoseGraphConfig {
|
||||
self.slam.pose_graph.clone()
|
||||
}
|
||||
|
||||
/// Get the background optimizer config
|
||||
pub fn background_optimizer_config(&self) -> BackgroundOptimizerConfig {
|
||||
self.slam.background_optimizer.clone()
|
||||
}
|
||||
|
||||
/// Get robot radius
|
||||
pub fn robot_radius(&self) -> f32 {
|
||||
self.sensor.robot.radius
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn test_default_config() {
|
||||
let config = VastuConfig::default();
|
||||
assert_eq!(config.grid.resolution, 0.025);
|
||||
assert_eq!(config.sensor.robot.radius, 0.17);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_yaml_roundtrip() {
|
||||
let config = VastuConfig::default();
|
||||
let yaml = serde_yaml::to_string(&config).unwrap();
|
||||
let parsed: VastuConfig = serde_yaml::from_str(&yaml).unwrap();
|
||||
assert_eq!(parsed.grid.resolution, config.grid.resolution);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_to_map_config() {
|
||||
let config = VastuConfig::default();
|
||||
let map_config = config.to_map_config();
|
||||
assert_eq!(map_config.grid.resolution, 0.025);
|
||||
assert_eq!(map_config.sensor.robot_radius, 0.17);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,210 @@
|
||||
# Core Module
|
||||
|
||||
Foundation types for the VastuSLAM library. This module defines the fundamental data structures used throughout the SLAM pipeline.
|
||||
|
||||
## Module Structure
|
||||
|
||||
```
|
||||
core/
|
||||
├── mod.rs # Module exports
|
||||
├── cell.rs # Grid cell types (CellType, Cell)
|
||||
├── point.rs # Coordinate types (GridCoord, WorldPoint)
|
||||
├── pose.rs # Robot pose (Pose2D, transforms)
|
||||
├── motion.rs # Odometry model (MotionModel, Odometry)
|
||||
├── sensors.rs # Sensor types (LidarScan, CliffSensors, BumperSensors)
|
||||
└── simd.rs # SIMD-optimized batch operations
|
||||
```
|
||||
|
||||
## Type Overview
|
||||
|
||||
### Coordinate Types (`point.rs`)
|
||||
|
||||
| Type | Description | Use Case |
|
||||
|------|-------------|----------|
|
||||
| `GridCoord` | Integer (i32, i32) grid cell indices | Occupancy grid access, path planning |
|
||||
| `WorldPoint` | Float (f32, f32) world coordinates in meters | Sensor data, robot position |
|
||||
|
||||
**Coordinate System (ROS REP-103):**
|
||||
```
|
||||
+X (Forward)
|
||||
↑
|
||||
│
|
||||
+Y ←───┼───→ -Y
|
||||
(Left) │ (Right)
|
||||
↓
|
||||
-X (Back)
|
||||
```
|
||||
|
||||
### Robot Pose (`pose.rs`)
|
||||
|
||||
`Pose2D` represents the robot's position and orientation:
|
||||
|
||||
```rust
|
||||
pub struct Pose2D {
|
||||
pub x: f32, // Position in meters (forward)
|
||||
pub y: f32, // Position in meters (left)
|
||||
pub theta: f32, // Orientation in radians (CCW from +X)
|
||||
}
|
||||
```
|
||||
|
||||
**Key Operations:**
|
||||
- `transform_point()` - Convert robot-frame point to world-frame
|
||||
- `inverse_transform_point()` - Convert world-frame point to robot-frame
|
||||
- `compose()` - Combine two poses (pose composition)
|
||||
- `inverse()` - Compute inverse transform
|
||||
|
||||
### Cell Types (`cell.rs`)
|
||||
|
||||
Semantic cell classification for multi-sensor fusion:
|
||||
|
||||
```
|
||||
Priority (highest to lowest):
|
||||
┌─────────┬──────────────────────────────────────────────────────┐
|
||||
│ Bump │ Physical collision detected (glass, mirrors) │
|
||||
├─────────┼──────────────────────────────────────────────────────┤
|
||||
│ Cliff │ Floor drop-off detected (stairs, ledges) │
|
||||
├─────────┼──────────────────────────────────────────────────────┤
|
||||
│ Wall │ Lidar-detected obstacle (walls, furniture) │
|
||||
├─────────┼──────────────────────────────────────────────────────┤
|
||||
│ Floor │ Traversable surface (free space) │
|
||||
├─────────┼──────────────────────────────────────────────────────┤
|
||||
│ Unknown │ Not yet observed │
|
||||
└─────────┴──────────────────────────────────────────────────────┘
|
||||
```
|
||||
|
||||
Higher-priority types override lower-priority ones. For example:
|
||||
- A cell marked as `Floor` can be upgraded to `Wall` if lidar detects an obstacle
|
||||
- A cell marked as `Wall` can be upgraded to `Bump` if the bumper triggers
|
||||
- A cell marked as `Bump` cannot be downgraded (physical evidence is definitive)
|
||||
|
||||
### Sensor Types (`sensors.rs`)
|
||||
|
||||
| Type | Description | Update Rate |
|
||||
|------|-------------|-------------|
|
||||
| `LidarScan` | 360° range measurements | ~5 Hz |
|
||||
| `CliffSensors` | 4 IR floor drop-off detectors | ~110 Hz |
|
||||
| `BumperSensors` | 2 mechanical collision sensors | Event-driven |
|
||||
| `SensorObservation` | Combined sensor snapshot | Per SLAM tick |
|
||||
|
||||
**Sensor Positions (CRL-200S Robot):**
|
||||
```
|
||||
Front
|
||||
LF ─────── RF (Cliff sensors)
|
||||
╲ ↑ ╱
|
||||
╲ │ ╱
|
||||
LS ──┬──┼──┬── RS (Cliff sensors)
|
||||
│ │
|
||||
L ─┴─────┴─ R (Bumpers)
|
||||
Back
|
||||
|
||||
LF/RF = Left/Right Front cliff (±5cm from center, 15cm forward)
|
||||
LS/RS = Left/Right Side cliff (±10cm from center, 12cm forward)
|
||||
L/R = Left/Right bumper (covers front arc)
|
||||
```
|
||||
|
||||
### Motion Model (`motion.rs`)
|
||||
|
||||
Probabilistic motion model for odometry uncertainty:
|
||||
|
||||
```rust
|
||||
pub struct MotionModel {
|
||||
pub alpha1: f32, // Rotation noise from rotation (rad/rad)
|
||||
pub alpha2: f32, // Rotation noise from translation (rad/m)
|
||||
pub alpha3: f32, // Translation noise from translation (m/m)
|
||||
pub alpha4: f32, // Translation noise from rotation (m/rad)
|
||||
}
|
||||
```
|
||||
|
||||
Based on Thrun's "Probabilistic Robotics" formulation. The model predicts pose uncertainty based on the motion magnitude.
|
||||
|
||||
### SIMD Operations (`simd.rs`)
|
||||
|
||||
SIMD-accelerated batch operations using `std::simd` (portable SIMD):
|
||||
|
||||
| Function | Description | Use Case |
|
||||
|----------|-------------|----------|
|
||||
| `transform_points_simd4` | Batch point transformation | Lidar scan projection |
|
||||
| `world_to_grid_simd4` | Batch coordinate conversion | Grid updates |
|
||||
| `distance_squared_simd4` | Batch distance calculation | Range filtering |
|
||||
| `is_valid_coords_simd4` | Batch bounds checking | Grid validation |
|
||||
|
||||
**PointCloud (SoA Layout):**
|
||||
```
|
||||
Structure-of-Arrays for cache-friendly SIMD access:
|
||||
|
||||
xs: [x₀, x₁, x₂, x₃, x₄, x₅, x₆, x₇, ...]
|
||||
ys: [y₀, y₁, y₂, y₃, y₄, y₅, y₆, y₇, ...]
|
||||
└────────────┘
|
||||
4-wide SIMD load
|
||||
```
|
||||
|
||||
All SIMD operations require input lengths to be multiples of 4 (LANES). Use `*_padded` constructors for automatic padding.
|
||||
|
||||
## Usage Examples
|
||||
|
||||
### Coordinate Transforms
|
||||
|
||||
```rust
|
||||
use vastu_slam::core::{Pose2D, WorldPoint};
|
||||
|
||||
// Robot at (1, 2) facing 90° left
|
||||
let robot_pose = Pose2D::new(1.0, 2.0, std::f32::consts::FRAC_PI_2);
|
||||
|
||||
// Point 1m ahead in robot frame
|
||||
let sensor_point = WorldPoint::new(1.0, 0.0);
|
||||
|
||||
// Transform to world coordinates
|
||||
let world_point = robot_pose.transform_point(sensor_point);
|
||||
// Result: (1.0, 3.0) - 1m in +Y direction
|
||||
```
|
||||
|
||||
### Processing Lidar Scans
|
||||
|
||||
```rust
|
||||
use vastu_slam::core::{LidarScan, Pose2D};
|
||||
use vastu_slam::core::simd::{PointCloud, transform_points};
|
||||
|
||||
// Create scan from raw data
|
||||
let scan = LidarScan::new(ranges, angles, 0.15, 8.0);
|
||||
|
||||
// Convert to SIMD-friendly point cloud
|
||||
let local_points = PointCloud::from_scan(&scan);
|
||||
|
||||
// Transform to world frame
|
||||
let robot_pose = Pose2D::new(x, y, theta);
|
||||
let world_points = transform_points(&local_points, &robot_pose);
|
||||
```
|
||||
|
||||
### Cell Priority Updates
|
||||
|
||||
```rust
|
||||
use vastu_slam::core::{Cell, CellType};
|
||||
|
||||
let mut cell = Cell::with_type(CellType::Floor);
|
||||
|
||||
// Wall overrides Floor
|
||||
cell.observe_with_priority(CellType::Wall);
|
||||
assert_eq!(cell.cell_type, CellType::Wall);
|
||||
|
||||
// Floor does NOT override Wall (lower priority)
|
||||
cell.observe_with_priority(CellType::Floor);
|
||||
assert_eq!(cell.cell_type, CellType::Wall);
|
||||
|
||||
// Bump overrides everything
|
||||
cell.observe_with_priority(CellType::Bump);
|
||||
assert_eq!(cell.cell_type, CellType::Bump);
|
||||
```
|
||||
|
||||
## Performance Notes
|
||||
|
||||
1. **SIMD Alignment**: Always use `PointCloud::from_scan()` or `from_tuples()` - they auto-pad to 4-element boundaries.
|
||||
|
||||
2. **Avoid `distance()`**: Use `distance_squared()` when comparing distances (avoids sqrt).
|
||||
|
||||
3. **Pre-compute Rotations**: For repeated transforms, use `RotationMatrix4::from_pose()` once and reuse.
|
||||
|
||||
4. **Batch Operations**: Process points in batches using SIMD functions rather than iterating one-by-one.
|
||||
|
||||
## Thread Safety
|
||||
|
||||
All types in this module are `Send + Sync` (no interior mutability). The SIMD functions are stateless and thread-safe.
|
||||
@@ -1,7 +1,40 @@
|
||||
//! Cell types for the occupancy grid.
|
||||
//!
|
||||
//! VastuMap uses semantic cell types that distinguish between different
|
||||
//! obstacle sources (walls, cliffs, invisible obstacles like glass).
|
||||
//! VastuSLAM uses semantic cell types that distinguish between different
|
||||
//! obstacle sources. This multi-sensor fusion approach allows the map to
|
||||
//! represent obstacles that lidar cannot detect (glass, cliffs).
|
||||
//!
|
||||
//! ## Cell Type Hierarchy
|
||||
//!
|
||||
//! ```text
|
||||
//! ┌─────────────────────────────────────────────────────────────────┐
|
||||
//! │ PRIORITY (highest first) │
|
||||
//! ├─────────┬──────────────────────────────────────────────────────┤
|
||||
//! │ Bump │ Physical collision - bumper triggered │
|
||||
//! │ │ Examples: glass doors, mirrors, thin furniture legs │
|
||||
//! ├─────────┼──────────────────────────────────────────────────────┤
|
||||
//! │ Cliff │ Floor drop-off - cliff sensor triggered │
|
||||
//! │ │ Examples: stairs, ledges, thresholds │
|
||||
//! ├─────────┼──────────────────────────────────────────────────────┤
|
||||
//! │ Wall │ Solid obstacle - lidar ray terminated │
|
||||
//! │ │ Examples: walls, furniture, doors │
|
||||
//! ├─────────┼──────────────────────────────────────────────────────┤
|
||||
//! │ Floor │ Traversable surface - no obstacles detected │
|
||||
//! │ │ Lidar rays pass through, safe for navigation │
|
||||
//! ├─────────┼──────────────────────────────────────────────────────┤
|
||||
//! │ Unknown │ Not yet observed by any sensor │
|
||||
//! │ │ Default state for unexplored areas │
|
||||
//! └─────────┴──────────────────────────────────────────────────────┘
|
||||
//! ```
|
||||
//!
|
||||
//! ## Priority Rules
|
||||
//!
|
||||
//! Higher-priority observations override lower-priority ones:
|
||||
//! - `Bump` cannot be downgraded (physical evidence is definitive)
|
||||
//! - `Cliff` can only be upgraded to `Bump`
|
||||
//! - `Wall` can be upgraded to `Cliff` or `Bump`
|
||||
//! - `Floor` can be upgraded to any obstacle type
|
||||
//! - `Unknown` can be set to any type
|
||||
|
||||
use serde::{Deserialize, Serialize};
|
||||
|
||||
@@ -0,0 +1,67 @@
|
||||
//! Core types for the VastuSLAM library.
|
||||
//!
|
||||
//! This module provides the fundamental types used throughout the SLAM pipeline.
|
||||
//! All types follow the ROS REP-103 coordinate convention:
|
||||
//! - **X-axis**: Forward (positive ahead of robot)
|
||||
//! - **Y-axis**: Left (positive to robot's left)
|
||||
//! - **Theta**: Counter-clockwise rotation from +X axis (radians)
|
||||
//!
|
||||
//! ## Type Categories
|
||||
//!
|
||||
//! ### Coordinates
|
||||
//! - [`GridCoord`]: Integer cell indices for occupancy grid access
|
||||
//! - [`WorldPoint`]: Floating-point world coordinates in meters
|
||||
//!
|
||||
//! ### Robot State
|
||||
//! - [`Pose2D`]: Robot position (x, y) and orientation (theta)
|
||||
//! - [`MotionModel`]: Probabilistic odometry uncertainty model
|
||||
//! - [`Odometry`]: Wheel encoder readings with differential drive kinematics
|
||||
//!
|
||||
//! ### Grid Cells
|
||||
//! - [`CellType`]: Semantic classification (Unknown, Floor, Wall, Cliff, Bump)
|
||||
//! - [`Cell`]: Grid cell with type, confidence, and observation count
|
||||
//!
|
||||
//! ### Sensors
|
||||
//! - [`LidarScan`]: 360° range measurements in polar coordinates
|
||||
//! - [`CliffSensors`]: Four IR floor drop-off detectors
|
||||
//! - [`BumperSensors`]: Two mechanical collision sensors
|
||||
//! - [`SensorObservation`]: Combined sensor snapshot at a timestamp
|
||||
//!
|
||||
//! ### SIMD Operations
|
||||
//! - [`PointCloud`]: Structure-of-Arrays point storage for vectorized operations
|
||||
//! - [`RotationMatrix4`]: Pre-computed rotation for batch transforms
|
||||
//! - [`GridCoordBatch`]: Batch of grid coordinates for SIMD processing
|
||||
//!
|
||||
//! ## Example
|
||||
//!
|
||||
//! ```rust,ignore
|
||||
//! use vastu_slam::core::{Pose2D, WorldPoint, LidarScan};
|
||||
//! use vastu_slam::core::simd::{PointCloud, transform_points};
|
||||
//!
|
||||
//! // Robot pose: 1m forward, 2m left, facing 90° CCW
|
||||
//! let pose = Pose2D::new(1.0, 2.0, std::f32::consts::FRAC_PI_2);
|
||||
//!
|
||||
//! // Transform sensor point to world frame
|
||||
//! let sensor_point = WorldPoint::new(1.0, 0.0); // 1m ahead in robot frame
|
||||
//! let world_point = pose.transform_point(sensor_point); // (1.0, 3.0) in world
|
||||
//!
|
||||
//! // Batch transform lidar scan using SIMD
|
||||
//! let scan = LidarScan::new(ranges, angles, 0.15, 8.0);
|
||||
//! let points = PointCloud::from_scan(&scan);
|
||||
//! let world_points = transform_points(&points, &pose);
|
||||
//! ```
|
||||
|
||||
mod cell;
|
||||
mod motion;
|
||||
mod point;
|
||||
mod pose;
|
||||
mod sensors;
|
||||
|
||||
pub mod simd;
|
||||
|
||||
pub use cell::{Cell, CellType};
|
||||
pub use motion::{MotionModel, Odometry};
|
||||
pub use point::{GridCoord, WorldPoint};
|
||||
pub use pose::{Pose2D, interpolate_pose, normalize_angle};
|
||||
pub use sensors::{BumperSensors, CliffSensors, LidarScan, SensorObservation};
|
||||
pub use simd::{GridCoordBatch, PointCloud, RotationMatrix4};
|
||||
@@ -0,0 +1,271 @@
|
||||
//! Odometry motion model for SLAM.
|
||||
//!
|
||||
//! Implements the probabilistic motion model for differential drive robots,
|
||||
//! following the formulation from "Probabilistic Robotics" (Thrun et al.).
|
||||
|
||||
use serde::{Deserialize, Serialize};
|
||||
|
||||
use super::Pose2D;
|
||||
|
||||
/// Odometry motion model parameters.
|
||||
///
|
||||
/// The model uses four noise parameters (alpha1-4) that characterize
|
||||
/// the uncertainty in robot motion:
|
||||
///
|
||||
/// - `alpha1`: Rotation noise from rotation
|
||||
/// - `alpha2`: Rotation noise from translation
|
||||
/// - `alpha3`: Translation noise from translation
|
||||
/// - `alpha4`: Translation noise from rotation
|
||||
///
|
||||
/// # Example
|
||||
///
|
||||
/// ```
|
||||
/// use vastu_slam::core::MotionModel;
|
||||
///
|
||||
/// // Default model for typical indoor robot
|
||||
/// let model = MotionModel::default();
|
||||
///
|
||||
/// // Custom model for high-precision robot
|
||||
/// let precise = MotionModel::new(0.01, 0.01, 0.02, 0.01);
|
||||
/// ```
|
||||
#[derive(Clone, Debug, Serialize, Deserialize)]
|
||||
pub struct MotionModel {
|
||||
/// Rotation noise from rotation (rad/rad)
|
||||
pub alpha1: f32,
|
||||
|
||||
/// Rotation noise from translation (rad/m)
|
||||
pub alpha2: f32,
|
||||
|
||||
/// Translation noise from translation (m/m)
|
||||
pub alpha3: f32,
|
||||
|
||||
/// Translation noise from rotation (m/rad)
|
||||
pub alpha4: f32,
|
||||
}
|
||||
|
||||
impl Default for MotionModel {
|
||||
fn default() -> Self {
|
||||
// Typical values for indoor mobile robot
|
||||
Self {
|
||||
alpha1: 0.05, // 5% rotation noise from rotation
|
||||
alpha2: 0.02, // 2% rotation noise from translation
|
||||
alpha3: 0.05, // 5% translation noise from translation
|
||||
alpha4: 0.02, // 2% translation noise from rotation
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl MotionModel {
|
||||
/// Create a new motion model with specified parameters
|
||||
pub fn new(alpha1: f32, alpha2: f32, alpha3: f32, alpha4: f32) -> Self {
|
||||
Self {
|
||||
alpha1,
|
||||
alpha2,
|
||||
alpha3,
|
||||
alpha4,
|
||||
}
|
||||
}
|
||||
|
||||
/// Create a low-noise motion model for high-precision robots
|
||||
pub fn low_noise() -> Self {
|
||||
Self {
|
||||
alpha1: 0.01,
|
||||
alpha2: 0.005,
|
||||
alpha3: 0.01,
|
||||
alpha4: 0.005,
|
||||
}
|
||||
}
|
||||
|
||||
/// Create a high-noise motion model for robots with poor odometry
|
||||
pub fn high_noise() -> Self {
|
||||
Self {
|
||||
alpha1: 0.15,
|
||||
alpha2: 0.05,
|
||||
alpha3: 0.15,
|
||||
alpha4: 0.05,
|
||||
}
|
||||
}
|
||||
|
||||
/// Predict the next pose given current pose and odometry delta.
|
||||
///
|
||||
/// This is the deterministic motion model (mean prediction).
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `pose` - Current robot pose
|
||||
/// * `odom_delta` - Odometry change (dx, dy, dtheta in robot frame)
|
||||
///
|
||||
/// # Returns
|
||||
/// Predicted pose after motion
|
||||
pub fn predict(&self, pose: &Pose2D, odom_delta: &Pose2D) -> Pose2D {
|
||||
pose.compose(odom_delta)
|
||||
}
|
||||
|
||||
/// Compute the expected standard deviation of the motion.
|
||||
///
|
||||
/// Returns (linear_stddev, angular_stddev) based on the motion magnitude.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `odom_delta` - Odometry change
|
||||
///
|
||||
/// # Returns
|
||||
/// Tuple of (linear standard deviation, angular standard deviation)
|
||||
pub fn motion_uncertainty(&self, odom_delta: &Pose2D) -> (f32, f32) {
|
||||
let translation = (odom_delta.x * odom_delta.x + odom_delta.y * odom_delta.y).sqrt();
|
||||
let rotation = odom_delta.theta.abs();
|
||||
|
||||
// Linear uncertainty
|
||||
let linear_var = self.alpha3 * translation + self.alpha4 * rotation;
|
||||
let linear_stddev = linear_var.sqrt().max(0.001); // Minimum 1mm
|
||||
|
||||
// Angular uncertainty
|
||||
let angular_var = self.alpha1 * rotation + self.alpha2 * translation;
|
||||
let angular_stddev = angular_var.sqrt().max(0.001); // Minimum 0.06 degrees
|
||||
|
||||
(linear_stddev, angular_stddev)
|
||||
}
|
||||
|
||||
/// Compute the covariance matrix for the motion.
|
||||
///
|
||||
/// Returns a 3x3 covariance matrix [xx, xy, xt, yx, yy, yt, tx, ty, tt]
|
||||
/// where x, y are position and t is theta.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `odom_delta` - Odometry change
|
||||
///
|
||||
/// # Returns
|
||||
/// 3x3 covariance matrix as a 9-element array (row-major)
|
||||
pub fn motion_covariance(&self, odom_delta: &Pose2D) -> [f32; 9] {
|
||||
let (linear_std, angular_std) = self.motion_uncertainty(odom_delta);
|
||||
let linear_var = linear_std * linear_std;
|
||||
let angular_var = angular_std * angular_std;
|
||||
|
||||
// Simple diagonal covariance (assumes independent x, y, theta)
|
||||
// For a more accurate model, this should account for the heading
|
||||
[
|
||||
linear_var,
|
||||
0.0,
|
||||
0.0, // Row 1: xx, xy, xt
|
||||
0.0,
|
||||
linear_var,
|
||||
0.0, // Row 2: yx, yy, yt
|
||||
0.0,
|
||||
0.0,
|
||||
angular_var, // Row 3: tx, ty, tt
|
||||
]
|
||||
}
|
||||
}
|
||||
|
||||
/// Odometry reading from wheel encoders.
|
||||
#[derive(Clone, Debug, Default)]
|
||||
pub struct Odometry {
|
||||
/// Timestamp in seconds
|
||||
pub timestamp: f64,
|
||||
|
||||
/// Left wheel distance traveled (meters)
|
||||
pub left_distance: f32,
|
||||
|
||||
/// Right wheel distance traveled (meters)
|
||||
pub right_distance: f32,
|
||||
|
||||
/// Wheel base (distance between wheels) in meters
|
||||
pub wheel_base: f32,
|
||||
}
|
||||
|
||||
impl Odometry {
|
||||
/// Create new odometry reading
|
||||
pub fn new(timestamp: f64, left: f32, right: f32, wheel_base: f32) -> Self {
|
||||
Self {
|
||||
timestamp,
|
||||
left_distance: left,
|
||||
right_distance: right,
|
||||
wheel_base,
|
||||
}
|
||||
}
|
||||
|
||||
/// Convert to pose delta (dx, dy, dtheta) in robot frame.
|
||||
///
|
||||
/// Uses differential drive kinematics to compute the pose change.
|
||||
pub fn to_pose_delta(&self) -> Pose2D {
|
||||
let delta_left = self.left_distance;
|
||||
let delta_right = self.right_distance;
|
||||
|
||||
// Differential drive kinematics
|
||||
let delta_trans = (delta_left + delta_right) / 2.0;
|
||||
let delta_rot = (delta_right - delta_left) / self.wheel_base;
|
||||
|
||||
// In robot frame: forward motion along x, no lateral motion
|
||||
Pose2D::new(delta_trans, 0.0, delta_rot)
|
||||
}
|
||||
|
||||
/// Compute pose delta between two odometry readings.
|
||||
pub fn delta_from(&self, previous: &Odometry) -> Pose2D {
|
||||
let delta_left = self.left_distance - previous.left_distance;
|
||||
let delta_right = self.right_distance - previous.right_distance;
|
||||
|
||||
let delta_trans = (delta_left + delta_right) / 2.0;
|
||||
let delta_rot = (delta_right - delta_left) / self.wheel_base;
|
||||
|
||||
Pose2D::new(delta_trans, 0.0, delta_rot)
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn test_motion_model_default() {
|
||||
let model = MotionModel::default();
|
||||
assert!(model.alpha1 > 0.0);
|
||||
assert!(model.alpha3 > 0.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_predict_straight_motion() {
|
||||
let model = MotionModel::default();
|
||||
let pose = Pose2D::new(0.0, 0.0, 0.0);
|
||||
let delta = Pose2D::new(1.0, 0.0, 0.0); // Move 1m forward
|
||||
|
||||
let predicted = model.predict(&pose, &delta);
|
||||
|
||||
assert!((predicted.x - 1.0).abs() < 1e-6);
|
||||
assert!((predicted.y - 0.0).abs() < 1e-6);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_motion_uncertainty() {
|
||||
let model = MotionModel::default();
|
||||
|
||||
// Large motion should have large uncertainty
|
||||
let large_delta = Pose2D::new(1.0, 0.0, 0.5);
|
||||
let (lin_std, ang_std) = model.motion_uncertainty(&large_delta);
|
||||
assert!(lin_std > 0.0);
|
||||
assert!(ang_std > 0.0);
|
||||
|
||||
// Small motion should have small uncertainty
|
||||
let small_delta = Pose2D::new(0.01, 0.0, 0.01);
|
||||
let (small_lin, small_ang) = model.motion_uncertainty(&small_delta);
|
||||
assert!(small_lin < lin_std);
|
||||
assert!(small_ang < ang_std);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_odometry_to_delta() {
|
||||
let odom = Odometry::new(0.0, 0.5, 0.5, 0.3);
|
||||
let delta = odom.to_pose_delta();
|
||||
|
||||
// Equal wheel distances = straight line motion
|
||||
assert!((delta.x - 0.5).abs() < 1e-6);
|
||||
assert!((delta.theta).abs() < 1e-6);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_odometry_rotation() {
|
||||
let odom = Odometry::new(0.0, -0.15, 0.15, 0.3);
|
||||
let delta = odom.to_pose_delta();
|
||||
|
||||
// Opposite wheel distances = pure rotation
|
||||
assert!((delta.x).abs() < 1e-6); // No forward motion
|
||||
assert!((delta.theta - 1.0).abs() < 1e-6); // 1 radian rotation
|
||||
}
|
||||
}
|
||||
@@ -1,14 +1,67 @@
|
||||
//! Point and coordinate types for the occupancy grid.
|
||||
//!
|
||||
//! This module defines the two primary coordinate types used throughout VastuSLAM:
|
||||
//!
|
||||
//! - [`GridCoord`]: Integer cell indices for grid operations
|
||||
//! - [`WorldPoint`]: Floating-point world coordinates in meters
|
||||
//!
|
||||
//! ## Coordinate Conversion
|
||||
//!
|
||||
//! Converting between world and grid coordinates requires knowing the grid's
|
||||
//! origin and resolution. Use [`GridStorage::world_to_grid`] and
|
||||
//! [`GridStorage::grid_to_world`] for conversion.
|
||||
//!
|
||||
//! ```text
|
||||
//! Grid Coordinate System:
|
||||
//!
|
||||
//! y (row)
|
||||
//! ↑
|
||||
//! │ ┌───┬───┬───┐
|
||||
//! 2 │ │ │ │ │
|
||||
//! │ ├───┼───┼───┤
|
||||
//! 1 │ │ │ ● │ │ ● = GridCoord(1, 1)
|
||||
//! │ ├───┼───┼───┤
|
||||
//! 0 │ │ │ │ │
|
||||
//! └──┴───┴───┴───┴──→ x (column)
|
||||
//! 0 1 2
|
||||
//! ```
|
||||
|
||||
use serde::{Deserialize, Serialize};
|
||||
use std::ops::{Add, Mul, Sub};
|
||||
|
||||
/// Grid coordinates (integer cell indices)
|
||||
/// Grid coordinates (integer cell indices).
|
||||
///
|
||||
/// Represents a discrete cell position in the occupancy grid. Grid coordinates
|
||||
/// are integer indices where (0, 0) is the grid's internal origin (typically
|
||||
/// not the world origin).
|
||||
///
|
||||
/// # Coordinate System
|
||||
///
|
||||
/// - **x**: Column index (increases rightward/forward in world)
|
||||
/// - **y**: Row index (increases upward/leftward in world)
|
||||
///
|
||||
/// # Neighbor Access
|
||||
///
|
||||
/// Use [`neighbors_4`](Self::neighbors_4) for 4-connected (Manhattan) neighbors,
|
||||
/// or [`neighbors_8`](Self::neighbors_8) for 8-connected (including diagonals).
|
||||
///
|
||||
/// # Example
|
||||
///
|
||||
/// ```rust,ignore
|
||||
/// use vastu_slam::core::GridCoord;
|
||||
///
|
||||
/// let coord = GridCoord::new(5, 10);
|
||||
/// let neighbors = coord.neighbors_4(); // N, E, S, W
|
||||
///
|
||||
/// // Manhattan distance for heuristics
|
||||
/// let goal = GridCoord::new(10, 15);
|
||||
/// let distance = coord.manhattan_distance(&goal); // 10
|
||||
/// ```
|
||||
#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash, Default, Serialize, Deserialize)]
|
||||
pub struct GridCoord {
|
||||
/// X coordinate (column index)
|
||||
/// X coordinate (column index, increases in +X world direction)
|
||||
pub x: i32,
|
||||
/// Y coordinate (row index)
|
||||
/// Y coordinate (row index, increases in +Y world direction)
|
||||
pub y: i32,
|
||||
}
|
||||
|
||||
@@ -76,12 +129,54 @@ impl Sub for GridCoord {
|
||||
}
|
||||
}
|
||||
|
||||
/// World coordinates (meters, f32)
|
||||
/// World coordinates in meters (f32 precision).
|
||||
///
|
||||
/// Represents a point in the 2D world coordinate system. All measurements
|
||||
/// are in meters, following the ROS REP-103 convention.
|
||||
///
|
||||
/// # Coordinate System (ROS REP-103)
|
||||
///
|
||||
/// ```text
|
||||
/// +X (Forward)
|
||||
/// ↑
|
||||
/// │
|
||||
/// +Y ←───┼───→ -Y
|
||||
/// (Left) │ (Right)
|
||||
/// ↓
|
||||
/// -X (Back)
|
||||
/// ```
|
||||
///
|
||||
/// # Vector Operations
|
||||
///
|
||||
/// `WorldPoint` supports common vector operations:
|
||||
/// - Addition/subtraction for displacement
|
||||
/// - Scalar multiplication for scaling
|
||||
/// - Dot product and cross product
|
||||
/// - Rotation around origin
|
||||
/// - Normalization to unit vector
|
||||
///
|
||||
/// # Example
|
||||
///
|
||||
/// ```rust,ignore
|
||||
/// use vastu_slam::core::WorldPoint;
|
||||
///
|
||||
/// let origin = WorldPoint::ZERO;
|
||||
/// let target = WorldPoint::new(3.0, 4.0);
|
||||
///
|
||||
/// // Distance calculation
|
||||
/// let distance = origin.distance(&target); // 5.0
|
||||
///
|
||||
/// // Angle from origin to target
|
||||
/// let angle = origin.angle_to(&target); // ~0.93 radians
|
||||
///
|
||||
/// // Create point at angle and distance from origin
|
||||
/// let point = origin.point_at(angle, distance); // (3.0, 4.0)
|
||||
/// ```
|
||||
#[derive(Clone, Copy, Debug, PartialEq, Default, Serialize, Deserialize)]
|
||||
pub struct WorldPoint {
|
||||
/// X coordinate in meters (forward in ROS convention)
|
||||
/// X coordinate in meters (positive = forward in robot frame)
|
||||
pub x: f32,
|
||||
/// Y coordinate in meters (left in ROS convention)
|
||||
/// Y coordinate in meters (positive = left in robot frame)
|
||||
pub y: f32,
|
||||
}
|
||||
|
||||
@@ -1,21 +1,84 @@
|
||||
//! Robot pose representation.
|
||||
//! Robot pose representation and coordinate frame transformations.
|
||||
//!
|
||||
//! This module provides [`Pose2D`] for representing robot pose (position + orientation)
|
||||
//! and utilities for coordinate frame transformations.
|
||||
//!
|
||||
//! ## Coordinate Frames
|
||||
//!
|
||||
//! VastuSLAM uses two primary coordinate frames:
|
||||
//!
|
||||
//! - **World Frame**: Fixed global coordinates (map origin)
|
||||
//! - **Robot Frame**: Moves with the robot (robot is at origin, facing +X)
|
||||
//!
|
||||
//! ```text
|
||||
//! World Frame: Robot Frame:
|
||||
//!
|
||||
//! +Y +X (Forward)
|
||||
//! ↑ ↑
|
||||
//! │ │
|
||||
//! │ ┌─┐ │
|
||||
//! │ │→│ Robot ←───┼───→
|
||||
//! │ └─┘ +Y│-Y
|
||||
//! └────────→ +X │
|
||||
//! ↓
|
||||
//! ```
|
||||
//!
|
||||
//! ## Frame Transformations
|
||||
//!
|
||||
//! - [`Pose2D::transform_point`]: Robot frame → World frame
|
||||
//! - [`Pose2D::inverse_transform_point`]: World frame → Robot frame
|
||||
//! - [`Pose2D::compose`]: Combine two poses (chain transforms)
|
||||
//! - [`Pose2D::inverse`]: Invert a pose transform
|
||||
|
||||
use super::point::WorldPoint;
|
||||
use serde::{Deserialize, Serialize};
|
||||
|
||||
/// Robot pose in world coordinates (x, y, theta)
|
||||
/// Robot pose in world coordinates (position + orientation).
|
||||
///
|
||||
/// Coordinate system: ROS REP-103
|
||||
/// - X: Forward (positive ahead of robot)
|
||||
/// - Y: Left (positive to robot's left)
|
||||
/// - Theta: Rotation angle in radians, CCW positive from +X axis
|
||||
/// Represents the robot's position (x, y) and heading (theta) in the world frame.
|
||||
/// This is the fundamental type for tracking robot state and performing coordinate
|
||||
/// transformations between robot and world frames.
|
||||
///
|
||||
/// # Coordinate System (ROS REP-103)
|
||||
///
|
||||
/// ```text
|
||||
/// +X (Forward)
|
||||
/// ↑
|
||||
/// │
|
||||
/// +Y ←─────┼─────→ -Y
|
||||
/// (Left) │ (Right)
|
||||
/// ↓
|
||||
/// -X (Back)
|
||||
///
|
||||
/// Theta = 0: Robot facing +X (forward)
|
||||
/// Theta = π/2: Robot facing +Y (left)
|
||||
/// Theta = π: Robot facing -X (backward)
|
||||
/// Theta = -π/2: Robot facing -Y (right)
|
||||
/// ```
|
||||
///
|
||||
/// # Example
|
||||
///
|
||||
/// ```rust,ignore
|
||||
/// use vastu_slam::core::{Pose2D, WorldPoint};
|
||||
/// use std::f32::consts::FRAC_PI_2;
|
||||
///
|
||||
/// // Robot at (1, 2) facing left (+Y direction)
|
||||
/// let pose = Pose2D::new(1.0, 2.0, FRAC_PI_2);
|
||||
///
|
||||
/// // Point 1m ahead in robot frame
|
||||
/// let sensor_point = WorldPoint::new(1.0, 0.0);
|
||||
///
|
||||
/// // Transform to world frame: 1m in +Y direction from robot
|
||||
/// let world_point = pose.transform_point(sensor_point);
|
||||
/// // Result: (1.0, 3.0)
|
||||
/// ```
|
||||
#[derive(Clone, Copy, Debug, Default, Serialize, Deserialize)]
|
||||
pub struct Pose2D {
|
||||
/// X position in meters
|
||||
/// X position in meters (world frame)
|
||||
pub x: f32,
|
||||
/// Y position in meters
|
||||
/// Y position in meters (world frame)
|
||||
pub y: f32,
|
||||
/// Orientation in radians (CCW positive from +X)
|
||||
/// Orientation in radians, counter-clockwise from +X axis (range: [-π, π])
|
||||
pub theta: f32,
|
||||
}
|
||||
|
||||
@@ -1,19 +1,89 @@
|
||||
//! Sensor input types for the occupancy grid.
|
||||
//!
|
||||
//! This module defines the sensor data structures used by VastuSLAM for
|
||||
//! multi-sensor occupancy grid mapping.
|
||||
//!
|
||||
//! ## Sensor Types
|
||||
//!
|
||||
//! | Sensor | Rate | Purpose |
|
||||
//! |--------|------|---------|
|
||||
//! | [`LidarScan`] | ~5 Hz | 360° obstacle detection |
|
||||
//! | [`CliffSensors`] | ~110 Hz | Floor drop-off detection |
|
||||
//! | [`BumperSensors`] | Event | Collision detection |
|
||||
//!
|
||||
//! ## CRL-200S Robot Sensor Layout
|
||||
//!
|
||||
//! ```text
|
||||
//! Front (0°)
|
||||
//! ┌───────────────┐
|
||||
//! │ LF RF │ Cliff sensors (IR)
|
||||
//! │ ╲ ↑ ╱ │
|
||||
//! │ ╲ │ ╱ │
|
||||
//! │LS ╲ │ ╱ RS │ Cliff sensors (side)
|
||||
//! │ ╲ │ ╱ │
|
||||
//! │ L ──┴── R │ Bumpers (mechanical)
|
||||
//! │ LIDAR │ 360° lidar (center)
|
||||
//! └───────────────┘
|
||||
//! Back
|
||||
//!
|
||||
//! LF/RF = Left/Right Front cliff (15cm fwd, ±5cm lateral)
|
||||
//! LS/RS = Left/Right Side cliff (12cm fwd, ±10cm lateral)
|
||||
//! L/R = Left/Right bumper (front arc coverage)
|
||||
//! ```
|
||||
//!
|
||||
//! ## Sensor Coordinate Frame
|
||||
//!
|
||||
//! All sensor positions are in the robot frame:
|
||||
//! - Origin: Robot center
|
||||
//! - +X: Forward
|
||||
//! - +Y: Left
|
||||
|
||||
use super::point::WorldPoint;
|
||||
use super::pose::Pose2D;
|
||||
use serde::{Deserialize, Serialize};
|
||||
|
||||
/// Lidar scan data in polar coordinates
|
||||
/// Lidar scan data in polar coordinates.
|
||||
///
|
||||
/// Represents a complete 360° lidar scan as a collection of range measurements
|
||||
/// at known angles. Invalid measurements (too close, too far, or no return)
|
||||
/// are filtered using [`is_valid_range`](Self::is_valid_range).
|
||||
///
|
||||
/// # Angular Convention
|
||||
///
|
||||
/// ```text
|
||||
/// 0° (+X, Forward)
|
||||
/// ↑
|
||||
/// │
|
||||
/// 90° (+Y) ├───────→ -90° (-Y)
|
||||
/// Left │ Right
|
||||
/// ↓
|
||||
/// ±180° (-X, Back)
|
||||
/// ```
|
||||
///
|
||||
/// Angles are in radians, counter-clockwise from +X axis.
|
||||
///
|
||||
/// # Example
|
||||
///
|
||||
/// ```rust,ignore
|
||||
/// use vastu_slam::core::{LidarScan, Pose2D};
|
||||
///
|
||||
/// // Create scan from raw measurements
|
||||
/// let scan = LidarScan::new(ranges, angles, 0.15, 8.0);
|
||||
///
|
||||
/// // Filter valid points and transform to world frame
|
||||
/// let robot_pose = Pose2D::new(x, y, theta);
|
||||
/// let world_points = scan.to_world_points(robot_pose);
|
||||
/// ```
|
||||
#[derive(Clone, Debug, Default)]
|
||||
pub struct LidarScan {
|
||||
/// Range measurements in meters (f32::INFINITY or > max_range = no return)
|
||||
/// Range measurements in meters.
|
||||
/// Values > max_range or f32::INFINITY indicate no return (ray passed through).
|
||||
pub ranges: Vec<f32>,
|
||||
/// Angle for each range in radians (0 = forward, CCW positive)
|
||||
/// Angle for each range in radians (0 = forward, counter-clockwise positive).
|
||||
pub angles: Vec<f32>,
|
||||
/// Minimum valid range (ignore closer measurements)
|
||||
/// Minimum valid range in meters (reject closer measurements, typically 0.15m).
|
||||
pub range_min: f32,
|
||||
/// Maximum valid range (treat further as "no hit")
|
||||
/// Maximum valid range in meters (treat farther as "no hit", typically 8.0m).
|
||||
pub range_max: f32,
|
||||
}
|
||||
|
||||
@@ -97,16 +167,50 @@ impl LidarScan {
|
||||
}
|
||||
}
|
||||
|
||||
/// Cliff sensor readings
|
||||
/// Cliff sensor readings from IR floor-detection sensors.
|
||||
///
|
||||
/// Cliff sensors detect floor drop-offs (stairs, ledges) by measuring
|
||||
/// the distance to the floor surface. When a cliff is detected, the
|
||||
/// sensor reports `true`.
|
||||
///
|
||||
/// # Sensor Positions (CRL-200S)
|
||||
///
|
||||
/// ```text
|
||||
/// ┌─────────┐
|
||||
/// LF │ ↑ │ RF (Front: ±5cm, 15cm forward)
|
||||
/// │ │ │
|
||||
/// LS │ │ │ RS (Side: ±10cm, 12cm forward)
|
||||
/// └─────────┘
|
||||
/// ```
|
||||
///
|
||||
/// # Usage
|
||||
///
|
||||
/// ```rust,ignore
|
||||
/// use vastu_slam::core::{CliffSensors, Pose2D};
|
||||
///
|
||||
/// let cliffs = CliffSensors {
|
||||
/// left_front: true,
|
||||
/// right_front: false,
|
||||
/// left_side: false,
|
||||
/// right_side: false,
|
||||
/// };
|
||||
///
|
||||
/// if cliffs.any_triggered() {
|
||||
/// // Get world positions of triggered sensors
|
||||
/// let robot_pose = Pose2D::new(x, y, theta);
|
||||
/// let cliff_points = cliffs.triggered_world_positions(robot_pose);
|
||||
/// // Mark these positions as Cliff in the grid
|
||||
/// }
|
||||
/// ```
|
||||
#[derive(Clone, Copy, Debug, Default, PartialEq, Eq, Serialize, Deserialize)]
|
||||
pub struct CliffSensors {
|
||||
/// Left side cliff sensor triggered
|
||||
/// Left side cliff sensor triggered (12cm fwd, 10cm left)
|
||||
pub left_side: bool,
|
||||
/// Left front cliff sensor triggered
|
||||
/// Left front cliff sensor triggered (15cm fwd, 5cm left)
|
||||
pub left_front: bool,
|
||||
/// Right front cliff sensor triggered
|
||||
/// Right front cliff sensor triggered (15cm fwd, 5cm right)
|
||||
pub right_front: bool,
|
||||
/// Right side cliff sensor triggered
|
||||
/// Right side cliff sensor triggered (12cm fwd, 10cm right)
|
||||
pub right_side: bool,
|
||||
}
|
||||
|
||||
@@ -165,12 +269,38 @@ impl CliffSensors {
|
||||
}
|
||||
}
|
||||
|
||||
/// Bumper sensor readings
|
||||
/// Bumper sensor readings from mechanical collision sensors.
|
||||
///
|
||||
/// Bumpers detect physical collisions with obstacles. This is crucial for
|
||||
/// detecting "invisible" obstacles that lidar cannot see, such as:
|
||||
/// - Glass doors and windows
|
||||
/// - Mirrors
|
||||
/// - Thin table/chair legs
|
||||
/// - Dark-colored objects that absorb lidar
|
||||
///
|
||||
/// # Collision Angle Estimation
|
||||
///
|
||||
/// ```text
|
||||
/// ↑ Forward
|
||||
/// │
|
||||
/// L ────┼──── R
|
||||
/// │
|
||||
///
|
||||
/// Both = center collision (0°)
|
||||
/// Left only = left collision (~30° left)
|
||||
/// Right only = right collision (~30° right)
|
||||
/// ```
|
||||
///
|
||||
/// # Priority
|
||||
///
|
||||
/// Bumper collisions have the highest priority in the cell type hierarchy.
|
||||
/// Once a cell is marked as `Bump`, it cannot be downgraded by lidar or
|
||||
/// cliff observations.
|
||||
#[derive(Clone, Copy, Debug, Default, PartialEq, Eq, Serialize, Deserialize)]
|
||||
pub struct BumperSensors {
|
||||
/// Left bumper triggered
|
||||
/// Left bumper triggered (covers left front arc)
|
||||
pub left: bool,
|
||||
/// Right bumper triggered
|
||||
/// Right bumper triggered (covers right front arc)
|
||||
pub right: bool,
|
||||
}
|
||||
|
||||
@@ -225,18 +355,35 @@ impl BumperSensors {
|
||||
}
|
||||
}
|
||||
|
||||
/// Combined sensor observation for a single timestep
|
||||
/// Combined sensor observation for a single timestep.
|
||||
///
|
||||
/// Bundles all sensor readings taken at approximately the same time,
|
||||
/// along with the robot's pose at that moment. This is the primary
|
||||
/// input type for [`crate::OccupancyGridMap::observe`].
|
||||
///
|
||||
/// # Sensor Availability
|
||||
///
|
||||
/// Not all sensors report at the same rate:
|
||||
/// - **Lidar**: ~5 Hz (every ~200ms) - may be `None` on most ticks
|
||||
/// - **Cliff/Bumper**: ~110 Hz - always available
|
||||
///
|
||||
/// # Processing Order
|
||||
///
|
||||
/// When processed by the map, sensors are handled by priority:
|
||||
/// 1. Lidar (lowest priority) - marks Floor and Wall
|
||||
/// 2. Cliff - marks Cliff (overrides Floor/Wall)
|
||||
/// 3. Bumper (highest priority) - marks Bump (overrides everything)
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct SensorObservation {
|
||||
/// Robot pose when observation was taken
|
||||
/// Robot pose when observation was taken (in world frame)
|
||||
pub pose: Pose2D,
|
||||
/// Lidar scan (None if not available this tick)
|
||||
/// Lidar scan (None if lidar didn't report this tick, ~5 Hz)
|
||||
pub lidar: Option<LidarScan>,
|
||||
/// Cliff sensor states
|
||||
/// Cliff sensor states (always available, ~110 Hz)
|
||||
pub cliffs: CliffSensors,
|
||||
/// Bumper sensor states
|
||||
/// Bumper sensor states (event-driven, only true on collision)
|
||||
pub bumpers: BumperSensors,
|
||||
/// Timestamp in microseconds since epoch
|
||||
/// Timestamp in microseconds since epoch (for temporal ordering)
|
||||
pub timestamp_us: u64,
|
||||
}
|
||||
|
||||
@@ -0,0 +1,350 @@
|
||||
# Grid Module
|
||||
|
||||
Occupancy grid implementation with multi-sensor fusion and SIMD-optimized storage.
|
||||
|
||||
## Module Structure
|
||||
|
||||
```
|
||||
grid/
|
||||
├── mod.rs # Module exports
|
||||
├── config.rs # Configuration types (GridConfig, SensorConfig, MapConfig)
|
||||
├── raycaster.rs # Bresenham ray casting for lidar
|
||||
├── lidar_update.rs # Lidar scan processing (log-odds Bayesian updates)
|
||||
├── cliff_update.rs # Cliff sensor processing
|
||||
├── bumper_update.rs # Bumper collision processing
|
||||
└── storage/
|
||||
├── mod.rs # Storage module exports
|
||||
├── core.rs # GridStorage implementation (SoA layout)
|
||||
└── types.rs # Helper types (CellMut, CellCounts)
|
||||
```
|
||||
|
||||
## Architecture Overview
|
||||
|
||||
### Structure-of-Arrays (SoA) Memory Layout
|
||||
|
||||
The grid uses SoA layout for SIMD-optimized operations:
|
||||
|
||||
```
|
||||
Traditional Array-of-Structs (AoS):
|
||||
cells: [Cell₀, Cell₁, Cell₂, Cell₃, ...]
|
||||
├─────────────────────────────────┤
|
||||
type conf obs swept type conf obs swept ...
|
||||
|
||||
Structure-of-Arrays (SoA) - Our Layout:
|
||||
cell_types: [T T T T T T T T T T T T T T T T|...] ← SIMD-friendly
|
||||
confidences: [C C C C C C C C C C C C C C C C|...]
|
||||
observations: [O O O O O O O O O O O O O O O O|...]
|
||||
swept_flags: [S S S S S S S S S S S S S S S S|...]
|
||||
log_odds: [L L L L L L L L L L L L L L L L|...]
|
||||
distance: [D D D D D D D D D D D D D D D D|...]
|
||||
```
|
||||
|
||||
Benefits:
|
||||
- SIMD operations process 16 cells at once (`u8x16`)
|
||||
- Cache-friendly for type-specific queries
|
||||
- Memory bandwidth optimization
|
||||
|
||||
### Log-Odds Occupancy Model
|
||||
|
||||
Based on Google Cartographer's probabilistic grid model:
|
||||
|
||||
```
|
||||
Probability ←→ Log-Odds Conversion:
|
||||
L(x) = log(P(x) / (1 - P(x)))
|
||||
|
||||
Bayesian Update:
|
||||
L_new = L_old + L_observation
|
||||
|
||||
Fixed-Point Storage:
|
||||
Stored as i16, actual = value / 100
|
||||
```
|
||||
|
||||
**Update Parameters:**
|
||||
|
||||
| Config | L_hit | L_miss | Obs. for Wall | Use Case |
|
||||
|--------|-------|--------|---------------|----------|
|
||||
| Cartographer | +20 | -4 | 3-5 | Robust mapping |
|
||||
| Balanced | +30 | -12 | 2-3 | General purpose |
|
||||
| Aggressive | +70 | -28 | 1 | Fast/controlled |
|
||||
|
||||
**Probability Thresholds:**
|
||||
- `L > 50` → Occupied (Wall)
|
||||
- `L < -50` → Free (Floor)
|
||||
- `-50 ≤ L ≤ 50` → Unknown
|
||||
|
||||
## Sensor Update Pipeline
|
||||
|
||||
```
|
||||
┌─────────────────┐
|
||||
│ LidarScan │
|
||||
│ (360° rays) │
|
||||
└────────┬────────┘
|
||||
│
|
||||
▼
|
||||
┌──────────────────────────────────────────────────────────┐
|
||||
│ Lidar Update │
|
||||
│ 1. For each valid ray: │
|
||||
│ - Apply L_MISS to cells along ray path (Bresenham) │
|
||||
│ - Apply L_HIT to endpoint cell (if range < max) │
|
||||
│ 2. Update distance field for new walls │
|
||||
└──────────────────────────────────────────────────────────┘
|
||||
│
|
||||
▼
|
||||
┌──────────────────────────────────────────────────────────┐
|
||||
│ Cliff Update │
|
||||
│ - Transform sensor positions to world frame │
|
||||
│ - Mark triggered sensor cells as Cliff (priority > Wall)│
|
||||
└──────────────────────────────────────────────────────────┘
|
||||
│
|
||||
▼
|
||||
┌──────────────────────────────────────────────────────────┐
|
||||
│ Bumper Update │
|
||||
│ - Estimate collision position from bumper zone │
|
||||
│ - Mark collision cells as Bump (highest priority) │
|
||||
└──────────────────────────────────────────────────────────┘
|
||||
```
|
||||
|
||||
## Key Types
|
||||
|
||||
### GridStorage
|
||||
|
||||
Core storage with SoA layout:
|
||||
|
||||
```rust
|
||||
pub struct GridStorage {
|
||||
cell_types: Vec<u8>, // CellType as u8
|
||||
confidences: Vec<u8>, // 0-255 confidence
|
||||
observation_counts: Vec<u8>, // Saturating counter
|
||||
swept_flags: Vec<u8>, // Coverage tracking
|
||||
log_odds: Vec<i16>, // Bayesian occupancy
|
||||
distance_field: Vec<f32>, // Distance to nearest wall
|
||||
// ... metadata
|
||||
}
|
||||
```
|
||||
|
||||
**Key Methods:**
|
||||
- `apply_hit(coord)` - Add obstacle evidence (log-odds += L_HIT)
|
||||
- `apply_miss(coord)` - Add free space evidence (log-odds += L_MISS)
|
||||
- `set_type_with_priority(coord, type)` - Respects priority hierarchy
|
||||
- `count_by_type()` - SIMD-optimized cell counting
|
||||
|
||||
### Configuration Types
|
||||
|
||||
```rust
|
||||
// Grid dimensions and resolution
|
||||
pub struct GridConfig {
|
||||
pub resolution: f32, // 0.025 = 2.5cm cells
|
||||
pub initial_width: usize, // 800 = 20m
|
||||
pub initial_height: usize,
|
||||
pub auto_expand: bool, // Grow grid as needed
|
||||
pub max_width: usize, // Memory limit
|
||||
pub max_height: usize,
|
||||
}
|
||||
|
||||
// Robot and sensor parameters
|
||||
pub struct SensorConfig {
|
||||
pub robot_radius: f32, // 0.17m
|
||||
pub lidar_offset: WorldPoint, // (-0.11, 0) from center
|
||||
pub max_lidar_range: f32, // 8.0m
|
||||
pub min_lidar_range: f32, // 0.15m
|
||||
}
|
||||
|
||||
// Log-odds update parameters
|
||||
pub struct LogOddsConfig {
|
||||
pub l_hit: i16, // Evidence for obstacle
|
||||
pub l_miss: i16, // Evidence for free space
|
||||
pub l_occupied_threshold: i16, // Wall classification threshold
|
||||
pub l_free_threshold: i16, // Floor classification threshold
|
||||
}
|
||||
```
|
||||
|
||||
## Usage Examples
|
||||
|
||||
### Basic Grid Operations
|
||||
|
||||
```rust
|
||||
use vastu_slam::grid::{GridStorage, GridConfig, MapConfig};
|
||||
use vastu_slam::core::{WorldPoint, GridCoord, CellType};
|
||||
|
||||
// Create centered grid (20m × 20m at 2.5cm resolution)
|
||||
let config = MapConfig::default();
|
||||
let mut storage = GridStorage::centered(
|
||||
config.grid.initial_width,
|
||||
config.grid.initial_height,
|
||||
config.grid.resolution,
|
||||
);
|
||||
|
||||
// Coordinate conversion
|
||||
let world = WorldPoint::new(1.0, 2.0);
|
||||
let grid = storage.world_to_grid(world); // GridCoord
|
||||
let back = storage.grid_to_world(grid); // Cell center
|
||||
|
||||
// Cell access
|
||||
let cell_type = storage.get_type(grid);
|
||||
storage.set_type_with_priority(grid, CellType::Wall);
|
||||
```
|
||||
|
||||
### Processing Lidar Scans
|
||||
|
||||
```rust
|
||||
use vastu_slam::grid::lidar_update::update_from_lidar;
|
||||
|
||||
let result = update_from_lidar(
|
||||
&mut storage,
|
||||
&lidar_scan,
|
||||
robot_pose,
|
||||
&sensor_config,
|
||||
&grid_config,
|
||||
);
|
||||
|
||||
println!("Updated {} cells, {} walls, {} floor",
|
||||
result.cells_updated, result.cells_wall, result.cells_floor);
|
||||
```
|
||||
|
||||
### Log-Odds Queries
|
||||
|
||||
```rust
|
||||
// Get occupancy probability at a position
|
||||
let prob = storage.get_probability(coord); // 0.0 - 1.0
|
||||
|
||||
// Check occupancy status
|
||||
if storage.is_occupied(coord) {
|
||||
// P > 0.62 (L > 50)
|
||||
}
|
||||
if storage.is_free(coord) {
|
||||
// P < 0.38 (L < -50)
|
||||
}
|
||||
|
||||
// Raw log-odds value
|
||||
let log_odds = storage.get_log_odds(coord); // i16
|
||||
```
|
||||
|
||||
### Distance Field Queries
|
||||
|
||||
```rust
|
||||
// Distance to nearest wall (meters)
|
||||
let dist = storage.get_distance(coord);
|
||||
|
||||
// Interpolated distance with gradients (for optimization)
|
||||
let (dist, grad_x, grad_y) = storage.get_distance_interpolated(world_point);
|
||||
```
|
||||
|
||||
## Ray Casting
|
||||
|
||||
### Bresenham Line Algorithm
|
||||
|
||||
Efficient integer-based ray tracing:
|
||||
|
||||
```rust
|
||||
use vastu_slam::grid::raycaster::{BresenhamLine, cells_along_ray};
|
||||
|
||||
// Iterate cells along a ray
|
||||
let start = GridCoord::new(0, 0);
|
||||
let end = GridCoord::new(10, 5);
|
||||
|
||||
for cell in BresenhamLine::new(start, end) {
|
||||
// Process each cell
|
||||
}
|
||||
|
||||
// Or collect all cells
|
||||
let cells = cells_along_ray(start, end);
|
||||
```
|
||||
|
||||
### Continuous Ray Casting
|
||||
|
||||
For queries requiring distance information:
|
||||
|
||||
```rust
|
||||
use vastu_slam::grid::raycaster::RayCast;
|
||||
|
||||
let ray = RayCast::new(
|
||||
start_point, // WorldPoint
|
||||
angle, // Radians
|
||||
max_range, // Meters
|
||||
grid_origin, // Grid origin
|
||||
resolution, // Grid resolution
|
||||
);
|
||||
|
||||
for (coord, distance) in ray {
|
||||
// coord: GridCoord, distance: f32
|
||||
}
|
||||
```
|
||||
|
||||
## SIMD Operations
|
||||
|
||||
### Cell Counting
|
||||
|
||||
```rust
|
||||
// SIMD-optimized (processes 16 cells per iteration)
|
||||
let counts = storage.count_by_type();
|
||||
|
||||
println!("Floor: {}, Wall: {}, Cliff: {}, Bump: {}",
|
||||
counts.floor, counts.wall, counts.cliff, counts.bump);
|
||||
|
||||
// Area calculation
|
||||
let explored_area = counts.known() as f32 * resolution * resolution;
|
||||
```
|
||||
|
||||
### Rect Traversability Check
|
||||
|
||||
```rust
|
||||
// SIMD-optimized check if rectangular region is all Floor
|
||||
let all_floor = storage.is_rect_all_floor(
|
||||
start_x, start_y,
|
||||
width, height,
|
||||
);
|
||||
```
|
||||
|
||||
## Grid Expansion
|
||||
|
||||
Dynamic grid growth when robot explores beyond bounds:
|
||||
|
||||
```rust
|
||||
// Auto-expand is typically enabled via config
|
||||
let config = GridConfig {
|
||||
auto_expand: true,
|
||||
max_width: 2000, // Limit to 50m
|
||||
max_height: 2000,
|
||||
..Default::default()
|
||||
};
|
||||
|
||||
// Manual expansion
|
||||
let expanded = storage.expand_to_include(
|
||||
far_point,
|
||||
max_width,
|
||||
max_height,
|
||||
);
|
||||
```
|
||||
|
||||
Expansion preserves all existing cell data and adjusts the origin.
|
||||
|
||||
## Memory Considerations
|
||||
|
||||
| Grid Size | Resolution | Memory |
|
||||
|-----------|------------|--------|
|
||||
| 20m × 20m | 2.5cm | ~2.5 MB |
|
||||
| 50m × 50m | 2.5cm | ~16 MB |
|
||||
| 100m × 100m | 5cm | ~16 MB |
|
||||
|
||||
Memory per cell: ~10 bytes (types + confidence + observations + log_odds + distance)
|
||||
|
||||
## Distance Field
|
||||
|
||||
The distance field stores the Euclidean distance to the nearest wall cell:
|
||||
|
||||
```
|
||||
Wall cells: distance = 0.0
|
||||
Adjacent cells: distance = resolution
|
||||
Further cells: distance = Euclidean distance to nearest wall
|
||||
Max distance: Capped at 1.0m by default
|
||||
```
|
||||
|
||||
**Uses:**
|
||||
- Gaussian scoring in scan matching
|
||||
- Navigation cost maps
|
||||
- Obstacle inflation
|
||||
|
||||
**Update Methods:**
|
||||
- `update_distance_field_from_wall(coord)` - Single wall update
|
||||
- `update_distance_field_batch(&[coords])` - Batch update (faster)
|
||||
- `recompute_distance_field()` - Full recomputation
|
||||
@@ -0,0 +1,90 @@
|
||||
//! Occupancy grid implementation with multi-sensor fusion.
|
||||
//!
|
||||
//! This module provides the core occupancy grid storage and update mechanisms
|
||||
//! for VastuSLAM. The grid uses a Structure-of-Arrays (SoA) layout for
|
||||
//! SIMD-optimized operations and log-odds Bayesian updates for robust
|
||||
//! occupancy estimation.
|
||||
//!
|
||||
//! ## Architecture
|
||||
//!
|
||||
//! ```text
|
||||
//! ┌─────────────────────────────────────────────────────────────┐
|
||||
//! │ Sensor Inputs │
|
||||
//! │ LidarScan (5Hz) │ CliffSensors (110Hz) │ BumperSensors│
|
||||
//! └─────────┬──────────┴───────────┬────────────┴───────┬───────┘
|
||||
//! │ │ │
|
||||
//! ▼ ▼ ▼
|
||||
//! ┌─────────────────┐ ┌─────────────────┐ ┌─────────────────┐
|
||||
//! │ lidar_update │ │ cliff_update │ │ bumper_update │
|
||||
//! │ (log-odds) │ │ (priority) │ │ (priority) │
|
||||
//! └────────┬────────┘ └────────┬────────┘ └────────┬────────┘
|
||||
//! │ │ │
|
||||
//! └──────────────────────┼─────────────────────┘
|
||||
//! ▼
|
||||
//! ┌───────────────────────┐
|
||||
//! │ GridStorage │
|
||||
//! │ (SoA, SIMD-ready) │
|
||||
//! └───────────────────────┘
|
||||
//! ```
|
||||
//!
|
||||
//! ## Key Components
|
||||
//!
|
||||
//! - [`GridStorage`]: Core storage using SoA layout with log-odds occupancy
|
||||
//! - [`GridConfig`]: Grid dimensions, resolution, and expansion settings
|
||||
//! - [`SensorConfig`]: Robot geometry and sensor parameters
|
||||
//! - [`LogOddsConfig`]: Bayesian update parameters (hit/miss log-odds)
|
||||
//! - [`MapConfig`]: Combined configuration for the full map
|
||||
//!
|
||||
//! ## Sensor Update Modules
|
||||
//!
|
||||
//! - [`lidar_update`]: Process 360° lidar scans using Bresenham ray casting
|
||||
//! - [`cliff_update`]: Mark cliff sensor detections with priority
|
||||
//! - [`bumper_update`]: Mark bumper collisions with highest priority
|
||||
//! - [`raycaster`]: Bresenham line algorithm for efficient ray tracing
|
||||
//!
|
||||
//! ## Log-Odds Model
|
||||
//!
|
||||
//! The grid uses log-odds representation for probabilistic occupancy:
|
||||
//!
|
||||
//! ```text
|
||||
//! L(x) = log(P(x) / (1 - P(x))) # Log-odds conversion
|
||||
//! L_new = L_old + L_observation # Bayesian update
|
||||
//!
|
||||
//! Thresholds:
|
||||
//! L > 50 → Occupied (Wall)
|
||||
//! L < -50 → Free (Floor)
|
||||
//! else → Unknown
|
||||
//! ```
|
||||
//!
|
||||
//! ## Example
|
||||
//!
|
||||
//! ```rust,ignore
|
||||
//! use vastu_slam::grid::{GridStorage, MapConfig, lidar_update};
|
||||
//!
|
||||
//! // Create grid from configuration
|
||||
//! let config = MapConfig::default();
|
||||
//! let mut storage = GridStorage::centered(800, 800, 0.025);
|
||||
//!
|
||||
//! // Process lidar scan
|
||||
//! let result = lidar_update::update_from_lidar(
|
||||
//! &mut storage,
|
||||
//! &lidar_scan,
|
||||
//! robot_pose,
|
||||
//! &config.sensor,
|
||||
//! &config.grid,
|
||||
//! );
|
||||
//!
|
||||
//! // Query occupancy
|
||||
//! let prob = storage.get_probability(coord); // 0.0 - 1.0
|
||||
//! let is_wall = storage.is_occupied(coord); // L > 50
|
||||
//! ```
|
||||
|
||||
pub mod bumper_update;
|
||||
pub mod cliff_update;
|
||||
mod config;
|
||||
pub mod lidar_update;
|
||||
pub mod raycaster;
|
||||
mod storage;
|
||||
|
||||
pub use config::{ConfigError, GridConfig, LogOddsConfig, MapConfig, SensorConfig};
|
||||
pub use storage::{CellCounts, CellMut, GridStorage};
|
||||
@@ -3,6 +3,53 @@
|
||||
//! This module provides efficient line drawing algorithms for:
|
||||
//! - Marking cells along lidar rays as free (Floor)
|
||||
//! - Marking endpoint cells as occupied (Wall)
|
||||
//!
|
||||
//! ## Algorithms
|
||||
//!
|
||||
//! ### Bresenham Line Algorithm
|
||||
//!
|
||||
//! Integer-only algorithm for drawing lines between grid cells.
|
||||
//! Optimal for ray tracing in occupancy grids:
|
||||
//!
|
||||
//! ```text
|
||||
//! From (0,0) to (7,3):
|
||||
//!
|
||||
//! 3 │ ●
|
||||
//! 2 │ ●●
|
||||
//! 1 │ ●●
|
||||
//! 0 ●●
|
||||
//! └──────────
|
||||
//! 0 1 2 3 4 5 6 7
|
||||
//! ```
|
||||
//!
|
||||
//! **Advantages:**
|
||||
//! - No floating-point operations
|
||||
//! - Deterministic cell coverage
|
||||
//! - No gaps in traced line
|
||||
//!
|
||||
//! ### Continuous Ray Casting
|
||||
//!
|
||||
//! Floating-point ray stepping for distance-aware queries.
|
||||
//! Useful when distance information is needed:
|
||||
//!
|
||||
//! ```text
|
||||
//! start ●────●────●────●────● end
|
||||
//! d=0 d=0.1 d=0.2 ...
|
||||
//! ```
|
||||
//!
|
||||
//! ## Usage
|
||||
//!
|
||||
//! ```rust,ignore
|
||||
//! use vastu_slam::grid::raycaster::{BresenhamLine, cells_along_ray};
|
||||
//!
|
||||
//! // Iterate cells along a ray
|
||||
//! for cell in BresenhamLine::new(start, end) {
|
||||
//! // Apply log-odds update
|
||||
//! }
|
||||
//!
|
||||
//! // Collect all cells (excluding endpoint for miss updates)
|
||||
//! let cells = cells_along_ray_excluding_end(start, end);
|
||||
//! ```
|
||||
|
||||
use crate::core::{GridCoord, WorldPoint};
|
||||
|
||||
@@ -3,6 +3,30 @@
|
||||
//! Uses Structure-of-Arrays (SoA) layout for SIMD optimization.
|
||||
//! Cell data is stored in separate arrays for each field, enabling
|
||||
//! efficient vectorized operations on ARM NEON and x86 SSE/AVX.
|
||||
//!
|
||||
//! ## Memory Layout
|
||||
//!
|
||||
//! ```text
|
||||
//! Traditional AoS: [Cell₀] [Cell₁] [Cell₂] [Cell₃] ...
|
||||
//! ↓ ↓ ↓ ↓
|
||||
//! TCOSLTCOSLTCOSLTCOSL (interleaved)
|
||||
//!
|
||||
//! Our SoA Layout: Types: [T T T T T T T T T T T T T T T T|...]
|
||||
//! Conf: [C C C C C C C C C C C C C C C C|...]
|
||||
//! Obs: [O O O O O O O O O O O O O O O O|...]
|
||||
//! Swept: [S S S S S S S S S S S S S S S S|...]
|
||||
//! LogOdds: [L L L L L L L L L L L L L L L L|...]
|
||||
//! Distance: [D D D D D D D D D D D D D D D D|...]
|
||||
//! └──────── 16 bytes ────────────┘
|
||||
//! SIMD u8x16 load
|
||||
//! ```
|
||||
//!
|
||||
//! ## Key Types
|
||||
//!
|
||||
//! - [`GridStorage`]: Main storage struct with coordinate conversion,
|
||||
//! log-odds updates, and SIMD-optimized queries
|
||||
//! - [`CellMut`]: Mutable reference to a single cell's fields
|
||||
//! - [`CellCounts`]: Statistics for each cell type
|
||||
|
||||
mod core;
|
||||
mod types;
|
||||
@@ -0,0 +1,334 @@
|
||||
# IO Module
|
||||
|
||||
Map persistence and export for VastuSLAM.
|
||||
|
||||
## Module Structure
|
||||
|
||||
```
|
||||
io/
|
||||
├── mod.rs # Module exports and documentation
|
||||
├── vastu_format.rs # Native .vastu binary format
|
||||
├── pgm.rs # ROS-compatible PGM/YAML export
|
||||
├── svg.rs # SVG visualization with trajectories
|
||||
└── scenario.rs # YAML-based test scenario definitions
|
||||
```
|
||||
|
||||
## Format Comparison
|
||||
|
||||
| Format | Purpose | Preserves |
|
||||
|--------|---------|-----------|
|
||||
| `.vastu` | Native persistence | All cell data (type, confidence, swept) |
|
||||
| `.pgm` + `.yaml` | ROS integration | Occupancy only (Free/Occupied/Unknown) |
|
||||
| `.svg` | Visualization/audit | Visual map + trajectories |
|
||||
|
||||
## Native Vastu Format
|
||||
|
||||
Binary format preserving full cell information:
|
||||
|
||||
### File Structure
|
||||
|
||||
```
|
||||
┌────────────────────────────────────┐
|
||||
│ Header (32 bytes) │
|
||||
├────────────────────────────────────┤
|
||||
│ Magic: "VASTU" (5 bytes) │
|
||||
│ Version: u8 (1 byte) │
|
||||
│ Width: u32 LE (4 bytes) │
|
||||
│ Height: u32 LE (4 bytes) │
|
||||
│ Resolution: f32 LE (4 bytes) │
|
||||
│ Origin X: f32 LE (4 bytes) │
|
||||
│ Origin Y: f32 LE (4 bytes) │
|
||||
│ Reserved (6 bytes) │
|
||||
├────────────────────────────────────┤
|
||||
│ Cell Data (W × H bytes) │
|
||||
│ Each byte: [type:4][flags:4] │
|
||||
│ type: 0=Unknown, 1=Floor, 2=Wall, │
|
||||
│ 3=Cliff, 4=Bump │
|
||||
│ flags: 0x80 = swept flag │
|
||||
└────────────────────────────────────┘
|
||||
```
|
||||
|
||||
### Usage
|
||||
|
||||
```rust
|
||||
use vastu_slam::io::{save_vastu, load_vastu};
|
||||
use std::path::Path;
|
||||
|
||||
// Save map
|
||||
save_vastu(map.storage(), Path::new("map.vastu"))?;
|
||||
|
||||
// Load map
|
||||
let storage = load_vastu(Path::new("map.vastu"))?;
|
||||
```
|
||||
|
||||
### Streaming API
|
||||
|
||||
```rust
|
||||
use vastu_slam::io::{write_vastu, read_vastu};
|
||||
use std::io::Cursor;
|
||||
|
||||
// Write to any Writer
|
||||
let mut buffer = Vec::new();
|
||||
write_vastu(&storage, &mut buffer)?;
|
||||
|
||||
// Read from any Reader
|
||||
let mut cursor = Cursor::new(buffer);
|
||||
let loaded = read_vastu(&mut cursor)?;
|
||||
```
|
||||
|
||||
## ROS Map Export (PGM)
|
||||
|
||||
Creates ROS map_server compatible files:
|
||||
|
||||
### Gray Values
|
||||
|
||||
| Value | Meaning | Cell Types |
|
||||
|-------|---------|------------|
|
||||
| 254 | Free | Floor |
|
||||
| 0 | Occupied | Wall, Cliff, Bump |
|
||||
| 205 | Unknown | Unknown |
|
||||
|
||||
### Usage
|
||||
|
||||
```rust
|
||||
use vastu_slam::io::export_ros_map;
|
||||
use std::path::Path;
|
||||
|
||||
// Creates map.pgm and map.yaml
|
||||
export_ros_map(storage, Path::new("map"))?;
|
||||
```
|
||||
|
||||
### Output Files
|
||||
|
||||
**map.pgm** (binary grayscale image):
|
||||
```
|
||||
P5
|
||||
# Generated by vastu-map
|
||||
800 800
|
||||
255
|
||||
[binary pixel data]
|
||||
```
|
||||
|
||||
**map.yaml** (ROS metadata):
|
||||
```yaml
|
||||
image: map.pgm
|
||||
resolution: 0.025
|
||||
origin: [-10.0, -10.0, 0.0]
|
||||
negate: 0
|
||||
occupied_thresh: 0.65
|
||||
free_thresh: 0.196
|
||||
```
|
||||
|
||||
### Individual Exports
|
||||
|
||||
```rust
|
||||
use vastu_slam::io::{export_pgm, export_yaml, write_pgm};
|
||||
|
||||
// Just PGM
|
||||
export_pgm(&storage, Path::new("map.pgm"))?;
|
||||
|
||||
// Just YAML
|
||||
export_yaml(&storage, "map.pgm", Path::new("map.yaml"))?;
|
||||
|
||||
// To any Writer
|
||||
let mut buffer = Vec::new();
|
||||
write_pgm(&storage, &mut buffer)?;
|
||||
```
|
||||
|
||||
## SVG Visualization
|
||||
|
||||
Renders maps with trajectory overlays for auditing:
|
||||
|
||||
### Basic Usage
|
||||
|
||||
```rust
|
||||
use vastu_slam::io::{SvgVisualizer, SvgConfig, markers_by_distance};
|
||||
use std::path::Path;
|
||||
|
||||
let config = SvgConfig::default();
|
||||
let visualizer = SvgVisualizer::new(storage, config)
|
||||
.with_title("SLAM Test Run")
|
||||
.with_ground_truth(gt_poses, markers_by_distance(>_poses, 0.5))
|
||||
.with_estimated(est_poses, markers_by_distance(&est_poses, 0.5))
|
||||
.with_odometry(odom_poses, vec![]);
|
||||
|
||||
visualizer.save(Path::new("output.svg"))?;
|
||||
```
|
||||
|
||||
### Configuration
|
||||
|
||||
```rust
|
||||
use vastu_slam::io::{SvgConfig, SvgColorScheme};
|
||||
|
||||
let config = SvgConfig {
|
||||
scale: 50.0, // Pixels per meter
|
||||
trajectory_width: 2.0, // Line width
|
||||
marker_radius: 4.0, // Pose marker size
|
||||
padding: 20.0, // Margin around map
|
||||
colors: SvgColorScheme {
|
||||
wall: "#333333",
|
||||
floor: "#FFFFFF",
|
||||
unknown: "#CCCCCC",
|
||||
gt_trajectory: "#22AA22", // Green
|
||||
estimated_trajectory: "#2222AA", // Blue
|
||||
odom_trajectory: "#AA2222", // Red
|
||||
},
|
||||
};
|
||||
```
|
||||
|
||||
### Marker Strategies
|
||||
|
||||
```rust
|
||||
use vastu_slam::io::{markers_by_distance, markers_by_time};
|
||||
|
||||
// Marker every 0.5 meters traveled
|
||||
let markers = markers_by_distance(&poses, 0.5);
|
||||
|
||||
// Marker every 1.0 seconds (given total time and pose count)
|
||||
let markers = markers_by_time(poses.len(), total_time_secs, 1.0);
|
||||
```
|
||||
|
||||
### Output Structure
|
||||
|
||||
The SVG includes:
|
||||
- Background grid of cells (Floor/Wall/Unknown)
|
||||
- Trajectory paths (lines)
|
||||
- Pose markers with direction indicators
|
||||
- Legend explaining colors
|
||||
|
||||
## Test Scenarios
|
||||
|
||||
YAML-based test definitions for reproducible testing:
|
||||
|
||||
### Example Scenario
|
||||
|
||||
```yaml
|
||||
name: "Square Path Test"
|
||||
description: "Robot drives in a 2m square"
|
||||
map: "simple_room"
|
||||
|
||||
start_pose:
|
||||
x: 2.0
|
||||
y: 2.0
|
||||
theta: 0.0
|
||||
|
||||
seed:
|
||||
scans: 20
|
||||
rotation: true
|
||||
|
||||
commands:
|
||||
# Forward 2m
|
||||
- type: move
|
||||
linear_velocity: 0.2
|
||||
angular_velocity: 0.0
|
||||
duration_ms: 10000
|
||||
|
||||
# Turn 90° left
|
||||
- type: move
|
||||
linear_velocity: 0.0
|
||||
angular_velocity: 0.5
|
||||
duration_ms: 3140
|
||||
|
||||
# Repeat for square...
|
||||
|
||||
svg_output:
|
||||
scale: 100.0
|
||||
marker_interval_m: 0.5
|
||||
trajectory_width: 3.0
|
||||
```
|
||||
|
||||
### Loading Scenarios
|
||||
|
||||
```rust
|
||||
use vastu_slam::io::{Scenario, Command};
|
||||
use std::path::Path;
|
||||
|
||||
// Load from file
|
||||
let scenario = Scenario::load(Path::new("scenarios/test.yaml"))?;
|
||||
|
||||
println!("Running: {}", scenario.name);
|
||||
println!("Duration: {:.1}s", scenario.total_duration_secs());
|
||||
|
||||
// Process commands
|
||||
for cmd in &scenario.commands {
|
||||
let (linear, angular) = cmd.velocities();
|
||||
let duration = cmd.duration_secs();
|
||||
// Execute motion...
|
||||
}
|
||||
|
||||
// Resolve map path
|
||||
let map_path = scenario.resolve_map_path(Path::new("maps"));
|
||||
```
|
||||
|
||||
### Command Types
|
||||
|
||||
```yaml
|
||||
# Movement command
|
||||
- type: move
|
||||
linear_velocity: 0.2 # m/s (forward positive)
|
||||
angular_velocity: 0.5 # rad/s (CCW positive)
|
||||
duration_ms: 5000 # milliseconds
|
||||
|
||||
# Stationary command
|
||||
- type: stay
|
||||
duration_ms: 1000
|
||||
```
|
||||
|
||||
### Seed Configuration
|
||||
|
||||
Initial map building before test starts:
|
||||
|
||||
```yaml
|
||||
seed:
|
||||
scans: 20 # Number of scans to build initial map
|
||||
rotation: true # 360° rotation for full coverage
|
||||
```
|
||||
|
||||
### SVG Output Configuration
|
||||
|
||||
```yaml
|
||||
svg_output:
|
||||
filename: "custom_output.svg" # Optional, defaults to scenario name
|
||||
scale: 50.0 # Pixels per meter
|
||||
marker_interval_m: 0.5 # Distance between markers
|
||||
marker_interval_s: 1.0 # Time between markers (alternative)
|
||||
show_scan_points: false # Show individual scan points
|
||||
trajectory_width: 2.0 # Line width
|
||||
```
|
||||
|
||||
## Error Handling
|
||||
|
||||
All I/O operations return typed errors:
|
||||
|
||||
```rust
|
||||
use vastu_slam::io::{IoError, ScenarioError};
|
||||
|
||||
// Vastu/PGM errors
|
||||
match load_vastu(path) {
|
||||
Ok(storage) => { /* use storage */ }
|
||||
Err(IoError::Io(msg)) => eprintln!("File error: {}", msg),
|
||||
Err(IoError::InvalidFormat(msg)) => eprintln!("Bad format: {}", msg),
|
||||
Err(IoError::VersionMismatch { expected, found }) => {
|
||||
eprintln!("Version {} != {}", found, expected);
|
||||
}
|
||||
}
|
||||
|
||||
// Scenario errors
|
||||
match Scenario::load(path) {
|
||||
Ok(scenario) => { /* use scenario */ }
|
||||
Err(ScenarioError::Io(msg)) => eprintln!("File error: {}", msg),
|
||||
Err(ScenarioError::Parse(msg)) => eprintln!("YAML error: {}", msg),
|
||||
Err(ScenarioError::InvalidMap(msg)) => eprintln!("Map error: {}", msg),
|
||||
}
|
||||
```
|
||||
|
||||
## Performance Notes
|
||||
|
||||
| Operation | Typical Size | Time |
|
||||
|-----------|--------------|------|
|
||||
| Save .vastu (800×800) | ~640KB | <10ms |
|
||||
| Load .vastu (800×800) | ~640KB | <10ms |
|
||||
| Export PGM (800×800) | ~640KB | <10ms |
|
||||
| Render SVG | Varies | <100ms |
|
||||
|
||||
The formats are designed for quick persistence during operation.
|
||||
@@ -10,7 +10,7 @@
|
||||
//! ## Saving and Loading Maps
|
||||
//!
|
||||
//! ```rust,ignore
|
||||
//! use vastu_map::io::{save_vastu, load_vastu};
|
||||
//! use vastu_slam::io::{save_vastu, load_vastu};
|
||||
//! use std::path::Path;
|
||||
//!
|
||||
//! // Save map
|
||||
@@ -25,7 +25,7 @@
|
||||
//! Export maps in ROS map_server format:
|
||||
//!
|
||||
//! ```rust,ignore
|
||||
//! use vastu_map::io::export_ros_map;
|
||||
//! use vastu_slam::io::export_ros_map;
|
||||
//! use std::path::Path;
|
||||
//!
|
||||
//! // Creates map.pgm and map.yaml
|
||||
@@ -37,7 +37,7 @@
|
||||
//! Generate SVG audit files with map and trajectory visualization:
|
||||
//!
|
||||
//! ```rust,ignore
|
||||
//! use vastu_map::io::{SvgVisualizer, SvgConfig, markers_by_distance};
|
||||
//! use vastu_slam::io::{SvgVisualizer, SvgConfig, markers_by_distance};
|
||||
//!
|
||||
//! let visualizer = SvgVisualizer::new(storage, SvgConfig::default())
|
||||
//! .with_title("Test Run")
|
||||
@@ -1,10 +1,10 @@
|
||||
//! # Vastu-Map
|
||||
//! # VastuSLAM
|
||||
//!
|
||||
//! Occupancy grid mapping library for robot exploration and navigation.
|
||||
//! 2D SLAM library with occupancy grid mapping, scan matching, and loop closure.
|
||||
//!
|
||||
//! ## Overview
|
||||
//!
|
||||
//! Vastu-Map provides a multi-layer occupancy grid with semantic cell types:
|
||||
//! VastuSLAM provides a multi-layer occupancy grid with semantic cell types:
|
||||
//!
|
||||
//! - **Unknown** - Not yet observed by any sensor
|
||||
//! - **Floor** - Traversable surface
|
||||
@@ -14,14 +14,15 @@
|
||||
//!
|
||||
//! ## Features
|
||||
//!
|
||||
//! - **Map Exploration**: Frontier-based autonomous exploration
|
||||
//! - **Path Finding**: A* on occupancy grid with safety margins
|
||||
//! - **Coverage Planning**: Full surface sweeping (future)
|
||||
//! - **Occupancy Grid Mapping**: Multi-sensor integration with priority-based updates
|
||||
//! - **Scan Matching**: Correlative and branch-and-bound scan-to-map alignment
|
||||
//! - **Loop Closure**: LiDAR-IRIS descriptors for place recognition
|
||||
//! - **Submap Architecture**: Cartographer-style reversible map updates
|
||||
//!
|
||||
//! ## Quick Start
|
||||
//!
|
||||
//! ```rust,ignore
|
||||
//! use vastu_map::{OccupancyGridMap, MapConfig, Pose2D, LidarScan};
|
||||
//! use vastu_slam::{OccupancyGridMap, MapConfig, Pose2D, LidarScan};
|
||||
//!
|
||||
//! // Create map with default configuration
|
||||
//! let config = MapConfig::default();
|
||||
@@ -54,40 +55,35 @@ pub mod grid;
|
||||
// Unified configuration
|
||||
pub mod config;
|
||||
|
||||
// Map queries (frontier detection, etc.)
|
||||
pub mod query;
|
||||
|
||||
// Path planning
|
||||
pub mod pathfinding;
|
||||
|
||||
// SLAM (scan matching, loop closure)
|
||||
pub mod slam;
|
||||
// Scan matching and loop closure
|
||||
pub mod matching;
|
||||
|
||||
// Submap architecture for reversible map updates
|
||||
pub mod submap;
|
||||
|
||||
// High-level exploration API
|
||||
pub mod explore;
|
||||
|
||||
/// Backward compatibility: re-export exploration types from explore::controller
|
||||
#[deprecated(note = "Use `explore::*` instead - exploration types are now in explore module")]
|
||||
pub mod exploration {
|
||||
pub use crate::explore::controller::*;
|
||||
}
|
||||
// SLAM operation modes (localization, etc.)
|
||||
pub mod modes;
|
||||
|
||||
// Persistence (save/load)
|
||||
pub mod io;
|
||||
|
||||
// Re-export commonly used types
|
||||
pub use core::{
|
||||
BumperSensors, Cell, CellType, CliffSensors, GridCoord, LidarScan, Pose2D, SensorObservation,
|
||||
WorldPoint,
|
||||
BumperSensors, Cell, CellType, CliffSensors, GridCoord, LidarScan, MotionModel, Odometry,
|
||||
Pose2D, SensorObservation, WorldPoint,
|
||||
};
|
||||
|
||||
pub use grid::{CellCounts, ConfigError, GridConfig, GridStorage, MapConfig, SensorConfig};
|
||||
|
||||
pub use config::{ConfigLoadError, VastuConfig};
|
||||
|
||||
pub use matching::{
|
||||
CorrelativeMatcher, CorrelativeMatcherConfig, LoopClosureDetector, LoopValidator, RobustKernel,
|
||||
ScanMatchResult, ScanMatcher,
|
||||
};
|
||||
|
||||
pub use modes::{LocalizationResult, Localizer, LocalizerConfig};
|
||||
|
||||
pub use submap::{
|
||||
InsertResult, MultiSubmapMatchConfig, MultiSubmapMatchResult, MultiSubmapMatcher, StoredScan,
|
||||
Submap, SubmapConfig, SubmapCorrection, SubmapGraphConfig, SubmapId, SubmapLoopClosure,
|
||||
@@ -107,8 +103,6 @@ pub struct ObserveResult {
|
||||
pub cells_cliff: usize,
|
||||
/// Cells marked as Bump (invisible obstacle)
|
||||
pub cells_bump: usize,
|
||||
/// Frontiers changed?
|
||||
pub frontiers_changed: bool,
|
||||
}
|
||||
|
||||
impl ObserveResult {
|
||||
@@ -119,7 +113,6 @@ impl ObserveResult {
|
||||
self.cells_wall += other.cells_wall;
|
||||
self.cells_cliff += other.cells_cliff;
|
||||
self.cells_bump += other.cells_bump;
|
||||
self.frontiers_changed |= other.frontiers_changed;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -138,8 +131,6 @@ pub struct MapCoverageStats {
|
||||
pub bump_cells: usize,
|
||||
/// Unknown cells still in grid
|
||||
pub unknown_cells: usize,
|
||||
/// Number of active frontiers
|
||||
pub frontier_count: usize,
|
||||
/// Estimated explored area (m²)
|
||||
pub explored_area_m2: f32,
|
||||
}
|
||||
@@ -153,7 +144,6 @@ impl From<CellCounts> for MapCoverageStats {
|
||||
cliff_cells: counts.cliff,
|
||||
bump_cells: counts.bump,
|
||||
unknown_cells: counts.unknown,
|
||||
frontier_count: 0,
|
||||
explored_area_m2: 0.0,
|
||||
}
|
||||
}
|
||||
@@ -336,9 +326,9 @@ impl OccupancyGridMap {
|
||||
&mut self,
|
||||
scan: &LidarScan,
|
||||
encoder_pose: Pose2D,
|
||||
matcher_config: &slam::CorrelativeMatcherConfig,
|
||||
) -> (ObserveResult, Pose2D, slam::ScanMatchResult) {
|
||||
let matcher = slam::CorrelativeMatcher::new(matcher_config.clone());
|
||||
matcher_config: &matching::CorrelativeMatcherConfig,
|
||||
) -> (ObserveResult, Pose2D, matching::ScanMatchResult) {
|
||||
let matcher = matching::CorrelativeMatcher::new(matcher_config.clone());
|
||||
let match_result = matcher.match_scan(scan, encoder_pose, &self.storage);
|
||||
|
||||
// Use matched pose if converged, otherwise fall back to encoder pose
|
||||
@@ -0,0 +1,324 @@
|
||||
# Matching Module
|
||||
|
||||
Scan-to-map matching and loop closure capabilities for correcting encoder drift during mapping.
|
||||
|
||||
## Module Structure
|
||||
|
||||
```
|
||||
matching/
|
||||
├── mod.rs # Module exports
|
||||
├── config.rs # CorrelativeMatcherConfig
|
||||
├── types.rs # ScanMatchResult, MatchQuality
|
||||
├── traits.rs # ScanMatcher trait
|
||||
├── robust.rs # Robust loss functions (Huber, Cauchy)
|
||||
├── matcher/ # Correlative scan matcher
|
||||
│ ├── mod.rs # Module tests
|
||||
│ ├── core.rs # CorrelativeMatcher implementation
|
||||
│ ├── lm.rs # Adaptive Levenberg-Marquardt optimizer
|
||||
│ ├── helpers.rs # Utility functions
|
||||
│ └── types.rs # ScratchBuffers for SIMD
|
||||
├── branch_bound/ # Branch-and-bound matcher
|
||||
│ ├── mod.rs # Module exports and tests
|
||||
│ ├── config.rs # BranchBoundConfig, BranchBoundResult
|
||||
│ ├── grids.rs # PrecomputedGrids (multi-resolution)
|
||||
│ ├── algorithm.rs # Search algorithm
|
||||
│ └── search_node.rs # Priority queue node
|
||||
├── loop_closure/ # Loop closure detection
|
||||
│ ├── mod.rs # Module exports
|
||||
│ ├── descriptor.rs # LiDAR-IRIS descriptor
|
||||
│ ├── detector.rs # LoopClosureDetector
|
||||
│ ├── validator.rs # Geometric validation
|
||||
│ └── graph.rs # PoseGraph with optimization
|
||||
└── background_optimizer.rs # Async pose graph optimization
|
||||
```
|
||||
|
||||
## Scan Matching Pipeline
|
||||
|
||||
```
|
||||
┌──────────────────────────────────────────────────────────────────────┐
|
||||
│ SCAN MATCHING PIPELINE │
|
||||
├──────────────────────────────────────────────────────────────────────┤
|
||||
│ │
|
||||
│ LidarScan + Encoder Pose │
|
||||
│ │ │
|
||||
│ ▼ │
|
||||
│ ┌─────────────────────────────────────────────────────────────────┐ │
|
||||
│ │ COARSE SEARCH (Multi-Resolution) │ │
|
||||
│ │ ┌──────────────────────────────────────────────────────────┐ │ │
|
||||
│ │ │ Search Window: ±30cm × ±30cm × ±8.6° │ │ │
|
||||
│ │ │ Resolution: 4cm linear, 2.3° angular │ │ │
|
||||
│ │ │ Method: Brute-force grid search │ │ │
|
||||
│ │ │ Scoring: Gaussian (distance field) or binary (cell type) │ │ │
|
||||
│ │ └──────────────────────────────────────────────────────────┘ │ │
|
||||
│ └───────────────────────────┬─────────────────────────────────────┘ │
|
||||
│ │ Best candidate │
|
||||
│ ▼ │
|
||||
│ ┌─────────────────────────────────────────────────────────────────┐ │
|
||||
│ │ FINE SEARCH │ │
|
||||
│ │ ┌──────────────────────────────────────────────────────────┐ │ │
|
||||
│ │ │ Search Window: ±6cm × ±6cm × ±3.4° │ │ │
|
||||
│ │ │ Resolution: 2cm linear, 1.1° angular │ │ │
|
||||
│ │ │ Centered on coarse result, penalized by prior │ │ │
|
||||
│ │ └──────────────────────────────────────────────────────────┘ │ │
|
||||
│ └───────────────────────────┬─────────────────────────────────────┘ │
|
||||
│ │ Refined candidate │
|
||||
│ ▼ │
|
||||
│ ┌─────────────────────────────────────────────────────────────────┐ │
|
||||
│ │ GAUSS-NEWTON REFINEMENT │ │
|
||||
│ │ ┌──────────────────────────────────────────────────────────┐ │ │
|
||||
│ │ │ Method: Adaptive Levenberg-Marquardt │ │ │
|
||||
│ │ │ Objective: Minimize distance to walls + prior constraint │ │ │
|
||||
│ │ │ Convergence: Sub-millimeter accuracy │ │ │
|
||||
│ │ └──────────────────────────────────────────────────────────┘ │ │
|
||||
│ └───────────────────────────┬─────────────────────────────────────┘ │
|
||||
│ │ │
|
||||
│ ▼ │
|
||||
│ ScanMatchResult │
|
||||
│ (pose, score, quality) │
|
||||
└──────────────────────────────────────────────────────────────────────┘
|
||||
```
|
||||
|
||||
## Key Components
|
||||
|
||||
### CorrelativeMatcher
|
||||
|
||||
The primary scan matcher using brute-force search with multi-resolution refinement:
|
||||
|
||||
```rust
|
||||
use vastu_slam::matching::{CorrelativeMatcher, CorrelativeMatcherConfig, ScanMatcher};
|
||||
|
||||
// Create matcher with default config
|
||||
let matcher = CorrelativeMatcher::with_defaults();
|
||||
|
||||
// Match scan against map
|
||||
let result = matcher.match_scan(&lidar_scan, encoder_pose, &grid_storage);
|
||||
|
||||
if result.converged {
|
||||
let corrected_pose = result.pose;
|
||||
let confidence = result.score; // 0.0 - 1.0
|
||||
}
|
||||
```
|
||||
|
||||
**Configuration Options:**
|
||||
|
||||
| Parameter | Default | Description |
|
||||
|-----------|---------|-------------|
|
||||
| `search_x` | 0.3m | X search window (±) |
|
||||
| `search_y` | 0.3m | Y search window (±) |
|
||||
| `search_theta` | 0.15 rad | Theta search window (±8.6°) |
|
||||
| `linear_resolution` | 0.02m | Linear step size |
|
||||
| `angular_resolution` | 0.02 rad | Angular step size (1.1°) |
|
||||
| `multi_resolution` | true | Use coarse-to-fine search |
|
||||
| `use_gaussian_scoring` | true | Continuous distance-based scoring |
|
||||
| `gaussian_sigma` | 0.10m | Scoring fall-off distance |
|
||||
|
||||
### Gaussian Scoring
|
||||
|
||||
Instead of binary hit/miss, uses the distance field for smooth gradients:
|
||||
|
||||
```
|
||||
Score(point) = exp(-d² / 2σ²)
|
||||
|
||||
Distance 0: score = 1.0 (on wall)
|
||||
Distance σ: score = 0.61
|
||||
Distance 2σ: score = 0.14
|
||||
Distance 3σ: score = 0.01
|
||||
```
|
||||
|
||||
This provides:
|
||||
- Smooth score landscape for optimization
|
||||
- Better gradients for Gauss-Newton refinement
|
||||
- Robustness to small alignment errors
|
||||
|
||||
### Branch-and-Bound Matcher
|
||||
|
||||
Efficient search for large search windows using precomputed multi-resolution grids:
|
||||
|
||||
```rust
|
||||
use vastu_slam::matching::{
|
||||
BranchBoundConfig, PrecomputedGrids, branch_and_bound_match
|
||||
};
|
||||
|
||||
// Precompute multi-resolution probability grids
|
||||
let grids = PrecomputedGrids::from_storage(&grid_storage);
|
||||
|
||||
// Run branch-and-bound search
|
||||
let config = BranchBoundConfig::default();
|
||||
let result = branch_and_bound_match(&scan_points, &grids, prior_pose, &config);
|
||||
```
|
||||
|
||||
**Grid Hierarchy:**
|
||||
|
||||
```
|
||||
Level 0 (1x): [0.8][0.2][0.9][0.1][0.7][0.3][0.8][0.2] ← Full resolution
|
||||
\ / \ / \ / \ /
|
||||
Level 1 (4x): [0.8] [0.9] [0.7] [0.8] ← Max of 4
|
||||
\ / \ /
|
||||
Level 2 (16x): [0.9] [0.8] ← Max of 16
|
||||
```
|
||||
|
||||
Coarse levels provide upper bounds for pruning - if upper bound < best score, skip entire subtree.
|
||||
|
||||
### Loop Closure Detection
|
||||
|
||||
Detects when the robot revisits a previously mapped location:
|
||||
|
||||
```rust
|
||||
use vastu_slam::matching::loop_closure::{
|
||||
LoopClosureDetector, LoopClosureConfig, LoopValidator
|
||||
};
|
||||
|
||||
// Create detector with LiDAR-IRIS descriptors
|
||||
let mut detector = LoopClosureDetector::new(LoopClosureConfig::default());
|
||||
|
||||
// Add scan to database
|
||||
detector.add_scan(pose_idx, &lidar_scan, robot_pose);
|
||||
|
||||
// Check for loop closures
|
||||
if let Some(closure) = detector.detect_loop(current_pose) {
|
||||
// Validate geometrically
|
||||
let validator = LoopValidator::new(LoopValidatorConfig::default());
|
||||
if validator.validate(&closure, ¤t_scan, &reference_scan).is_valid {
|
||||
// Apply loop closure constraint
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
**LiDAR-IRIS Descriptor:**
|
||||
|
||||
Binary descriptor based on scan structure:
|
||||
1. Project scan to radial signature (range histogram by angle)
|
||||
2. Extract features using Fourier transform
|
||||
3. Compare using Hamming distance
|
||||
|
||||
Advantages:
|
||||
- Rotation-invariant
|
||||
- Compact (few KB per scan)
|
||||
- Fast comparison (bit operations)
|
||||
|
||||
### Pose Graph Optimization
|
||||
|
||||
Optimize pose graph to correct accumulated drift:
|
||||
|
||||
```rust
|
||||
use vastu_slam::matching::{BackgroundOptimizer, BackgroundOptimizerConfig};
|
||||
|
||||
let mut optimizer = BackgroundOptimizer::with_defaults();
|
||||
|
||||
// Add poses with odometry constraints
|
||||
let (pose_idx, _) = optimizer.add_pose(pose, Some(odom_delta));
|
||||
|
||||
// Add loop closure
|
||||
let trigger = optimizer.add_loop_closure(loop_closure);
|
||||
|
||||
// Run optimization (synchronous mode for embedded)
|
||||
let result = optimizer.optimize_sync();
|
||||
|
||||
// Apply corrections
|
||||
for correction in result.corrections {
|
||||
// Update submap poses, etc.
|
||||
}
|
||||
```
|
||||
|
||||
**Architecture:**
|
||||
|
||||
```
|
||||
┌─────────────────────────────────────────────────────────────────────┐
|
||||
│ SLAM Thread │
|
||||
│ Scan → Match → Insert Submap → Loop Closure Detector → trigger │
|
||||
└─────────────────────────────────────┬───────────────────────────────┘
|
||||
│
|
||||
▼
|
||||
┌─────────────────────────────────────────────────────────────────────┐
|
||||
│ Background Optimizer │
|
||||
│ Copy Graph Snapshot → Optimize (N iters) → Return Corrections │
|
||||
└─────────────────────────────────────────────────────────────────────┘
|
||||
```
|
||||
|
||||
**Modes:**
|
||||
- **Synchronous** (embedded): Call `optimize_sync()` directly
|
||||
- **Threaded** (desktop): Use background thread with channels
|
||||
|
||||
## SIMD Optimization
|
||||
|
||||
The matcher supports SIMD-accelerated operations:
|
||||
|
||||
```rust
|
||||
use vastu_slam::matching::ScratchBuffers;
|
||||
|
||||
// Create reusable scratch buffers
|
||||
let mut scratch = ScratchBuffers::new(scan.ranges.len());
|
||||
|
||||
// SIMD-optimized matching
|
||||
let result = matcher.match_scan_simd(&scan, prior_pose, &storage, &mut scratch);
|
||||
```
|
||||
|
||||
**SIMD-Accelerated Operations:**
|
||||
1. Point transformation (sensor → world frame)
|
||||
2. World → grid coordinate conversion
|
||||
3. Scoring (partially - memory access is scalar)
|
||||
|
||||
**Performance:**
|
||||
- ~2-3x speedup on ARM NEON
|
||||
- ~3-4x speedup on x86 AVX2
|
||||
|
||||
## Match Quality Classification
|
||||
|
||||
```rust
|
||||
pub enum MatchQuality {
|
||||
Excellent, // score > 0.8
|
||||
Good, // score > 0.6
|
||||
Marginal, // score > 0.4
|
||||
Poor, // score > 0.2
|
||||
Failed, // score <= 0.2
|
||||
}
|
||||
```
|
||||
|
||||
**Acceptance Policy:**
|
||||
- `Excellent`, `Good`, `Marginal`: Accept for pose correction
|
||||
- `Poor`, `Failed`: Reject (use encoder pose)
|
||||
|
||||
## Gauss-Newton Refinement
|
||||
|
||||
Sub-pixel optimization using distance field gradients:
|
||||
|
||||
```
|
||||
Objective: min Σᵢ d(Tᵢ)² + λₜ||t - t₀||² + λᵣ(θ - θ₀)²
|
||||
|
||||
Where:
|
||||
- d(Tᵢ) = distance from transformed point i to nearest wall
|
||||
- t, t₀ = current and prior translation
|
||||
- θ, θ₀ = current and prior rotation
|
||||
- λₜ, λᵣ = translation/rotation prior weights
|
||||
```
|
||||
|
||||
**Adaptive Levenberg-Marquardt:**
|
||||
- Damping increases when steps are rejected
|
||||
- Damping decreases when steps improve cost
|
||||
- Automatic step size control
|
||||
|
||||
## Configuration Presets
|
||||
|
||||
```rust
|
||||
// Default: Balanced for typical use
|
||||
let config = CorrelativeMatcherConfig::default();
|
||||
|
||||
// Fast: Reduced search for real-time with good odometry
|
||||
let config = CorrelativeMatcherConfig::fast();
|
||||
|
||||
// Thorough: Large search for poor odometry or recovery
|
||||
let config = CorrelativeMatcherConfig::thorough();
|
||||
|
||||
// Disabled: Pass-through (for odometry-only mode)
|
||||
let config = CorrelativeMatcherConfig::disabled();
|
||||
```
|
||||
|
||||
## Thread Safety
|
||||
|
||||
| Type | Thread Safety |
|
||||
|------|---------------|
|
||||
| `CorrelativeMatcher` | `Send + Sync` |
|
||||
| `ScratchBuffers` | `Send` (not `Sync`) |
|
||||
| `PrecomputedGrids` | `Send + Sync` |
|
||||
| `LoopClosureDetector` | `Send` (not `Sync` - internal state) |
|
||||
| `BackgroundOptimizer` | `Send` (owns mutex-protected graph) |
|
||||
+1
-1
@@ -45,7 +45,7 @@ use std::thread::{self, JoinHandle};
|
||||
use serde::{Deserialize, Serialize};
|
||||
|
||||
use crate::core::Pose2D;
|
||||
use crate::slam::loop_closure::{LoopClosure, PoseGraph, PoseGraphConfig};
|
||||
use crate::matching::loop_closure::{LoopClosure, PoseGraph, PoseGraphConfig};
|
||||
|
||||
/// Configuration for background optimizer.
|
||||
#[derive(Clone, Debug, Serialize, Deserialize)]
|
||||
+1
-1
@@ -11,7 +11,7 @@ use crate::core::{LidarScan, Pose2D, normalize_angle};
|
||||
use crate::grid::GridStorage;
|
||||
|
||||
use super::descriptor::LidarIris;
|
||||
use crate::slam::{
|
||||
use crate::matching::{
|
||||
BranchBoundConfig, CorrelativeMatcher, CorrelativeMatcherConfig, PrecomputedGrids,
|
||||
branch_and_bound_match_simd,
|
||||
};
|
||||
@@ -3,12 +3,15 @@
|
||||
//! This module provides:
|
||||
//! - **LiDAR-IRIS**: Compact binary descriptor for place recognition
|
||||
//! - **LoopClosureDetector**: Detects when robot revisits a location
|
||||
//! - **LoopValidator**: Geometric verification of loop closure candidates
|
||||
//! - **PoseGraph**: Stores poses and constraints, with optimization
|
||||
|
||||
mod descriptor;
|
||||
mod detector;
|
||||
mod graph;
|
||||
mod validator;
|
||||
|
||||
pub use descriptor::LidarIris;
|
||||
pub use detector::{LoopClosure, LoopClosureConfig, LoopClosureDetector};
|
||||
pub use graph::{LoopEdge, OdometryEdge, PoseGraph, PoseGraphConfig};
|
||||
pub use validator::{LoopValidator, LoopValidatorConfig, RejectionReason, ValidationResult};
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user