Enuxia-Public/enuxia-vacuum
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Move test harness from library to example-local module

Browse Source 
The harness is only used by the exploration example, not as part of
the public API. This change:

- Moves harness from src/harness/ to examples/exploration/harness/
- Removes integration-tests feature flag from Cargo.toml
- Moves sangam-io, svg, image to dev-dependencies
- Removes unused scenario module and path.rs

Example now runs with: cargo run --example exploration --release
This commit is contained in:
codetiger
2025-12-24 16:49:48 +05:30
parent 2eb6b6ce42
commit 5cf2a3cc81
14 changed files with 93 additions and 1435 deletions
Download Patch File Download Diff File Expand all files Collapse all files
vastu-map/Cargo.toml
+4 -7
View File
@@ -7,7 +7,6 @@ license = "MIT"
[features]
default = []
integration-tests = ["dep:sangam-io", "dep:svg", "dep:image"]
[dependencies]
rstar = "0.12" # R-tree spatial index
@@ -16,19 +15,17 @@ rand = "0.9" # RANSAC randomness
serde = { version = "1.0", features = ["derive"] } # Config serialization
serde_yaml = "0.9" # YAML config files
# Integration test dependencies (optional)
sangam-io = { path = "../sangam-io", features = ["mock"], optional = true }
svg = { version = "0.18", optional = true }
image = { version = "0.25", default-features = false, features = ["png", "pnm"], optional = true }
[dev-dependencies]
criterion = "0.8" # Benchmarks
env_logger = "0.11" # Test logging
approx = "0.5" # Approximate comparisons (tests only)
# Simulation dependencies (examples only)
sangam-io = { path = "../sangam-io", features = ["mock"] }
svg = "0.18"
image = { version = "0.25", default-features = false, features = ["png", "pnm"] }
[[example]]
name = "exploration"
required-features = ["integration-tests"]
[profile.release]
opt-level = 3
vastu-map/examples/exploration/harness/adapters.rs
+28
View File
@@ -0,0 +1,28 @@
//! Type conversions between sangam-io and vastu-map
//!
//! sangam-io's lidar simulator produces `Vec<(f32, f32, u8)>` which maps
//! directly to vastu-map's `PolarScan.points` format.
use vastu_map::core::{PointCloud2D, PolarScan};
/// Convert lidar scan data from sangam-io format to vastu-map PointCloud2D.
///
/// Uses full 360° lidar data for better line extraction quality.
///
/// # Arguments
/// * `lidar_data` - Raw lidar data from LidarSimulator::generate_scan()
/// * `min_quality` - Minimum quality value to include (0-255)
/// * `min_range` - Minimum valid range in meters
/// * `max_range` - Maximum valid range in meters
///
/// # Returns
/// Cartesian point cloud in robot frame, filtered by quality and range.
pub fn lidar_to_point_cloud(
lidar_data: Vec<(f32, f32, u8)>,
min_quality: u8,
min_range: f32,
max_range: f32,
) -> PointCloud2D {
let polar = PolarScan { points: lidar_data };
polar.to_cartesian(min_quality, min_range, max_range)
}
vastu-map/src/harness/harness.rs → vastu-map/examples/exploration/harness/harness.rs
+16 -308
View File
@@ -1,25 +1,7 @@
//! Core test harness for integration tests and examples
//! Core test harness for exploration example
//!
//! Provides synchronous simulation using sangam-io's mock device components
//! directly (without spawning daemon threads).
//!
//! # Configuration
//!
//! SLAM algorithm parameters can be customized via YAML configuration:
//!
//! - Set `VASTU_MAP_CONFIG` environment variable to specify a custom config file
//! - Falls back to `configs/default.yaml` in the project root
//! - Falls back to `VectorMapConfig::default()` if no YAML file is found
//!
//! ```bash
//! # Use custom config
//! VASTU_MAP_CONFIG=my_config.yaml cargo run --example exploration
//!
//! # Use default config file
//! cargo run --example exploration
//! ```
use std::path::Path;
use sangam_io::devices::mock::config::{LidarConfig, RobotConfig};
use sangam_io::devices::mock::lidar_sim::LidarSimulator;
@@ -27,17 +9,16 @@ use sangam_io::devices::mock::map_loader::SimulationMap;
use sangam_io::devices::mock::noise::NoiseGenerator;
use sangam_io::devices::mock::physics::PhysicsState;
use crate::config::ExplorationConfig;
use crate::core::Pose2D;
use crate::core::math::normalize_angle;
use crate::integration::{ScanStore, ScanStoreConfig};
use crate::odometry::WheelOdometry;
use crate::query::ClearanceVisibilityGraph;
use crate::{Map, Path as PlannedPath, VectorMap, VectorMapConfig};
use vastu_map::config::ExplorationConfig;
use vastu_map::core::Pose2D;
use vastu_map::core::math::normalize_angle;
use vastu_map::integration::{ScanStore, ScanStoreConfig};
use vastu_map::odometry::WheelOdometry;
use vastu_map::query::ClearanceVisibilityGraph;
use vastu_map::{Map, Path as PlannedPath, VectorMap, VectorMapConfig};
use super::adapters::lidar_to_point_cloud;
use super::metrics::{ConvergenceStats, TestMetrics, TimingStats};
use super::path::PathSegment;
use super::visualization::Visualizer;
/// Test harness configuration.
@@ -73,56 +54,6 @@ pub struct HarnessConfig {
pub fast_mode: bool,
}
/// Environment variable name for custom SLAM config file path.
const SLAM_CONFIG_ENV_VAR: &str = "VASTU_MAP_CONFIG";
/// Load VectorMapConfig from YAML file.
///
/// Priority:
/// 1. `VASTU_MAP_CONFIG` environment variable (if set)
/// 2. `configs/default.yaml` in project root
/// 3. `VectorMapConfig::default()` as fallback
fn load_slam_config() -> VectorMapConfig {
// Check environment variable first
if let Ok(config_path) = std::env::var(SLAM_CONFIG_ENV_VAR) {
match VectorMapConfig::from_yaml_file(&config_path) {
Ok(config) => {
log::info!("Loaded SLAM config from env var: {}", config_path);
return config;
}
Err(e) => {
log::warn!(
"Failed to load SLAM config from {}: {}, using default",
config_path,
e
);
}
}
}
// Try default config file in project root
let default_config_path = Path::new(env!("CARGO_MANIFEST_DIR")).join("configs/default.yaml");
if default_config_path.exists() {
match VectorMapConfig::from_yaml_file(&default_config_path) {
Ok(config) => {
log::info!("Loaded SLAM config from: {}", default_config_path.display());
return config;
}
Err(e) => {
log::warn!(
"Failed to load default SLAM config from {}: {}, using builtin defaults",
default_config_path.display(),
e
);
}
}
}
// Fall back to builtin defaults
log::info!("Using builtin VectorMapConfig::default()");
VectorMapConfig::default()
}
impl Default for HarnessConfig {
fn default() -> Self {
// Use lidar config with zero offsets for test accuracy
@@ -143,7 +74,7 @@ impl Default for HarnessConfig {
start_theta: 0.0,
robot: RobotConfig::default(),
lidar,
slam: load_slam_config(),
slam: VectorMapConfig::default(),
random_seed: 42, // Fixed seed for reproducibility
min_quality: 50,
min_range: 0.15,
@@ -155,68 +86,6 @@ impl Default for HarnessConfig {
}
}
impl HarnessConfig {
/// Create config for the simple_room map.
pub fn simple_room() -> Self {
let map_file = concat!(env!("CARGO_MANIFEST_DIR"), "/data/maps/simple_room.yaml");
Self {
map_file: map_file.to_string(),
start_x: 2.5,
start_y: 2.5,
..Default::default()
}
}
/// Create config for the medium_room map (8x8m with obstacles).
pub fn medium_room() -> Self {
let map_file = concat!(env!("CARGO_MANIFEST_DIR"), "/data/maps/medium_room.yaml");
Self {
map_file: map_file.to_string(),
start_x: 4.0,
start_y: 4.0,
slam: load_slam_config(),
..Default::default()
}
}
/// Create config for the large_room map.
pub fn large_room() -> Self {
let map_file = concat!(env!("CARGO_MANIFEST_DIR"), "/data/maps/large_room.yaml");
Self {
map_file: map_file.to_string(),
start_x: 5.0,
start_y: 5.0,
..Default::default()
}
}
/// Create config for the corridor map.
pub fn corridor() -> Self {
let map_file = concat!(env!("CARGO_MANIFEST_DIR"), "/data/maps/corridor.yaml");
Self {
map_file: map_file.to_string(),
start_x: 1.0,
start_y: 2.5,
..Default::default()
}
}
}
/// Result of running a test scenario.
#[derive(Debug)]
pub struct TestResult {
/// Computed metrics
pub metrics: TestMetrics,
/// Final SLAM pose estimate
pub final_pose: Pose2D,
/// Ground truth pose from physics
pub ground_truth_pose: Pose2D,
/// Number of observations processed
pub observations: usize,
/// Total simulation time in seconds
pub sim_time: f32,
}
/// Per-observation record for trajectory visualization.
#[derive(Clone, Debug)]
pub struct ObservationRecord {
@@ -272,7 +141,7 @@ pub struct TestHarness {
/// Ground truth pose from last scan (for computing odometry)
last_physics_pose: Pose2D,
/// Last lidar scan in robot frame (for visualization)
last_scan_robot_frame: Option<crate::core::PointCloud2D>,
last_scan_robot_frame: Option<vastu_map::core::PointCloud2D>,
/// ICP convergence statistics across all observations
convergence_stats: ConvergenceStats,
/// Trajectory history for enhanced visualization
@@ -369,72 +238,6 @@ impl TestHarness {
self
}
/// Run simulation along a path.
pub fn run_path(&mut self, segments: &[PathSegment]) -> TestResult {
for segment in segments {
self.run_segment(segment);
}
self.finalize()
}
/// Run a single path segment.
fn run_segment(&mut self, segment: &PathSegment) {
if self.config.fast_mode {
// Fast mode: skip intermediate physics, run only at lidar rate (~22x speedup)
// Each step covers the time of one lidar interval
let lidar_dt = self.config.dt * self.config.lidar_interval as f32;
let lidar_steps = (segment.duration / lidar_dt).max(1.0) as usize;
for _ in 0..lidar_steps {
// Simulate encoder ticks from wheel velocities
self.simulate_encoder_ticks(lidar_dt, segment.linear_vel, segment.angular_vel);
// Single physics update covering the full lidar interval
self.physics.update(
lidar_dt,
segment.linear_vel,
segment.angular_vel,
&self.map,
&self.config.robot,
);
self.sim_time += lidar_dt;
self.process_lidar_scan();
}
} else {
// Normal mode: full physics simulation at configured rate
let steps = (segment.duration / self.config.dt) as usize;
for _ in 0..steps {
// Simulate encoder ticks from wheel velocities
self.simulate_encoder_ticks(
self.config.dt,
segment.linear_vel,
segment.angular_vel,
);
// Update physics
self.physics.update(
self.config.dt,
segment.linear_vel,
segment.angular_vel,
&self.map,
&self.config.robot,
);
self.tick_count += 1;
self.sim_time += self.config.dt;
// Generate lidar scan at configured interval
if self.tick_count >= self.config.lidar_interval {
self.tick_count = 0;
self.process_lidar_scan();
}
}
}
}
/// Process a lidar scan and feed to SLAM.
fn process_lidar_scan(&mut self) {
// Generate lidar scan
@@ -570,52 +373,10 @@ impl TestHarness {
self.right_encoder_ticks += right_delta_ticks;
}
/// Finalize the test and compute metrics.
///
/// This generates the visualization SVG if enabled.
pub fn finalize(&mut self) -> TestResult {
let final_pose = self.slam.current_pose();
let ground_truth_pose =
Pose2D::new(self.physics.x(), self.physics.y(), self.physics.theta());
// Compute metrics using the simulation map, including convergence stats
let metrics = TestMetrics::compute(
&self.slam,
&self.map,
final_pose,
ground_truth_pose,
self.convergence_stats.clone(),
);
// Generate enhanced visualization if enabled
if let Some(ref viz) = self.visualizer {
viz.render_full(
&self.slam,
&self.map,
final_pose,
ground_truth_pose,
&self.scan_store,
&self.trajectory,
&metrics,
);
}
// Print timing summary
self.timing_stats.print_summary();
TestResult {
metrics,
final_pose,
ground_truth_pose,
observations: self.observations,
sim_time: self.sim_time,
}
}
/// Finalize the test with CBVG visualization.
///
/// Extended version of `finalize` that also renders the clearance-based
/// visibility graph and planned path in the SVG output.
/// Generates the SVG visualization with the clearance-based visibility
/// graph and planned path, then prints a timing summary.
///
/// # Arguments
/// * `graph` - Optional CBVG to visualize
@@ -624,7 +385,7 @@ impl TestHarness {
&mut self,
graph: Option<&ClearanceVisibilityGraph>,
path: Option<&PlannedPath>,
) -> TestResult {
) {
let final_pose = self.slam.current_pose();
let ground_truth_pose =
Pose2D::new(self.physics.x(), self.physics.y(), self.physics.theta());
@@ -655,14 +416,6 @@ impl TestHarness {
// Print timing summary
self.timing_stats.print_summary();
TestResult {
metrics,
final_pose,
ground_truth_pose,
observations: self.observations,
sim_time: self.sim_time,
}
}
/// Get current SLAM map reference.
@@ -675,16 +428,6 @@ impl TestHarness {
&mut self.slam
}
/// Get current physics state.
pub fn physics(&self) -> &PhysicsState {
&self.physics
}
/// Get current SLAM pose estimate.
pub fn slam_pose(&self) -> Pose2D {
self.last_slam_pose
}
/// Get current physics (ground truth) pose.
pub fn physics_pose(&self) -> Pose2D {
Pose2D::new(self.physics.x(), self.physics.y(), self.physics.theta())
@@ -695,11 +438,6 @@ impl TestHarness {
&self.config
}
/// Get simulation map (for collision checking).
pub fn simulation_map(&self) -> &SimulationMap {
&self.map
}
/// Run a single simulation step and return collision status.
///
/// This is used for interactive exploration tests where the controller
@@ -721,10 +459,9 @@ impl TestHarness {
// Simulate encoder ticks
self.simulate_encoder_ticks(dt, linear_vel, angular_vel);
// Update physics and get collision status
let collided =
self.physics
.update(dt, linear_vel, angular_vel, &self.map, &self.config.robot);
// Update physics
self.physics
.update(dt, linear_vel, angular_vel, &self.map, &self.config.robot);
self.sim_time += dt;
self.tick_count += 1;
@@ -733,12 +470,9 @@ impl TestHarness {
let (left_bumper, right_bumper) = self.check_bumpers();
StepResult {
collided,
left_bumper,
right_bumper,
physics_pose: Pose2D::new(self.physics.x(), self.physics.y(), self.physics.theta()),
slam_pose: self.last_slam_pose,
sim_time: self.sim_time,
}
}
@@ -786,47 +520,21 @@ impl TestHarness {
self.process_lidar_scan();
}
/// Check if it's time for a lidar scan (based on tick count).
pub fn should_scan(&self) -> bool {
self.tick_count >= self.config.lidar_interval
}
/// Get the lidar scan interval.
pub fn lidar_interval(&self) -> usize {
self.config.lidar_interval
}
/// Get current simulation time.
pub fn sim_time(&self) -> f32 {
self.sim_time
}
/// Get number of observations processed.
pub fn observation_count(&self) -> usize {
self.observations
}
/// Get scan store for visualization.
pub fn scan_store(&self) -> &ScanStore {
&self.scan_store
}
}
/// Result of a single simulation step.
#[derive(Clone, Debug)]
pub struct StepResult {
/// Whether a collision occurred during this step.
pub collided: bool,
/// Whether the left bumper is triggered.
pub left_bumper: bool,
/// Whether the right bumper is triggered.
pub right_bumper: bool,
/// Current physics (ground truth) pose.
pub physics_pose: Pose2D,
/// Current SLAM pose estimate.
pub slam_pose: Pose2D,
/// Current simulation time.
pub sim_time: f32,
}
impl StepResult {
vastu-map/src/harness/metrics.rs → vastu-map/examples/exploration/harness/metrics.rs
+10 -227
View File
@@ -7,10 +7,10 @@
use sangam_io::devices::mock::map_loader::SimulationMap;
use crate::TimingBreakdown;
use crate::VectorMap;
use crate::core::Pose2D;
use crate::features::Line2D;
use vastu_map::TimingBreakdown;
use vastu_map::VectorMap;
use vastu_map::core::Pose2D;
use vastu_map::features::Line2D;
/// Statistics collected during ICP matching over a test run.
#[derive(Debug, Clone, Default)]
@@ -137,7 +137,7 @@ impl TimingStats {
}
/// Get number of observations recorded.
pub fn observation_count(&self) -> usize {
fn observation_count(&self) -> usize {
self.total.count
}
@@ -197,13 +197,8 @@ pub struct TestMetrics {
pub position_error: f32,
/// Orientation error in radians
pub orientation_error: f32,
/// Average distance from detected lines to actual walls in meters
pub wall_distance: f32,
/// Percentage of detected line points that are near actual walls (0.0-1.0)
pub wall_accuracy: f32,
/// Percentage of detected lines that are axis-aligned (0.0-1.0)
/// Indoor walls are typically at 0°, 90°, 180°, or 270°
pub orientation_accuracy: f32,
/// Number of detected walls/features
pub detected_walls: usize,
/// ICP convergence statistics
@@ -231,100 +226,16 @@ impl TestMetrics {
let detected_walls = detected_lines.len();
// Compute wall accuracy using the simulation map
let (wall_distance, wall_accuracy) = compute_wall_accuracy(detected_lines, map);
// Compute orientation accuracy (how many lines are axis-aligned)
let orientation_accuracy = compute_orientation_accuracy(detected_lines);
let wall_accuracy = compute_wall_accuracy(detected_lines, map);
Self {
position_error,
orientation_error,
wall_distance,
wall_accuracy,
orientation_accuracy,
detected_walls,
convergence,
}
}
/// Check if metrics pass acceptance criteria.
pub fn passes(&self, criteria: &AcceptanceCriteria) -> bool {
self.position_error <= criteria.max_position_error
&& self.orientation_error <= criteria.max_orientation_error
&& self.wall_accuracy >= criteria.min_wall_accuracy
&& self.convergence.avg_iterations() <= criteria.max_avg_iterations
&& self.convergence.convergence_failures <= criteria.max_convergence_failures
}
/// Format metrics as a human-readable string.
pub fn summary(&self) -> String {
format!(
"Position error: {:.3}m, Orientation error: {:.1}°, \
Wall distance: {:.3}m, Wall accuracy: {:.0}%, \
Orientation accuracy: {:.0}%, Lines: {}, \
ICP avg: {:.1} iters, max: {}, failures: {}",
self.position_error,
self.orientation_error.to_degrees(),
self.wall_distance,
self.wall_accuracy * 100.0,
self.orientation_accuracy * 100.0,
self.detected_walls,
self.convergence.avg_iterations(),
self.convergence.max_iterations,
self.convergence.convergence_failures
)
}
}
/// Acceptance criteria for test pass/fail determination.
#[derive(Clone, Debug)]
pub struct AcceptanceCriteria {
/// Maximum position error in meters
pub max_position_error: f32,
/// Maximum orientation error in radians
pub max_orientation_error: f32,
/// Minimum wall accuracy (0.0-1.0)
pub min_wall_accuracy: f32,
/// Maximum average ICP iterations per observation
pub max_avg_iterations: f32,
/// Maximum allowed convergence failures
pub max_convergence_failures: usize,
}
impl Default for AcceptanceCriteria {
fn default() -> Self {
Self {
max_position_error: 0.1, // 10cm
max_orientation_error: 0.087, // ~5 degrees
min_wall_accuracy: 0.7, // 70% of line points near walls
max_avg_iterations: 15.0, // Avg ICP iterations < 15
max_convergence_failures: 0, // No convergence failures allowed
}
}
}
impl AcceptanceCriteria {
/// Strict criteria for high-precision tests.
pub fn strict() -> Self {
Self {
max_position_error: 0.05, // 5cm
max_orientation_error: 0.035, // ~2 degrees
min_wall_accuracy: 0.85, // 85%
max_avg_iterations: 10.0, // Avg ICP iterations < 10
max_convergence_failures: 0, // No convergence failures allowed
}
}
/// Lenient criteria for noisy/challenging tests.
pub fn lenient() -> Self {
Self {
max_position_error: 0.2, // 20cm
max_orientation_error: 0.175, // ~10 degrees
min_wall_accuracy: 0.5, // 50%
max_avg_iterations: 25.0, // Avg ICP iterations < 25
max_convergence_failures: 2, // Allow up to 2 convergence failures
}
}
}
/// Compute angle difference normalized to [-PI, PI].
@@ -345,21 +256,14 @@ const WALL_DISTANCE_THRESHOLD: f32 = 0.15; // 15cm
/// Number of sample points along each line.
const SAMPLES_PER_LINE: usize = 20;
/// Maximum angle deviation (radians) from axis-aligned to count as valid.
/// 10 degrees tolerance for axis-aligned detection.
const AXIS_ANGLE_TOLERANCE: f32 = 0.175;
/// Compute wall accuracy by checking if detected lines lie on occupied pixels.
///
/// Returns (average_distance, accuracy) where:
/// - average_distance: mean distance from line sample points to nearest wall
/// - accuracy: fraction of sample points that are within threshold of a wall
fn compute_wall_accuracy(detected: &[Line2D], map: &SimulationMap) -> (f32, f32) {
/// Returns accuracy: fraction of sample points that are within threshold of a wall.
fn compute_wall_accuracy(detected: &[Line2D], map: &SimulationMap) -> f32 {
if detected.is_empty() {
return (0.0, 0.0);
return 0.0;
}
let mut total_distance = 0.0;
let mut near_wall_count = 0;
let mut total_samples = 0;
@@ -374,7 +278,6 @@ fn compute_wall_accuracy(detected: &[Line2D], map: &SimulationMap) -> (f32, f32)
// Find distance to nearest occupied cell
let dist = distance_to_nearest_wall(x, y, map, resolution);
total_distance += dist;
total_samples += 1;
if dist <= WALL_DISTANCE_THRESHOLD {
@@ -383,10 +286,7 @@ fn compute_wall_accuracy(detected: &[Line2D], map: &SimulationMap) -> (f32, f32)
}
}
let avg_distance = total_distance / total_samples as f32;
let accuracy = near_wall_count as f32 / total_samples as f32;
(avg_distance, accuracy)
near_wall_count as f32 / total_samples as f32
}
/// Find distance from point to nearest occupied cell in map.
@@ -423,120 +323,3 @@ fn distance_to_nearest_wall(x: f32, y: f32, map: &SimulationMap, resolution: f32
// No wall found within search radius
WALL_DISTANCE_THRESHOLD * 2.0
}
/// Compute orientation accuracy by checking if detected lines are axis-aligned.
///
/// Indoor environments typically have walls at 0°, 90°, 180°, or 270° (cardinal directions).
/// This metric measures what fraction of detected lines align with these orientations.
///
/// Returns fraction of lines that are axis-aligned (0.0-1.0).
fn compute_orientation_accuracy(detected: &[Line2D]) -> f32 {
if detected.is_empty() {
return 0.0;
}
let mut axis_aligned_count = 0;
for line in detected {
let angle = line.angle();
// Check if line is axis-aligned (horizontal or vertical)
// Angles at 0, PI, -PI are horizontal; angles at PI/2, -PI/2 are vertical
if is_axis_aligned(angle) {
axis_aligned_count += 1;
}
}
axis_aligned_count as f32 / detected.len() as f32
}
/// Check if an angle is axis-aligned (horizontal or vertical within tolerance).
fn is_axis_aligned(angle: f32) -> bool {
use std::f32::consts::{FRAC_PI_2, PI};
// Normalize angle to [0, PI) since line direction is symmetric
let normalized = if angle < 0.0 { angle + PI } else { angle };
let normalized = if normalized >= PI {
normalized - PI
} else {
normalized
};
// Check if close to 0 (horizontal) or PI/2 (vertical)
let dist_to_horizontal = normalized.min(PI - normalized);
let dist_to_vertical = (normalized - FRAC_PI_2).abs();
dist_to_horizontal < AXIS_ANGLE_TOLERANCE || dist_to_vertical < AXIS_ANGLE_TOLERANCE
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn test_angle_diff() {
assert!((angle_diff(0.0, 0.0)).abs() < 0.001);
assert!((angle_diff(0.1, 0.0) - 0.1).abs() < 0.001);
assert!((angle_diff(std::f32::consts::PI, -std::f32::consts::PI)).abs() < 0.001);
}
#[test]
fn test_acceptance_criteria() {
let metrics = TestMetrics {
position_error: 0.05,
orientation_error: 0.05,
wall_distance: 0.05,
wall_accuracy: 0.9,
orientation_accuracy: 1.0,
detected_walls: 4,
convergence: ConvergenceStats {
total_iterations: 50,
observation_count: 10,
max_iterations: 8,
convergence_failures: 0,
},
};
assert!(metrics.passes(&AcceptanceCriteria::default()));
assert!(metrics.passes(&AcceptanceCriteria::lenient()));
}
#[test]
fn test_convergence_stats() {
let mut stats = ConvergenceStats::new();
assert_eq!(stats.avg_iterations(), 0.0);
stats.record(5, true);
stats.record(10, true);
stats.record(15, false); // Failed to converge
assert_eq!(stats.observation_count, 3);
assert_eq!(stats.total_iterations, 30);
assert_eq!(stats.max_iterations, 15);
assert_eq!(stats.convergence_failures, 1);
assert!((stats.avg_iterations() - 10.0).abs() < 0.01);
}
#[test]
fn test_is_axis_aligned() {
use std::f32::consts::{FRAC_PI_2, FRAC_PI_4, PI};
// Horizontal lines (0 degrees)
assert!(is_axis_aligned(0.0));
assert!(is_axis_aligned(0.1)); // Within 10 degree tolerance
assert!(is_axis_aligned(-0.1));
assert!(is_axis_aligned(PI)); // 180 degrees is also horizontal
assert!(is_axis_aligned(-PI));
// Vertical lines (90 degrees)
assert!(is_axis_aligned(FRAC_PI_2));
assert!(is_axis_aligned(-FRAC_PI_2));
assert!(is_axis_aligned(FRAC_PI_2 + 0.1)); // Within tolerance
assert!(is_axis_aligned(FRAC_PI_2 - 0.1));
// Diagonal lines should NOT be axis-aligned
assert!(!is_axis_aligned(FRAC_PI_4)); // 45 degrees
assert!(!is_axis_aligned(-FRAC_PI_4)); // -45 degrees
assert!(!is_axis_aligned(FRAC_PI_4 * 3.0)); // 135 degrees
}
}
vastu-map/examples/exploration/harness/mod.rs
+18
View File
@@ -0,0 +1,18 @@
//! Test harness for the exploration example
//!
//! This module provides a simulation environment for testing
//! vastu-map's SLAM algorithms using sangam-io's mock device.
//!
//! # Features
//!
//! - Synchronous simulation with physics, lidar, and SLAM
//! - Ground truth tracking for accuracy metrics
//! - SVG visualization output
mod adapters;
mod harness;
mod metrics;
mod visualization;
// Re-exports
pub use harness::{HarnessConfig, TestHarness};
vastu-map/src/harness/visualization.rs → vastu-map/examples/exploration/harness/visualization.rs
+5 -153
View File
@@ -16,11 +16,11 @@ use svg::node::element::{
Circle, Definitions, Group, Line, Marker, Path, Polyline, Rectangle, Text,
};
use crate::Path as PlannedPath;
use crate::VectorMap;
use crate::core::{Point2D, Pose2D};
use crate::integration::ScanStore;
use crate::query::{ClearanceVisibilityGraph, NodeType};
use vastu_map::Path as PlannedPath;
use vastu_map::VectorMap;
use vastu_map::core::{Point2D, Pose2D};
use vastu_map::integration::ScanStore;
use vastu_map::query::{ClearanceVisibilityGraph, NodeType};
use super::harness::TrajectoryHistory;
use super::metrics::TestMetrics;
@@ -96,84 +96,6 @@ impl Visualizer {
format!("#{:02X}{:02X}00", r, g)
}
/// Render the enhanced SLAM visualization with trajectory, drift, and metrics.
#[allow(clippy::too_many_arguments)]
pub fn render_full(
&self,
slam: &VectorMap,
map: &SimulationMap,
final_pose: Pose2D,
truth_pose: Pose2D,
scan_store: &ScanStore,
trajectory: &TrajectoryHistory,
metrics: &TestMetrics,
) {
// Compute bounds from map
let map_width_m = map.width() as f32 * map.resolution();
let map_height_m = map.height() as f32 * map.resolution();
let width = (map_width_m * self.scale + 2.0 * self.margin) as i32;
let height = (map_height_m * self.scale + 2.0 * self.margin) as i32;
// Create document with arrow marker definition
let mut doc = Document::new()
.set("width", width)
.set("height", height)
.set("viewBox", (0, 0, width, height));
// Add marker definitions for arrows
let defs = self.create_marker_definitions();
doc = doc.add(defs);
// Layer 0: Background
doc = doc.add(
Rectangle::new()
.set("x", 0)
.set("y", 0)
.set("width", width)
.set("height", height)
.set("fill", "white"),
);
// Layer 1: Map walls (gray)
doc = doc.add(self.render_map_walls(map, height as f32));
// Layer 2: All scans from scan store (semi-transparent)
doc = doc.add(self.render_all_scans(scan_store, height as f32));
// Layer 3: Final lidar scan (full opacity) - use last scan from store
if let Some(last_scan) = scan_store.latest() {
let world_points = last_scan.world_points();
doc = doc.add(self.render_world_points(&world_points, height as f32));
}
// Layer 4: Detected walls (teal)
doc = doc.add(self.render_detected(slam, height as f32));
// Layer 5: Trajectories with confidence heatmap
doc = doc.add(self.render_trajectories(trajectory, height as f32));
// Layer 6: Drift vectors
doc = doc.add(self.render_drift_vectors(trajectory, height as f32));
// Layer 7: Final poses
doc = doc.add(self.render_poses(final_pose, truth_pose, height as f32));
// Layer 8: Enhanced legend
doc = doc.add(self.render_enhanced_legend(height as f32));
// Layer 9: Metrics overlay
doc = doc.add(self.render_metrics_overlay(metrics, width));
// Ensure output directory exists
if let Some(parent) = std::path::Path::new(&self.output_path).parent() {
std::fs::create_dir_all(parent).ok();
}
// Save
svg::save(&self.output_path, &doc).expect("Failed to save SVG");
}
/// Create SVG marker definitions for arrows.
fn create_marker_definitions(&self) -> Definitions {
let arrow_marker = Marker::new()
@@ -891,74 +813,4 @@ impl Visualizer {
// Save
svg::save(&self.output_path, &doc).expect("Failed to save SVG");
}
/// Render standalone CBVG visualization.
///
/// Creates a simple SVG showing just the CBVG, detected walls,
/// and optionally a planned path. Useful for path planning debugging.
#[allow(dead_code)]
pub fn render_graph_only(
&self,
slam: &VectorMap,
graph: &ClearanceVisibilityGraph,
path: Option<&PlannedPath>,
bounds: (f32, f32, f32, f32), // (min_x, min_y, max_x, max_y)
) {
let (min_x, min_y, max_x, max_y) = bounds;
let map_width_m = max_x - min_x;
let map_height_m = max_y - min_y;
let width = (map_width_m * self.scale + 2.0 * self.margin) as i32;
let height = (map_height_m * self.scale + 2.0 * self.margin) as i32;
let mut doc = Document::new()
.set("width", width)
.set("height", height)
.set("viewBox", (0, 0, width, height));
// Background
doc = doc.add(
Rectangle::new()
.set("x", 0)
.set("y", 0)
.set("width", width)
.set("height", height)
.set("fill", "white"),
);
// Detected walls
doc = doc.add(self.render_detected(slam, height as f32));
// CBVG
doc = doc.add(self.render_cbvg(graph, height as f32, true));
// Planned path (if provided)
if let Some(p) = path {
doc = doc.add(self.render_planned_path(p, height as f32));
}
// Ensure output directory exists
if let Some(parent) = std::path::Path::new(&self.output_path).parent() {
std::fs::create_dir_all(parent).ok();
}
svg::save(&self.output_path, &doc).expect("Failed to save SVG");
}
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn test_transform_point() {
let viz = Visualizer::new("test.svg");
let (x, y) = viz.transform_point(Point2D::new(0.0, 0.0), 0.0, 0.0, 600.0);
// At origin (0,0) with svg_height=600, margin=50, scale=100
// x = 0 * 100 + 50 = 50
// y = 600 - (0 * 100 + 50) = 550
assert!((x - 50.0).abs() < 0.001);
assert!((y - 550.0).abs() < 0.001);
}
}
vastu-map/examples/exploration.rs → vastu-map/examples/exploration/main.rs
+12 -5
View File
@@ -10,11 +10,14 @@
//! # Running
//!
//! ```bash
//! cargo run --example exploration --release --features integration-tests
//! cargo run --example exploration --release
//! ```
//!
//! Output SVG is saved to `results/exploration_test.svg`
mod harness;
use harness::{HarnessConfig, TestHarness};
use vastu_map::Map;
use vastu_map::Path as PlannedPath;
use vastu_map::config::ExplorationConfig as MapExplorationConfig;
@@ -23,7 +26,6 @@ use vastu_map::exploration::{
CollisionEvent, CollisionType, ExplorationConfig, ExplorationController, ExplorationState,
VelocityCommand,
};
use vastu_map::harness::{HarnessConfig, TestHarness};
use vastu_map::integration::CoplanarMergeConfig;
use vastu_map::query::cbvg::ClearanceVisibilityGraph;
@@ -61,8 +63,13 @@ fn main() {
// Ensure results directory exists
std::fs::create_dir_all("results").ok();
// Setup harness with medium_room map and visualization
let harness_config = HarnessConfig::medium_room();
// Setup harness with map file and starting position
let harness_config = HarnessConfig {
map_file: concat!(env!("CARGO_MANIFEST_DIR"), "/data/maps/medium_room.yaml").to_string(),
start_x: 4.0,
start_y: 4.0,
..Default::default()
};
// Enable VectorMap exploration mode for point-cloud-based line re-fitting
// This will re-fit lines every 10 observations using accumulated scan data
@@ -294,7 +301,7 @@ fn run_exploration_loop(harness: &mut TestHarness, config: ExplorationConfig) ->
);
// Finalize to generate visualization with CBVG
let _test_result = harness.finalize_with_graph(last_cbvg.as_ref(), last_planned_path.as_ref());
harness.finalize_with_graph(last_cbvg.as_ref(), last_planned_path.as_ref());
ExplorationResult {
completed: harness.slam().frontiers().is_empty(),
vastu-map/src/harness/adapters.rs
-78
View File
@@ -1,78 +0,0 @@
//! Type conversions between sangam-io and vastu-map
//!
//! sangam-io's lidar simulator produces `Vec<(f32, f32, u8)>` which maps
//! directly to vastu-map's `PolarScan.points` format.
use crate::core::{PointCloud2D, PolarScan};
/// Convert lidar scan data from sangam-io format to vastu-map PointCloud2D.
///
/// Uses full 360° lidar data for better line extraction quality.
///
/// # Arguments
/// * `lidar_data` - Raw lidar data from LidarSimulator::generate_scan()
/// * `min_quality` - Minimum quality value to include (0-255)
/// * `min_range` - Minimum valid range in meters
/// * `max_range` - Maximum valid range in meters
///
/// # Returns
/// Cartesian point cloud in robot frame, filtered by quality and range.
pub fn lidar_to_point_cloud(
lidar_data: Vec<(f32, f32, u8)>,
min_quality: u8,
min_range: f32,
max_range: f32,
) -> PointCloud2D {
let polar = PolarScan { points: lidar_data };
polar.to_cartesian(min_quality, min_range, max_range)
}
#[cfg(test)]
mod tests {
use super::*;
use std::f32::consts::FRAC_PI_2;
#[test]
fn test_lidar_to_point_cloud() {
let lidar_data = vec![
(0.0, 1.0, 100), // Forward, 1m
(FRAC_PI_2, 2.0, 100), // Left, 2m
(std::f32::consts::PI, 1.5, 50), // Behind, low quality
];
let cloud = lidar_to_point_cloud(lidar_data, 60, 0.1, 10.0);
// Third point filtered by quality
assert_eq!(cloud.len(), 2);
// First point should be ~(1, 0)
assert!((cloud.xs[0] - 1.0).abs() < 0.001);
assert!(cloud.ys[0].abs() < 0.001);
// Second point should be ~(0, 2)
assert!(cloud.xs[1].abs() < 0.001);
assert!((cloud.ys[1] - 2.0).abs() < 0.001);
}
#[test]
fn test_lidar_to_point_cloud_360() {
// Test that full 360 degree scan is preserved
let lidar_data: Vec<_> = (0..360)
.map(|i| {
let angle = (i as f32).to_radians();
(angle, 1.0, 100u8)
})
.collect();
let cloud = lidar_to_point_cloud(lidar_data, 50, 0.1, 10.0);
// Should have all 360 points (no subsampling)
assert_eq!(cloud.len(), 360, "Should have all points");
// Points should form a circle around origin in robot frame
for i in 0..cloud.len() {
let dist = (cloud.xs[i].powi(2) + cloud.ys[i].powi(2)).sqrt();
assert!((dist - 1.0).abs() < 0.01, "Points should be at radius 1.0");
}
}
}
vastu-map/src/harness/mod.rs
-54
View File
@@ -1,54 +0,0 @@
//! Test harness for integration testing and examples
//!
//! This module provides a complete simulation environment for testing
//! vastu-map's SLAM algorithms using sangam-io's mock device.
//!
//! # Features
//!
//! - Synchronous simulation with physics, lidar, and SLAM
//! - Ground truth tracking for accuracy metrics
//! - SVG visualization output
//! - YAML scenario support
//!
//! # Usage
//!
//! ```rust,ignore
//! use vastu_map::harness::{HarnessConfig, TestHarness};
//!
//! let config = HarnessConfig::medium_room();
//! let mut harness = TestHarness::new(config)?
//! .with_visualization("output.svg");
//!
//! // Run exploration loop
//! loop {
//! let step = harness.step(0.2, 0.0); // forward at 0.2 m/s
//! harness.process_lidar();
//!
//! if step.any_bumper() {
//! break;
//! }
//! }
//!
//! let result = harness.finalize();
//! println!("Position error: {:.3}m", result.metrics.position_error);
//! ```
pub mod adapters;
pub mod harness;
pub mod metrics;
pub mod path;
pub mod scenario;
pub mod visualization;
// Re-exports for convenience
pub use adapters::lidar_to_point_cloud;
pub use harness::{
HarnessConfig, ObservationRecord, StepResult, TestHarness, TestResult, TrajectoryHistory,
};
pub use metrics::{AcceptanceCriteria, ConvergenceStats, TestMetrics, TimingStats};
pub use path::PathSegment;
pub use scenario::{
PathCommand, PoseConfig, ScenarioConfig, WheelCommand, convert_path_commands, load_scenario,
run_all_scenarios, run_scenario,
};
pub use visualization::Visualizer;
vastu-map/src/harness/path.rs
-93
View File
@@ -1,93 +0,0 @@
//! Robot path segment for simulation and testing
//!
//! Provides PathSegment for defining robot motion commands.
/// A path segment with velocity commands and duration.
#[derive(Clone, Debug)]
pub struct PathSegment {
/// Linear velocity in m/s (positive = forward)
pub linear_vel: f32,
/// Angular velocity in rad/s (positive = CCW)
pub angular_vel: f32,
/// Duration in seconds
pub duration: f32,
}
impl PathSegment {
/// Create a forward motion segment.
pub fn forward(speed: f32, duration: f32) -> Self {
Self {
linear_vel: speed,
angular_vel: 0.0,
duration,
}
}
/// Create a left turn segment (CCW).
pub fn turn_left(angular_speed: f32, angle: f32) -> Self {
Self {
linear_vel: 0.0,
angular_vel: angular_speed,
duration: angle / angular_speed,
}
}
/// Create a right turn segment (CW).
pub fn turn_right(angular_speed: f32, angle: f32) -> Self {
Self {
linear_vel: 0.0,
angular_vel: -angular_speed,
duration: angle / angular_speed,
}
}
/// Create an arc motion segment.
pub fn arc(linear: f32, angular: f32, duration: f32) -> Self {
Self {
linear_vel: linear,
angular_vel: angular,
duration,
}
}
/// Create a stop segment (no motion).
pub fn stop(duration: f32) -> Self {
Self {
linear_vel: 0.0,
angular_vel: 0.0,
duration,
}
}
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn test_forward_segment() {
let seg = PathSegment::forward(0.5, 2.0);
assert_eq!(seg.linear_vel, 0.5);
assert_eq!(seg.angular_vel, 0.0);
assert_eq!(seg.duration, 2.0);
}
#[test]
fn test_turn_segments() {
let left = PathSegment::turn_left(1.0, PI / 2.0);
assert!(left.angular_vel > 0.0);
assert_eq!(left.linear_vel, 0.0);
let right = PathSegment::turn_right(1.0, PI / 2.0);
assert!(right.angular_vel < 0.0);
assert_eq!(right.linear_vel, 0.0);
}
#[test]
fn test_stop_segment() {
let seg = PathSegment::stop(5.0);
assert_eq!(seg.linear_vel, 0.0);
assert_eq!(seg.angular_vel, 0.0);
assert_eq!(seg.duration, 5.0);
}
}
vastu-map/src/harness/scenario/config.rs
-188
View File
@@ -1,188 +0,0 @@
//! YAML configuration structures for scenario-based tests
//!
//! Allows defining test scenarios in YAML files with wheel distance commands
//! for precise, encoder-based robot movement.
use serde::Deserialize;
/// Top-level scenario configuration loaded from YAML.
#[derive(Debug, Deserialize)]
pub struct ScenarioConfig {
/// Scenario name (used for output files)
pub name: String,
/// Path to map YAML file (relative to scenario file)
pub map: String,
/// Starting pose
#[serde(default)]
pub start_pose: PoseConfig,
/// Path as commands (wheel movements or stops)
pub path: Vec<PathCommand>,
}
/// Robot pose configuration.
#[derive(Debug, Deserialize)]
pub struct PoseConfig {
/// X position in meters
#[serde(default = "default_2_5")]
pub x: f32,
/// Y position in meters
#[serde(default = "default_2_5")]
pub y: f32,
/// Orientation in radians
#[serde(default)]
pub theta: f32,
}
impl Default for PoseConfig {
fn default() -> Self {
Self {
x: 2.5,
y: 2.5,
theta: 0.0,
}
}
}
/// Wheel distance command - precise encoder-based movement.
///
/// For differential drive robot (wheel_base = 0.233m):
/// - Forward/Back: left = right = distance
/// - Rotate in place: left = -right = θ * wheel_base / 2 (θ in radians)
/// - 90° = π/2 * 0.233 / 2 = 0.183m per wheel
/// - 180° = π * 0.233 / 2 = 0.366m per wheel
/// - 360° = 2π * 0.233 / 2 = 0.732m per wheel
/// - Arc: Different distances create curves
#[derive(Debug, Deserialize)]
pub struct WheelCommand {
/// Left wheel distance in meters (positive = forward)
pub left: f32,
/// Right wheel distance in meters (positive = forward)
pub right: f32,
/// Optional speed in m/s (defaults to 0.2 m/s if not specified)
#[serde(default)]
pub speed: Option<f32>,
}
/// Path command - either a wheel command or a stop command.
#[derive(Debug, Deserialize)]
#[serde(untagged)]
pub enum PathCommand {
/// Stop for a specified duration in seconds
Stop {
/// Duration to stay stationary in seconds
stop: f32,
},
/// Wheel distance command
Wheel(WheelCommand),
}
fn default_2_5() -> f32 {
2.5
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn test_deserialize_scenario() {
let yaml = r#"
name: "test_scenario"
map: "../maps/simple_room.yaml"
start_pose:
x: 2.5
y: 2.5
theta: 0.0
path:
- {left: 0.5, right: 0.5}
- {left: -0.183, right: 0.183}
"#;
let config: ScenarioConfig = serde_yaml::from_str(yaml).expect("Failed to parse YAML");
assert_eq!(config.name, "test_scenario");
assert_eq!(config.path.len(), 2);
match &config.path[0] {
PathCommand::Wheel(cmd) => {
assert_eq!(cmd.left, 0.5);
assert_eq!(cmd.right, 0.5);
assert!(cmd.speed.is_none());
}
_ => panic!("Expected Wheel command"),
}
}
#[test]
fn test_default_pose() {
let yaml = r#"
name: "minimal"
map: "test.yaml"
path:
- {left: 1.0, right: 1.0}
"#;
let config: ScenarioConfig = serde_yaml::from_str(yaml).expect("Failed to parse YAML");
assert_eq!(config.start_pose.x, 2.5);
assert_eq!(config.start_pose.y, 2.5);
assert_eq!(config.start_pose.theta, 0.0);
}
#[test]
fn test_wheel_command_with_speed() {
let yaml = r#"
name: "speed_test"
map: "test.yaml"
path:
- {left: 1.0, right: 1.0, speed: 0.5}
"#;
let config: ScenarioConfig = serde_yaml::from_str(yaml).expect("Failed to parse YAML");
match &config.path[0] {
PathCommand::Wheel(cmd) => {
assert_eq!(cmd.left, 1.0);
assert_eq!(cmd.right, 1.0);
assert_eq!(cmd.speed, Some(0.5));
}
_ => panic!("Expected Wheel command"),
}
}
#[test]
fn test_stop_command() {
let yaml = r#"
name: "stop_test"
map: "test.yaml"
path:
- {stop: 5.0}
"#;
let config: ScenarioConfig = serde_yaml::from_str(yaml).expect("Failed to parse YAML");
match &config.path[0] {
PathCommand::Stop { stop } => {
assert_eq!(*stop, 5.0);
}
_ => panic!("Expected Stop command"),
}
}
#[test]
fn test_mixed_commands() {
let yaml = r#"
name: "mixed_test"
map: "test.yaml"
path:
- {left: 1.0, right: 1.0}
- {stop: 2.0}
- {left: 0.5, right: 0.5, speed: 0.3}
"#;
let config: ScenarioConfig = serde_yaml::from_str(yaml).expect("Failed to parse YAML");
assert_eq!(config.path.len(), 3);
// First: wheel without speed
assert!(matches!(&config.path[0], PathCommand::Wheel(cmd) if cmd.speed.is_none()));
// Second: stop
assert!(matches!(&config.path[1], PathCommand::Stop { stop } if *stop == 2.0));
// Third: wheel with speed
assert!(matches!(&config.path[2], PathCommand::Wheel(cmd) if cmd.speed == Some(0.3)));
}
}
vastu-map/src/harness/scenario/mod.rs
-10
View File
@@ -1,10 +0,0 @@
//! YAML scenario configuration and runner
//!
//! Provides structures and functions for defining and running
//! test scenarios from YAML configuration files.
pub mod config;
pub mod runner;
pub use config::{PathCommand, PoseConfig, ScenarioConfig, WheelCommand};
pub use runner::{convert_path_commands, load_scenario, run_all_scenarios, run_scenario};
vastu-map/src/harness/scenario/runner.rs
-308
View File
@@ -1,308 +0,0 @@
//! YAML scenario runner
//!
//! Loads and executes test scenarios defined in YAML files.
use std::path::Path;
use super::config::{PathCommand, ScenarioConfig};
use crate::harness::harness::{HarnessConfig, TestHarness, TestResult};
use crate::harness::path::PathSegment;
/// Wheel base of the robot in meters (from RobotConfig default).
const WHEEL_BASE: f32 = 0.233;
/// Default wheel speed for simulation in m/s.
const DEFAULT_SPEED: f32 = 0.2;
/// Load a scenario configuration from a YAML file.
pub fn load_scenario<P: AsRef<Path>>(
path: P,
) -> Result<ScenarioConfig, Box<dyn std::error::Error>> {
let content = std::fs::read_to_string(path)?;
Ok(serde_yaml::from_str(&content)?)
}
/// Run a scenario from a YAML file.
///
/// Returns the test result or an error if the scenario failed to load/run.
pub fn run_scenario<P: AsRef<Path>>(
yaml_path: P,
) -> Result<TestResult, Box<dyn std::error::Error>> {
let yaml_path = yaml_path.as_ref();
let config = load_scenario(yaml_path)?;
// Resolve map path relative to YAML file
let yaml_dir = yaml_path.parent().unwrap_or(Path::new("."));
let map_file = yaml_dir
.join(&config.map)
.canonicalize()
.map(|p| p.to_string_lossy().to_string())
.unwrap_or_else(|_| yaml_dir.join(&config.map).to_string_lossy().to_string());
let harness_config = HarnessConfig {
map_file,
start_x: config.start_pose.x,
start_y: config.start_pose.y,
start_theta: config.start_pose.theta,
..Default::default()
};
let path = convert_path_commands(&config.path);
// Ensure output directory exists
std::fs::create_dir_all("results").ok();
let svg_path = format!("results/{}.svg", config.name);
let mut harness = TestHarness::new(harness_config)?.with_visualization(&svg_path);
Ok(harness.run_path(&path))
}
/// Convert path commands to PathSegments.
///
/// Handles both wheel commands and stop commands:
/// - Wheel commands use differential drive kinematics
/// - Stop commands create stationary segments
pub fn convert_path_commands(commands: &[PathCommand]) -> Vec<PathSegment> {
commands
.iter()
.filter_map(|cmd| match cmd {
PathCommand::Stop { stop } => {
if *stop < 0.001 {
return None; // Skip zero-duration stops
}
Some(PathSegment::stop(*stop))
}
PathCommand::Wheel(wheel) => {
// Calculate linear and angular distances
let linear_dist = (wheel.left + wheel.right) / 2.0;
let angular_dist = (wheel.right - wheel.left) / WHEEL_BASE; // radians
// Calculate duration based on max wheel distance and speed
let max_wheel_dist = wheel.left.abs().max(wheel.right.abs());
if max_wheel_dist < 0.001 {
return None; // Skip zero-distance commands
}
let speed = wheel.speed.unwrap_or(DEFAULT_SPEED);
let duration = max_wheel_dist / speed;
// Calculate velocities
let linear_vel = linear_dist / duration;
let angular_vel = angular_dist / duration;
Some(PathSegment {
linear_vel,
angular_vel,
duration,
})
}
})
.collect()
}
/// Run all YAML scenarios in a directory.
///
/// Returns a vector of (scenario_name, result) tuples.
pub fn run_all_scenarios<P: AsRef<Path>>(dir: P) -> Vec<(String, Result<TestResult, String>)> {
let mut results = Vec::new();
let entries = match std::fs::read_dir(&dir) {
Ok(entries) => entries,
Err(e) => {
log::warn!("Failed to read scenario directory: {}", e);
return results;
}
};
// Collect and sort entries for consistent ordering
let mut yaml_files: Vec<_> = entries
.filter_map(|e| e.ok())
.map(|e| e.path())
.filter(|p| p.extension().map_or(false, |e| e == "yaml" || e == "yml"))
.filter(|p| !p.to_string_lossy().contains("/maps/")) // Skip map yaml files
.collect();
yaml_files.sort();
for path in yaml_files {
let name = path
.file_stem()
.unwrap_or_default()
.to_string_lossy()
.to_string();
println!("\n--- Running scenario: {} ---", name);
let result = run_scenario(&path).map_err(|e| e.to_string());
match &result {
Ok(r) => println!(" {}", r.metrics.summary()),
Err(e) => println!(" FAILED: {}", e),
}
results.push((name, result));
}
results
}
#[cfg(test)]
mod tests {
use super::*;
use crate::harness::scenario::config::WheelCommand;
#[test]
fn test_wheel_command_forward() {
let commands = vec![PathCommand::Wheel(WheelCommand {
left: 1.0,
right: 1.0,
speed: None,
})];
let segments = convert_path_commands(&commands);
assert_eq!(segments.len(), 1);
let seg = &segments[0];
// Forward: linear = 1.0, angular = 0
assert!((seg.linear_vel - DEFAULT_SPEED).abs() < 0.01);
assert!(seg.angular_vel.abs() < 0.01);
assert!((seg.duration - 5.0).abs() < 0.01); // 1.0m / 0.2m/s = 5s
}
#[test]
fn test_wheel_command_rotate() {
// 90 degree turn: left = -0.183, right = 0.183
let turn_dist = std::f32::consts::FRAC_PI_2 * WHEEL_BASE / 2.0;
let commands = vec![PathCommand::Wheel(WheelCommand {
left: -turn_dist,
right: turn_dist,
speed: None,
})];
let segments = convert_path_commands(&commands);
assert_eq!(segments.len(), 1);
let seg = &segments[0];
// Rotation in place: linear = 0
assert!(seg.linear_vel.abs() < 0.01);
// Angular should be positive (CCW)
assert!(seg.angular_vel > 0.0);
}
#[test]
fn test_wheel_command_arc() {
// Arc motion: different wheel distances
let commands = vec![PathCommand::Wheel(WheelCommand {
left: 0.5,
right: 0.7,
speed: None,
})];
let segments = convert_path_commands(&commands);
assert_eq!(segments.len(), 1);
let seg = &segments[0];
// Both linear and angular should be non-zero
assert!(seg.linear_vel > 0.0);
assert!(seg.angular_vel > 0.0); // Turning left (outer wheel faster)
}
#[test]
fn test_skip_zero_commands() {
let commands = vec![
PathCommand::Wheel(WheelCommand {
left: 0.0,
right: 0.0,
speed: None,
}),
PathCommand::Wheel(WheelCommand {
left: 1.0,
right: 1.0,
speed: None,
}),
];
let segments = convert_path_commands(&commands);
// Zero command should be skipped
assert_eq!(segments.len(), 1);
}
#[test]
fn test_wheel_command_with_speed() {
// 1m forward at 0.5 m/s should take 2 seconds
let commands = vec![PathCommand::Wheel(WheelCommand {
left: 1.0,
right: 1.0,
speed: Some(0.5),
})];
let segments = convert_path_commands(&commands);
assert_eq!(segments.len(), 1);
let seg = &segments[0];
assert!((seg.linear_vel - 0.5).abs() < 0.01);
assert!(seg.angular_vel.abs() < 0.01);
assert!((seg.duration - 2.0).abs() < 0.01); // 1.0m / 0.5m/s = 2s
}
#[test]
fn test_stop_command() {
let commands = vec![PathCommand::Stop { stop: 5.0 }];
let segments = convert_path_commands(&commands);
assert_eq!(segments.len(), 1);
let seg = &segments[0];
assert!(seg.linear_vel.abs() < 0.001);
assert!(seg.angular_vel.abs() < 0.001);
assert!((seg.duration - 5.0).abs() < 0.01);
}
#[test]
fn test_skip_zero_stop() {
let commands = vec![
PathCommand::Stop { stop: 0.0 },
PathCommand::Wheel(WheelCommand {
left: 1.0,
right: 1.0,
speed: None,
}),
];
let segments = convert_path_commands(&commands);
// Zero-duration stop should be skipped
assert_eq!(segments.len(), 1);
}
#[test]
fn test_mixed_commands() {
let commands = vec![
PathCommand::Wheel(WheelCommand {
left: 1.0,
right: 1.0,
speed: None,
}),
PathCommand::Stop { stop: 2.0 },
PathCommand::Wheel(WheelCommand {
left: 0.5,
right: 0.5,
speed: Some(0.25),
}),
];
let segments = convert_path_commands(&commands);
assert_eq!(segments.len(), 3);
// First: forward at default speed
assert!((segments[0].linear_vel - DEFAULT_SPEED).abs() < 0.01);
assert!((segments[0].duration - 5.0).abs() < 0.01);
// Second: stop for 2 seconds
assert!(segments[1].linear_vel.abs() < 0.001);
assert!((segments[1].duration - 2.0).abs() < 0.01);
// Third: forward at 0.25 m/s for 2 seconds
assert!((segments[2].linear_vel - 0.25).abs() < 0.01);
assert!((segments[2].duration - 2.0).abs() < 0.01); // 0.5m / 0.25m/s = 2s
}
}
vastu-map/src/lib.rs
-4
View File
@@ -123,10 +123,6 @@ pub mod odometry;
pub mod query;
pub mod vector_map;
// Test harness module (feature-gated)
#[cfg(feature = "integration-tests")]
pub mod harness;
// Re-export main types at crate root
pub use motion_model::MotionModel;
pub use vector_map::{ConfigError, VectorMap, VectorMapConfig, YamlConfigError};