Refactor test harness into library module for reusability
Move integration test harness from tests/integration/ to src/harness/ as a feature-gated library module, enabling use by examples and tools. - Add pub mod harness (behind integration-tests feature) - Reorganize data files into data/maps/ and data/scenarios/ - Convert integration test to exploration example - Remove test-only boilerplate (mod.rs, yaml_tests.rs)
This commit is contained in:
codetiger
@@ -58,5 +58,3 @@ analysis/
|
||||
*.log
|
||||
|
||||
results/
|
||||
dataset/
|
||||
maps/
|
||||
@@ -26,9 +26,8 @@ criterion = "0.8" # Benchmarks
|
||||
env_logger = "0.11" # Test logging
|
||||
approx = "0.5" # Approximate comparisons (tests only)
|
||||
|
||||
[[test]]
|
||||
name = "integration"
|
||||
path = "tests/integration/mod.rs"
|
||||
[[example]]
|
||||
name = "exploration"
|
||||
required-features = ["integration-tests"]
|
||||
|
||||
[profile.release]
|
||||
|
||||
+1
-1
@@ -13,7 +13,7 @@
|
||||
# Path: Series of arcs creating an S-curve pattern
|
||||
|
||||
name: "drift_continuous_curve"
|
||||
map: "maps/medium_room.yaml"
|
||||
map: "../maps/medium_room.yaml"
|
||||
|
||||
start_pose:
|
||||
x: 4.0
|
||||
+1
-1
@@ -13,7 +13,7 @@
|
||||
# Path: Zigzag covering ~20m total distance
|
||||
|
||||
name: "drift_long_path"
|
||||
map: "maps/large_room.yaml"
|
||||
map: "../maps/large_room.yaml"
|
||||
|
||||
start_pose:
|
||||
x: 2.0
|
||||
+1
-1
@@ -13,7 +13,7 @@
|
||||
# Path: Move 3m down the corridor
|
||||
|
||||
name: "icp_sparse_corridor"
|
||||
map: "maps/corridor.yaml"
|
||||
map: "../maps/corridor.yaml"
|
||||
|
||||
start_pose:
|
||||
x: 0.5 # Center of 1m-wide corridor
|
||||
+1
-1
@@ -14,7 +14,7 @@
|
||||
# Expected: Final pose within 15cm/5 degrees of starting pose
|
||||
|
||||
name: "loop_closure_square"
|
||||
map: "maps/medium_room.yaml"
|
||||
map: "../maps/medium_room.yaml"
|
||||
|
||||
start_pose:
|
||||
x: 3.5
|
||||
+1
-1
@@ -7,7 +7,7 @@
|
||||
# Wheel base: 0.233m
|
||||
|
||||
name: "medium_room_exploration"
|
||||
map: "maps/medium_room.yaml"
|
||||
map: "../maps/medium_room.yaml"
|
||||
|
||||
start_pose:
|
||||
x: 4.0
|
||||
+1
-1
@@ -10,7 +10,7 @@
|
||||
# - Small box at (2m, 6m)
|
||||
|
||||
name: "medium_room_obstacles"
|
||||
map: "maps/medium_room.yaml"
|
||||
map: "../maps/medium_room.yaml"
|
||||
|
||||
start_pose:
|
||||
x: 4.0
|
||||
+1
-1
@@ -7,7 +7,7 @@
|
||||
# Path: Rectangle around the room (keeping 1m from walls)
|
||||
|
||||
name: "medium_room_perimeter"
|
||||
map: "maps/medium_room.yaml"
|
||||
map: "../maps/medium_room.yaml"
|
||||
|
||||
start_pose:
|
||||
x: 1.0
|
||||
+1
-1
@@ -9,7 +9,7 @@
|
||||
# Expected: Final orientation within 5 degrees of starting orientation (0 rad)
|
||||
|
||||
name: "pure_rotation_360"
|
||||
map: "maps/medium_room.yaml"
|
||||
map: "../maps/medium_room.yaml"
|
||||
|
||||
start_pose:
|
||||
x: 4.0
|
||||
+1
-1
@@ -8,7 +8,7 @@
|
||||
# Move 0.5m forward in a straight line
|
||||
|
||||
name: "simple_room_straight_line"
|
||||
map: "maps/simple_room.yaml"
|
||||
map: "../maps/simple_room.yaml"
|
||||
|
||||
start_pose:
|
||||
x: 1.5
|
||||
+1
-1
@@ -12,7 +12,7 @@
|
||||
# - Map building accuracy without motion
|
||||
|
||||
name: "static_scan_5s"
|
||||
map: "maps/medium_room.yaml"
|
||||
map: "../maps/medium_room.yaml"
|
||||
|
||||
start_pose:
|
||||
x: 4.0
|
||||
+1
-1
@@ -7,7 +7,7 @@
|
||||
# Move 1m forward in a straight line
|
||||
|
||||
name: "straight_line_test"
|
||||
map: "maps/medium_room.yaml"
|
||||
map: "../maps/medium_room.yaml"
|
||||
|
||||
start_pose:
|
||||
x: 2.0
|
||||
@@ -0,0 +1,323 @@
|
||||
//! Autonomous exploration example
|
||||
//!
|
||||
//! Demonstrates the complete exploration pipeline using vastu_map's public APIs:
|
||||
//! 1. Frontier detection
|
||||
//! 2. Path planning to frontiers
|
||||
//! 3. Path following with collision detection
|
||||
//! 4. Virtual wall insertion on unexpected collisions
|
||||
//! 5. Exploration completion when no frontiers remain
|
||||
//!
|
||||
//! # Running
|
||||
//!
|
||||
//! ```bash
|
||||
//! cargo run --example exploration --release --features integration-tests
|
||||
//! ```
|
||||
//!
|
||||
//! Output SVG is saved to `results/exploration_test.svg`
|
||||
|
||||
use vastu_map::Map;
|
||||
use vastu_map::Path as PlannedPath;
|
||||
use vastu_map::config::ExplorationConfig as MapExplorationConfig;
|
||||
use vastu_map::core::{Point2D, Pose2D};
|
||||
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;
|
||||
|
||||
/// Maximum simulation time in seconds.
|
||||
const MAX_TIME: f32 = 200.0; // 3.3 minutes
|
||||
|
||||
/// Result of exploration.
|
||||
#[derive(Debug)]
|
||||
pub struct ExplorationResult {
|
||||
/// Whether exploration completed (no frontiers remain).
|
||||
pub completed: bool,
|
||||
/// Total simulation time.
|
||||
pub sim_time: f32,
|
||||
/// Number of exploration cycles.
|
||||
pub cycles: usize,
|
||||
/// Number of collisions handled.
|
||||
pub collisions: usize,
|
||||
/// Number of virtual walls added.
|
||||
pub virtual_walls: usize,
|
||||
/// Map line count before optimization.
|
||||
pub map_lines_before: usize,
|
||||
/// Final map line count (after optimization).
|
||||
pub map_lines: usize,
|
||||
/// Number of lines merged during optimization.
|
||||
pub lines_merged: usize,
|
||||
/// Initial frontier count.
|
||||
pub initial_frontiers: usize,
|
||||
/// Final frontier count (should be 0 on success).
|
||||
pub remaining_frontiers: usize,
|
||||
}
|
||||
|
||||
fn main() {
|
||||
env_logger::init();
|
||||
|
||||
// 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();
|
||||
|
||||
// Enable VectorMap exploration mode for point-cloud-based line re-fitting
|
||||
// This will re-fit lines every 10 observations using accumulated scan data
|
||||
let map_exploration_config = MapExplorationConfig::default()
|
||||
.with_refit_interval(10)
|
||||
.with_gap_threshold(0.3);
|
||||
|
||||
let mut harness = TestHarness::new(harness_config)
|
||||
.expect("Failed to create harness")
|
||||
.with_exploration_mode(Some(map_exploration_config))
|
||||
.with_visualization("results/exploration_test.svg");
|
||||
|
||||
// Create robot exploration config (for path following)
|
||||
let exploration_config = ExplorationConfig::default()
|
||||
.with_max_linear_speed(0.3)
|
||||
.with_max_angular_speed(1.0)
|
||||
.with_backoff_distance(0.15)
|
||||
.with_robot_radius(0.15);
|
||||
|
||||
// Run exploration
|
||||
let result = run_exploration_loop(&mut harness, exploration_config);
|
||||
|
||||
// Print results
|
||||
println!("\n=== Exploration Results ===");
|
||||
println!("Completed: {}", result.completed);
|
||||
println!("Simulation time: {:.1}s", result.sim_time);
|
||||
println!("Cycles: {}", result.cycles);
|
||||
println!("Collisions handled: {}", result.collisions);
|
||||
println!("Virtual walls added: {}", result.virtual_walls);
|
||||
println!(
|
||||
"Map lines: {} before optimization -> {} after ({} merged)",
|
||||
result.map_lines_before, result.map_lines, result.lines_merged
|
||||
);
|
||||
println!(
|
||||
"Frontiers: {} initial -> {} remaining",
|
||||
result.initial_frontiers, result.remaining_frontiers
|
||||
);
|
||||
println!("\nVisualization saved to: results/exploration_test.svg");
|
||||
}
|
||||
|
||||
/// Run the exploration loop using the ExplorationController with region-based TSP.
|
||||
///
|
||||
/// This uses a continuous control loop where the controller is called every step,
|
||||
/// properly integrating with its state machine for path following and collision handling.
|
||||
fn run_exploration_loop(harness: &mut TestHarness, config: ExplorationConfig) -> ExplorationResult {
|
||||
let mut cycles = 0;
|
||||
let mut collisions = 0;
|
||||
let mut virtual_walls = 0;
|
||||
|
||||
// Variables to capture the last CBVG and path for visualization
|
||||
let mut last_cbvg: Option<ClearanceVisibilityGraph> = None;
|
||||
let mut last_planned_path: Option<PlannedPath> = None;
|
||||
|
||||
println!("Starting exploration with region-based TSP planning...");
|
||||
|
||||
// Create the exploration controller with region-based algorithm
|
||||
let mut controller = ExplorationController::new(config.clone());
|
||||
|
||||
// Phase 1: Initial map update (longer for initial observation)
|
||||
wait_for_map_update(harness, 2.0);
|
||||
|
||||
let initial_frontiers = harness.slam().frontiers().len();
|
||||
println!("Initial frontiers: {}", initial_frontiers);
|
||||
|
||||
// Start exploration
|
||||
controller.start();
|
||||
|
||||
let mut step_count = 0;
|
||||
let mut last_target: Option<Point2D> = None;
|
||||
let mut last_cycle_print = 0;
|
||||
let mut pending_collision: Option<CollisionEvent> = None;
|
||||
const MAX_STEPS: usize = 20000;
|
||||
const LIDAR_INTERVAL: usize = 5; // Process lidar every N steps
|
||||
|
||||
while step_count < MAX_STEPS && harness.sim_time() < MAX_TIME {
|
||||
step_count += 1;
|
||||
|
||||
// Get current pose
|
||||
let physics_pose = harness.physics_pose();
|
||||
let current_pose = Pose2D::new(physics_pose.x, physics_pose.y, physics_pose.theta);
|
||||
|
||||
// Update controller with any pending collision
|
||||
let step = controller.update(harness.slam(), current_pose, pending_collision.take());
|
||||
|
||||
// Check completion states
|
||||
match &step.state {
|
||||
ExplorationState::Complete => {
|
||||
println!(" Exploration complete - no frontiers remain!");
|
||||
break;
|
||||
}
|
||||
ExplorationState::Failed { reason } => {
|
||||
println!(" Exploration failed - {}", reason);
|
||||
break;
|
||||
}
|
||||
ExplorationState::Idle => {
|
||||
break;
|
||||
}
|
||||
ExplorationState::SelectingFrontier => {
|
||||
// Step simulation forward while selecting
|
||||
harness.step(0.0, 0.0);
|
||||
// Process lidar while selecting
|
||||
if step_count % LIDAR_INTERVAL == 0 {
|
||||
harness.process_lidar();
|
||||
}
|
||||
}
|
||||
ExplorationState::FollowingPath {
|
||||
target_frontier,
|
||||
path,
|
||||
..
|
||||
} => {
|
||||
// Track cycles (new target = new cycle)
|
||||
let is_new_target = last_target
|
||||
.map(|last| last.distance(*target_frontier) > 0.3)
|
||||
.unwrap_or(true);
|
||||
|
||||
if is_new_target {
|
||||
cycles += 1;
|
||||
last_target = Some(*target_frontier);
|
||||
|
||||
let robot_pos = current_pose.position();
|
||||
println!(
|
||||
" Cycle {}: Path to ({:.2}, {:.2}), dist: {:.2}m, path: {:.2}m, {} waypoints",
|
||||
cycles,
|
||||
target_frontier.x,
|
||||
target_frontier.y,
|
||||
robot_pos.distance(*target_frontier),
|
||||
path.length,
|
||||
path.points.len()
|
||||
);
|
||||
|
||||
// Capture CBVG for visualization using visibility-based building
|
||||
// This ensures nodes are only in lidar-scanned areas
|
||||
last_cbvg = Some(harness.slam().build_cbvg());
|
||||
last_planned_path = Some(path.clone());
|
||||
}
|
||||
|
||||
// Execute velocity command from controller (or stop if none)
|
||||
let vel = step.velocity.unwrap_or(VelocityCommand::new(0.0, 0.0));
|
||||
let step_result = harness.step(vel.linear, vel.angular);
|
||||
|
||||
// Check for collision
|
||||
if step_result.any_bumper() {
|
||||
collisions += 1;
|
||||
|
||||
let collision_type = match (step_result.left_bumper, step_result.right_bumper) {
|
||||
(true, true) => CollisionType::BumperBoth,
|
||||
(true, false) => CollisionType::BumperLeft,
|
||||
(false, true) => CollisionType::BumperRight,
|
||||
_ => CollisionType::BumperBoth,
|
||||
};
|
||||
|
||||
let theta = step_result.physics_pose.theta;
|
||||
let radius = harness.config().robot.robot_radius;
|
||||
let point = Point2D::new(
|
||||
step_result.physics_pose.x + (radius + 0.02) * theta.cos(),
|
||||
step_result.physics_pose.y + (radius + 0.02) * theta.sin(),
|
||||
);
|
||||
|
||||
// Create collision event for controller
|
||||
pending_collision = Some(CollisionEvent::new(collision_type, point, theta));
|
||||
|
||||
// Also add virtual wall directly to map
|
||||
let wall = pending_collision
|
||||
.as_ref()
|
||||
.unwrap()
|
||||
.to_virtual_wall(config.virtual_wall_length);
|
||||
harness.slam_mut().add_line(wall.line);
|
||||
virtual_walls += 1;
|
||||
|
||||
println!(
|
||||
" Collision at ({:.2}, {:.2}) - added virtual wall",
|
||||
point.x, point.y
|
||||
);
|
||||
}
|
||||
|
||||
// Process lidar periodically
|
||||
if step_count % LIDAR_INTERVAL == 0 {
|
||||
harness.process_lidar();
|
||||
}
|
||||
|
||||
// Log progress periodically
|
||||
if cycles > last_cycle_print && step_count % 100 == 0 {
|
||||
let end_pos = harness.physics_pose().position();
|
||||
let current_frontiers = harness.slam().frontiers().len();
|
||||
println!(
|
||||
" Position: ({:.2}, {:.2}), frontiers: {}, unreachable: {}",
|
||||
end_pos.x,
|
||||
end_pos.y,
|
||||
current_frontiers,
|
||||
controller.unreachable_frontier_count()
|
||||
);
|
||||
last_cycle_print = cycles;
|
||||
}
|
||||
}
|
||||
ExplorationState::RecoveringFromCollision { .. } => {
|
||||
// Execute backoff velocity (or stop if none)
|
||||
let vel = step.velocity.unwrap_or(VelocityCommand::new(0.0, 0.0));
|
||||
harness.step(vel.linear, vel.angular);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Final log
|
||||
let end_pos = harness.physics_pose().position();
|
||||
let current_frontiers = harness.slam().frontiers().len();
|
||||
println!(
|
||||
" Final position: ({:.2}, {:.2}), frontiers: {}, unreachable: {}",
|
||||
end_pos.x,
|
||||
end_pos.y,
|
||||
current_frontiers,
|
||||
controller.unreachable_frontier_count()
|
||||
);
|
||||
|
||||
// Get line count before optimization
|
||||
let map_lines_before = harness.slam().lines().len();
|
||||
|
||||
// Optimize coplanar lines with tight thresholds
|
||||
let coplanar_config = CoplanarMergeConfig::default()
|
||||
.with_max_angle_diff(3.0 * std::f32::consts::PI / 180.0)
|
||||
.with_max_perpendicular_dist(0.05);
|
||||
|
||||
let lines_merged = harness.slam_mut().optimize_lines(&coplanar_config);
|
||||
|
||||
println!(
|
||||
"\n Line optimization: {} lines -> {} lines ({} merged)",
|
||||
map_lines_before,
|
||||
harness.slam().lines().len(),
|
||||
lines_merged
|
||||
);
|
||||
|
||||
// Finalize to generate visualization with CBVG
|
||||
let _test_result = harness.finalize_with_graph(last_cbvg.as_ref(), last_planned_path.as_ref());
|
||||
|
||||
ExplorationResult {
|
||||
completed: harness.slam().frontiers().is_empty(),
|
||||
sim_time: harness.sim_time(),
|
||||
cycles,
|
||||
collisions,
|
||||
virtual_walls,
|
||||
map_lines_before,
|
||||
map_lines: harness.slam().lines().len(),
|
||||
lines_merged,
|
||||
initial_frontiers,
|
||||
remaining_frontiers: harness.slam().frontiers().len(),
|
||||
}
|
||||
}
|
||||
|
||||
/// Wait for map update by running lidar scans for specified duration.
|
||||
fn wait_for_map_update(harness: &mut TestHarness, duration: f32) {
|
||||
let start_time = harness.sim_time();
|
||||
|
||||
while harness.sim_time() - start_time < duration {
|
||||
// Stop the robot
|
||||
harness.step(0.0, 0.0);
|
||||
// Process lidar
|
||||
harness.process_lidar();
|
||||
}
|
||||
}
|
||||
@@ -3,7 +3,7 @@
|
||||
//! 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};
|
||||
use crate::core::{PointCloud2D, PolarScan};
|
||||
|
||||
/// Convert lidar scan data from sangam-io format to vastu-map PointCloud2D.
|
||||
///
|
||||
@@ -1,4 +1,4 @@
|
||||
//! Core test harness for integration tests
|
||||
//! Core test harness for integration tests and examples
|
||||
//!
|
||||
//! Provides synchronous simulation using sangam-io's mock device components
|
||||
//! directly (without spawning daemon threads).
|
||||
@@ -13,10 +13,10 @@
|
||||
//!
|
||||
//! ```bash
|
||||
//! # Use custom config
|
||||
//! VASTU_MAP_CONFIG=my_config.yaml cargo test --features integration-tests
|
||||
//! VASTU_MAP_CONFIG=my_config.yaml cargo run --example exploration
|
||||
//!
|
||||
//! # Use default config file
|
||||
//! cargo test --features integration-tests
|
||||
//! cargo run --example exploration
|
||||
//! ```
|
||||
|
||||
use std::path::Path;
|
||||
@@ -27,18 +27,18 @@ use sangam_io::devices::mock::map_loader::SimulationMap;
|
||||
use sangam_io::devices::mock::noise::NoiseGenerator;
|
||||
use sangam_io::devices::mock::physics::PhysicsState;
|
||||
|
||||
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 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 crate::adapters::lidar_to_point_cloud;
|
||||
use crate::metrics::{ConvergenceStats, TestMetrics, TimingStats};
|
||||
use crate::path_generator::PathSegment;
|
||||
use crate::visualization::Visualizer;
|
||||
use super::adapters::lidar_to_point_cloud;
|
||||
use super::metrics::{ConvergenceStats, TestMetrics, TimingStats};
|
||||
use super::path::PathSegment;
|
||||
use super::visualization::Visualizer;
|
||||
|
||||
/// Test harness configuration.
|
||||
#[derive(Clone)]
|
||||
@@ -158,11 +158,7 @@ impl Default for HarnessConfig {
|
||||
impl HarnessConfig {
|
||||
/// Create config for the simple_room map.
|
||||
pub fn simple_room() -> Self {
|
||||
// Use env!() to get project root at compile time
|
||||
let map_file = concat!(
|
||||
env!("CARGO_MANIFEST_DIR"),
|
||||
"/../sangam-io/maps/simple_room.yaml"
|
||||
);
|
||||
let map_file = concat!(env!("CARGO_MANIFEST_DIR"), "/data/maps/simple_room.yaml");
|
||||
Self {
|
||||
map_file: map_file.to_string(),
|
||||
start_x: 2.5,
|
||||
@@ -171,26 +167,9 @@ impl HarnessConfig {
|
||||
}
|
||||
}
|
||||
|
||||
/// Create config for the large_room_obstacles map.
|
||||
pub fn large_room_obstacles() -> Self {
|
||||
let map_file = concat!(
|
||||
env!("CARGO_MANIFEST_DIR"),
|
||||
"/../sangam-io/maps/large_room_obstacles.yaml"
|
||||
);
|
||||
Self {
|
||||
map_file: map_file.to_string(),
|
||||
start_x: 5.0,
|
||||
start_y: 5.0,
|
||||
..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"),
|
||||
"/tests/integration/scenarios/maps/medium_room.yaml"
|
||||
);
|
||||
let map_file = concat!(env!("CARGO_MANIFEST_DIR"), "/data/maps/medium_room.yaml");
|
||||
Self {
|
||||
map_file: map_file.to_string(),
|
||||
start_x: 4.0,
|
||||
@@ -202,10 +181,7 @@ impl HarnessConfig {
|
||||
|
||||
/// Create config for the large_room map.
|
||||
pub fn large_room() -> Self {
|
||||
let map_file = concat!(
|
||||
env!("CARGO_MANIFEST_DIR"),
|
||||
"/tests/integration/scenarios/maps/large_room.yaml"
|
||||
);
|
||||
let map_file = concat!(env!("CARGO_MANIFEST_DIR"), "/data/maps/large_room.yaml");
|
||||
Self {
|
||||
map_file: map_file.to_string(),
|
||||
start_x: 5.0,
|
||||
@@ -213,6 +189,17 @@ impl HarnessConfig {
|
||||
..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.
|
||||
@@ -285,7 +272,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<vastu_map::core::PointCloud2D>,
|
||||
last_scan_robot_frame: Option<crate::core::PointCloud2D>,
|
||||
/// ICP convergence statistics across all observations
|
||||
convergence_stats: ConvergenceStats,
|
||||
/// Trajectory history for enhanced visualization
|
||||
@@ -6,10 +6,11 @@
|
||||
//! - Timing statistics for algorithm performance
|
||||
|
||||
use sangam_io::devices::mock::map_loader::SimulationMap;
|
||||
use vastu_map::TimingBreakdown;
|
||||
use vastu_map::VectorMap;
|
||||
use vastu_map::core::Pose2D;
|
||||
use vastu_map::features::Line2D;
|
||||
|
||||
use crate::TimingBreakdown;
|
||||
use crate::VectorMap;
|
||||
use crate::core::Pose2D;
|
||||
use crate::features::Line2D;
|
||||
|
||||
/// Statistics collected during ICP matching over a test run.
|
||||
#[derive(Debug, Clone, Default)]
|
||||
@@ -0,0 +1,54 @@
|
||||
//! 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;
|
||||
@@ -1,8 +1,6 @@
|
||||
//! Robot path segment for integration tests
|
||||
//! Robot path segment for simulation and testing
|
||||
//!
|
||||
//! Provides PathSegment for defining robot motion commands in YAML scenarios.
|
||||
|
||||
use std::f32::consts::PI;
|
||||
//! Provides PathSegment for defining robot motion commands.
|
||||
|
||||
/// A path segment with velocity commands and duration.
|
||||
#[derive(Clone, Debug)]
|
||||
@@ -0,0 +1,10 @@
|
||||
//! 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};
|
||||
+6
-5
@@ -2,11 +2,12 @@
|
||||
//!
|
||||
//! Loads and executes test scenarios defined in YAML files.
|
||||
|
||||
use crate::harness::{HarnessConfig, TestHarness, TestResult};
|
||||
use crate::path_generator::PathSegment;
|
||||
use crate::yaml_config::{PathCommand, ScenarioConfig};
|
||||
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;
|
||||
|
||||
@@ -62,7 +63,7 @@ pub fn run_scenario<P: AsRef<Path>>(
|
||||
/// Handles both wheel commands and stop commands:
|
||||
/// - Wheel commands use differential drive kinematics
|
||||
/// - Stop commands create stationary segments
|
||||
fn convert_path_commands(commands: &[PathCommand]) -> Vec<PathSegment> {
|
||||
pub fn convert_path_commands(commands: &[PathCommand]) -> Vec<PathSegment> {
|
||||
commands
|
||||
.iter()
|
||||
.filter_map(|cmd| match cmd {
|
||||
@@ -148,7 +149,7 @@ pub fn run_all_scenarios<P: AsRef<Path>>(dir: P) -> Vec<(String, Result<TestResu
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use crate::yaml_config::WheelCommand;
|
||||
use crate::harness::scenario::config::WheelCommand;
|
||||
|
||||
#[test]
|
||||
fn test_wheel_command_forward() {
|
||||
+7
-7
@@ -16,14 +16,14 @@ use svg::node::element::{
|
||||
Circle, Definitions, Group, Line, Marker, Path, Polyline, Rectangle, Text,
|
||||
};
|
||||
|
||||
use vastu_map::Path as PlannedPath;
|
||||
use vastu_map::VectorMap;
|
||||
use vastu_map::core::{Point2D, Pose2D};
|
||||
use vastu_map::integration::ScanStore;
|
||||
use vastu_map::query::{ClearanceVisibilityGraph, NodeType};
|
||||
use crate::Path as PlannedPath;
|
||||
use crate::VectorMap;
|
||||
use crate::core::{Point2D, Pose2D};
|
||||
use crate::integration::ScanStore;
|
||||
use crate::query::{ClearanceVisibilityGraph, NodeType};
|
||||
|
||||
use crate::harness::TrajectoryHistory;
|
||||
use crate::metrics::TestMetrics;
|
||||
use super::harness::TrajectoryHistory;
|
||||
use super::metrics::TestMetrics;
|
||||
|
||||
/// Colorblind-friendly color palette (Okabe-Ito).
|
||||
mod colors {
|
||||
@@ -123,6 +123,10 @@ 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};
|
||||
|
||||
@@ -1,184 +0,0 @@
|
||||
# Integration Tests
|
||||
|
||||
YAML-based integration testing framework for vastu-map SLAM algorithms using sangam-io's mock device simulator.
|
||||
|
||||
## Quick Start
|
||||
|
||||
```bash
|
||||
# Run all integration tests
|
||||
cargo test --features integration-tests -- --nocapture
|
||||
|
||||
# Run a specific scenario
|
||||
cargo test --features integration-tests -- test_straight_line --nocapture
|
||||
|
||||
# Run all YAML scenarios with summary
|
||||
cargo test --features integration-tests -- test_yaml_scenarios --nocapture
|
||||
```
|
||||
|
||||
## Architecture
|
||||
|
||||
```
|
||||
tests/integration/
|
||||
├── mod.rs # Module exports
|
||||
├── harness.rs # Core test harness (physics + SLAM simulation)
|
||||
├── metrics.rs # Validation metrics (position/orientation error, wall accuracy)
|
||||
├── visualization.rs # SVG trajectory visualization
|
||||
├── yaml_config.rs # YAML scenario configuration structs
|
||||
├── yaml_runner.rs # Scenario loader and executor
|
||||
├── path_generator.rs # PathSegment for motion commands
|
||||
├── adapters.rs # Type conversions (sangam-io ↔ vastu-map)
|
||||
└── scenarios/
|
||||
├── yaml_tests.rs # Test entry points
|
||||
├── maps/ # Map definitions (PGM + YAML)
|
||||
└── *.yaml # Test scenarios
|
||||
```
|
||||
|
||||
## YAML Scenario Format
|
||||
|
||||
Each scenario defines a robot path through a simulated environment:
|
||||
|
||||
```yaml
|
||||
name: "my_scenario"
|
||||
map: "maps/medium_room.yaml"
|
||||
start_pose:
|
||||
x: 4.0
|
||||
y: 4.0
|
||||
theta: 0.0 # radians, 0 = facing right (+X)
|
||||
path:
|
||||
- {left: 1.0, right: 1.0} # Forward 1m
|
||||
- {left: -0.183, right: 0.183} # Turn left 90°
|
||||
- {left: 0.5, right: 0.5, speed: 0.3} # Forward 0.5m at 0.3 m/s
|
||||
- {stop: 2.0} # Stationary for 2 seconds
|
||||
```
|
||||
|
||||
### Wheel Commands
|
||||
|
||||
Uses differential drive kinematics (wheel_base = 0.233m):
|
||||
|
||||
| Motion | Left Wheel | Right Wheel | Notes |
|
||||
|--------|------------|-------------|-------|
|
||||
| Forward 1m | 1.0 | 1.0 | Equal distances |
|
||||
| Backward 1m | -1.0 | -1.0 | Negative = reverse |
|
||||
| Turn left 90° | -0.183 | 0.183 | θ × wheel_base / 2 |
|
||||
| Turn right 90° | 0.183 | -0.183 | Opposite signs |
|
||||
| Arc left | 0.5 | 0.7 | Outer wheel travels more |
|
||||
|
||||
### Common Turn Values
|
||||
|
||||
| Angle | Per-wheel distance |
|
||||
|-------|-------------------|
|
||||
| 90° (π/2) | 0.183m |
|
||||
| 180° (π) | 0.366m |
|
||||
| 360° (2π) | 0.732m |
|
||||
|
||||
## Adding New Scenarios
|
||||
|
||||
1. Create a YAML file in `scenarios/`:
|
||||
|
||||
```yaml
|
||||
name: "my_new_test"
|
||||
map: "maps/medium_room.yaml"
|
||||
start_pose:
|
||||
x: 2.0
|
||||
y: 4.0
|
||||
theta: 0.0
|
||||
path:
|
||||
- {left: 1.0, right: 1.0}
|
||||
```
|
||||
|
||||
2. Add a test function in `scenarios/yaml_tests.rs`:
|
||||
|
||||
```rust
|
||||
#[test]
|
||||
fn test_my_new_test() {
|
||||
run_single_scenario("my_new_test");
|
||||
}
|
||||
```
|
||||
|
||||
3. Run the test:
|
||||
|
||||
```bash
|
||||
cargo test --features integration-tests -- test_my_new_test --nocapture
|
||||
```
|
||||
|
||||
## Available Scenarios
|
||||
|
||||
### Basic Tests
|
||||
- `static_scan_test` - Stationary observation (5 seconds)
|
||||
- `straight_line_test` - Pure forward motion
|
||||
- `pure_rotation_test` - Rotation only, no translation
|
||||
- `simple_room_straight_line` - Simple room with straight motion
|
||||
|
||||
### Complex Scenarios
|
||||
- `loop_closure_test` - 2m square path, returns to start
|
||||
- `medium_room_perimeter` - Full room perimeter
|
||||
- `medium_room_exploration` - Multi-room exploration
|
||||
- `medium_room_obstacles` - Navigation around obstacles
|
||||
|
||||
### Stress Tests
|
||||
- `icp_sparse_corridor` - Narrow corridor (sparse features)
|
||||
- `drift_continuous_curve` - Curved path (odometry drift)
|
||||
- `drift_long_path` - Extended path (cumulative error)
|
||||
|
||||
## Test Output
|
||||
|
||||
### Metrics
|
||||
|
||||
Each test reports:
|
||||
- **Position error**: Euclidean distance from ground truth (meters)
|
||||
- **Orientation error**: Angular difference from ground truth (degrees)
|
||||
- **Wall accuracy**: Percentage of detected lines near actual walls
|
||||
- **ICP stats**: Average/max iterations, convergence failures
|
||||
|
||||
### Visualization
|
||||
|
||||
SVG files are generated in `results/`:
|
||||
|
||||
```bash
|
||||
open results/straight_line_test.svg
|
||||
```
|
||||
|
||||
Shows:
|
||||
- Ground truth trajectory (blue)
|
||||
- SLAM estimated trajectory (red → yellow → green by confidence)
|
||||
- Detected walls (teal)
|
||||
- Map walls (gray background)
|
||||
|
||||
## Test Harness
|
||||
|
||||
The `TestHarness` provides:
|
||||
|
||||
1. **Physics simulation** via sangam-io's mock device
|
||||
2. **Lidar ray-casting** at 5Hz
|
||||
3. **SLAM pipeline** execution
|
||||
4. **Ground truth comparison**
|
||||
|
||||
```rust
|
||||
let config = HarnessConfig {
|
||||
map_file: "path/to/map.yaml".into(),
|
||||
start_x: 2.5,
|
||||
start_y: 2.5,
|
||||
start_theta: 0.0,
|
||||
..Default::default()
|
||||
};
|
||||
|
||||
let mut harness = TestHarness::new(config)?
|
||||
.with_visualization("output.svg");
|
||||
|
||||
let result = harness.run_path(&path_segments);
|
||||
println!("{}", result.metrics.summary());
|
||||
```
|
||||
|
||||
## Maps
|
||||
|
||||
Maps are defined as PGM images with YAML metadata:
|
||||
|
||||
```
|
||||
scenarios/maps/
|
||||
├── simple_room.yaml # 5m × 5m basic room
|
||||
├── medium_room.yaml # 8m × 8m with obstacles
|
||||
├── large_room.yaml # Expansive space
|
||||
└── corridor.yaml # Narrow corridor
|
||||
```
|
||||
|
||||
Each map YAML references a PGM file and defines resolution/origin.
|
||||
@@ -1,31 +0,0 @@
|
||||
//! Integration tests for vastu-map using sangam-io mock device
|
||||
//!
|
||||
//! These tests verify the SLAM pipeline using simulated sensor data from
|
||||
//! sangam-io's mock device, which provides ray-cast lidar in simulated environments.
|
||||
//!
|
||||
//! # Running Tests
|
||||
//!
|
||||
//! ```bash
|
||||
//! # Run all integration tests
|
||||
//! cargo test --features integration-tests -- --nocapture
|
||||
//!
|
||||
//! # Run specific scenario
|
||||
//! cargo test --features integration-tests square_room
|
||||
//!
|
||||
//! # View generated visualizations
|
||||
//! open test_output/square_room.svg
|
||||
//! ```
|
||||
|
||||
mod adapters;
|
||||
mod harness;
|
||||
mod metrics;
|
||||
mod path_generator;
|
||||
mod scenarios;
|
||||
mod visualization;
|
||||
mod yaml_config;
|
||||
mod yaml_runner;
|
||||
|
||||
// Re-export for test convenience
|
||||
pub use harness::{HarnessConfig, StepResult, TestHarness, TestResult};
|
||||
pub use metrics::{AcceptanceCriteria, TestMetrics, TimingStats};
|
||||
pub use path_generator::PathSegment;
|
||||
@@ -1,387 +0,0 @@
|
||||
//! Autonomous exploration integration test.
|
||||
//!
|
||||
//! Tests the complete exploration pipeline using vastu_map's public APIs:
|
||||
//! 1. Frontier detection
|
||||
//! 2. Path planning to frontiers
|
||||
//! 3. Path following with collision detection
|
||||
//! 4. Virtual wall insertion on unexpected collisions
|
||||
//! 5. Exploration completion when no frontiers remain
|
||||
//!
|
||||
//! # Exploration Algorithm
|
||||
//!
|
||||
//! 1. Stop and wait for map update (1 second of lidar scans)
|
||||
//! 2. Find the nearest frontier from updated map
|
||||
//! 3. Plan path and move towards frontier
|
||||
//! 4. Stop and wait for map update
|
||||
//! 5. Re-check frontiers, find nearest
|
||||
//! 6. Repeat until no frontiers remain
|
||||
//!
|
||||
//! # Running the test
|
||||
//!
|
||||
//! ```bash
|
||||
//! cargo test --features integration-tests exploration -- --nocapture
|
||||
//! ```
|
||||
//!
|
||||
//! Output SVG is saved to `results/exploration_test.svg`
|
||||
|
||||
use crate::{HarnessConfig, TestHarness};
|
||||
use vastu_map::Map;
|
||||
use vastu_map::Path as PlannedPath;
|
||||
use vastu_map::config::ExplorationConfig as MapExplorationConfig;
|
||||
use vastu_map::core::{Point2D, Pose2D};
|
||||
use vastu_map::exploration::{
|
||||
CollisionEvent, CollisionType, ExplorationConfig, ExplorationController, ExplorationState,
|
||||
VelocityCommand,
|
||||
};
|
||||
use vastu_map::integration::CoplanarMergeConfig;
|
||||
use vastu_map::query::cbvg::ClearanceVisibilityGraph;
|
||||
|
||||
/// Maximum simulation time in seconds.
|
||||
const MAX_TIME: f32 = 200.0; // 3.3 minutes
|
||||
|
||||
/// Result of exploration test.
|
||||
#[derive(Debug)]
|
||||
pub struct ExplorationTestResult {
|
||||
/// Whether exploration completed (no frontiers remain).
|
||||
pub completed: bool,
|
||||
/// Total simulation time.
|
||||
pub sim_time: f32,
|
||||
/// Number of exploration cycles.
|
||||
pub cycles: usize,
|
||||
/// Number of collisions handled.
|
||||
pub collisions: usize,
|
||||
/// Number of virtual walls added.
|
||||
pub virtual_walls: usize,
|
||||
/// Map line count before optimization.
|
||||
pub map_lines_before: usize,
|
||||
/// Final map line count (after optimization).
|
||||
pub map_lines: usize,
|
||||
/// Number of lines merged during optimization.
|
||||
pub lines_merged: usize,
|
||||
/// Initial frontier count.
|
||||
pub initial_frontiers: usize,
|
||||
/// Final frontier count (should be 0 on success).
|
||||
pub remaining_frontiers: usize,
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
/// Test autonomous exploration of medium_room map with visualization.
|
||||
///
|
||||
/// This test verifies:
|
||||
/// 1. The robot can explore the entire map
|
||||
/// 2. Collisions are handled with virtual walls
|
||||
/// 3. All frontiers are eventually closed
|
||||
///
|
||||
/// Output: `results/exploration_test.svg`
|
||||
#[test]
|
||||
fn test_autonomous_exploration() {
|
||||
env_logger::try_init().ok();
|
||||
|
||||
// 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();
|
||||
|
||||
// Enable VectorMap exploration mode for point-cloud-based line re-fitting
|
||||
// This will re-fit lines every 10 observations using accumulated scan data
|
||||
let map_exploration_config = MapExplorationConfig::default()
|
||||
.with_refit_interval(10)
|
||||
.with_gap_threshold(0.3);
|
||||
|
||||
let mut harness = TestHarness::new(harness_config)
|
||||
.expect("Failed to create harness")
|
||||
.with_exploration_mode(Some(map_exploration_config))
|
||||
.with_visualization("results/exploration_test.svg");
|
||||
|
||||
// Create robot exploration config (for path following)
|
||||
let exploration_config = ExplorationConfig::default()
|
||||
.with_max_linear_speed(0.3)
|
||||
.with_max_angular_speed(1.0)
|
||||
.with_backoff_distance(0.15)
|
||||
.with_robot_radius(0.15);
|
||||
|
||||
// Run exploration
|
||||
let result = run_exploration_loop(&mut harness, exploration_config);
|
||||
|
||||
// Print results
|
||||
println!("\n=== Exploration Test Results ===");
|
||||
println!("Completed: {}", result.completed);
|
||||
println!("Simulation time: {:.1}s", result.sim_time);
|
||||
println!("Cycles: {}", result.cycles);
|
||||
println!("Collisions handled: {}", result.collisions);
|
||||
println!("Virtual walls added: {}", result.virtual_walls);
|
||||
println!(
|
||||
"Map lines: {} before optimization -> {} after ({} merged)",
|
||||
result.map_lines_before, result.map_lines, result.lines_merged
|
||||
);
|
||||
println!(
|
||||
"Frontiers: {} initial -> {} remaining",
|
||||
result.initial_frontiers, result.remaining_frontiers
|
||||
);
|
||||
println!("\nVisualization saved to: results/exploration_test.svg");
|
||||
|
||||
// Assertions for exploration quality
|
||||
// 1. Map should have reasonable number of lines (room walls + obstacles)
|
||||
// Note: line optimization can merge many lines, so threshold is conservative
|
||||
assert!(
|
||||
result.map_lines >= 10,
|
||||
"Map has too few lines: {} (expected >= 10 for room + obstacles)",
|
||||
result.map_lines
|
||||
);
|
||||
|
||||
// 2. Robot should have explored significant number of frontiers
|
||||
// (better metric than cycles, since nearby frontiers don't count as new cycles)
|
||||
let frontiers_explored = result
|
||||
.initial_frontiers
|
||||
.saturating_sub(result.remaining_frontiers);
|
||||
assert!(
|
||||
frontiers_explored >= 8 || result.completed,
|
||||
"Robot did not explore enough: only {} frontiers explored (expected >= 8)",
|
||||
frontiers_explored
|
||||
);
|
||||
|
||||
// 3. Virtual walls should be added when collisions happen
|
||||
if result.collisions > 0 {
|
||||
assert!(
|
||||
result.virtual_walls > 0,
|
||||
"Collisions occurred but no virtual walls added"
|
||||
);
|
||||
}
|
||||
|
||||
// 4. Exploration should either complete or have reasonable remaining frontiers
|
||||
// Note: Frontier count can fluctuate as robot explores and discovers new areas
|
||||
assert!(
|
||||
result.completed || result.remaining_frontiers <= 30,
|
||||
"Too many frontiers remaining: {} (expected <= 30 or completed)",
|
||||
result.remaining_frontiers
|
||||
);
|
||||
}
|
||||
|
||||
/// Run the exploration loop using the ExplorationController with region-based TSP.
|
||||
///
|
||||
/// This uses a continuous control loop where the controller is called every step,
|
||||
/// properly integrating with its state machine for path following and collision handling.
|
||||
fn run_exploration_loop(
|
||||
harness: &mut TestHarness,
|
||||
config: ExplorationConfig,
|
||||
) -> ExplorationTestResult {
|
||||
let mut cycles = 0;
|
||||
let mut collisions = 0;
|
||||
let mut virtual_walls = 0;
|
||||
|
||||
// Variables to capture the last CBVG and path for visualization
|
||||
let mut last_cbvg: Option<ClearanceVisibilityGraph> = None;
|
||||
let mut last_planned_path: Option<PlannedPath> = None;
|
||||
|
||||
println!("Starting exploration with region-based TSP planning...");
|
||||
|
||||
// Create the exploration controller with region-based algorithm
|
||||
let mut controller = ExplorationController::new(config.clone());
|
||||
|
||||
// Phase 1: Initial map update (longer for initial observation)
|
||||
wait_for_map_update(harness, 2.0);
|
||||
|
||||
let initial_frontiers = harness.slam().frontiers().len();
|
||||
println!("Initial frontiers: {}", initial_frontiers);
|
||||
|
||||
// Start exploration
|
||||
controller.start();
|
||||
|
||||
let mut step_count = 0;
|
||||
let mut last_target: Option<Point2D> = None;
|
||||
let mut last_cycle_print = 0;
|
||||
let mut pending_collision: Option<CollisionEvent> = None;
|
||||
const MAX_STEPS: usize = 20000;
|
||||
const LIDAR_INTERVAL: usize = 5; // Process lidar every N steps
|
||||
|
||||
while step_count < MAX_STEPS && harness.sim_time() < MAX_TIME {
|
||||
step_count += 1;
|
||||
|
||||
// Get current pose
|
||||
let physics_pose = harness.physics_pose();
|
||||
let current_pose = Pose2D::new(physics_pose.x, physics_pose.y, physics_pose.theta);
|
||||
|
||||
// Update controller with any pending collision
|
||||
let step = controller.update(harness.slam(), current_pose, pending_collision.take());
|
||||
|
||||
// Check completion states
|
||||
match &step.state {
|
||||
ExplorationState::Complete => {
|
||||
println!(" Exploration complete - no frontiers remain!");
|
||||
break;
|
||||
}
|
||||
ExplorationState::Failed { reason } => {
|
||||
println!(" Exploration failed - {}", reason);
|
||||
break;
|
||||
}
|
||||
ExplorationState::Idle => {
|
||||
break;
|
||||
}
|
||||
ExplorationState::SelectingFrontier => {
|
||||
// Step simulation forward while selecting
|
||||
harness.step(0.0, 0.0);
|
||||
// Process lidar while selecting
|
||||
if step_count % LIDAR_INTERVAL == 0 {
|
||||
harness.process_lidar();
|
||||
}
|
||||
}
|
||||
ExplorationState::FollowingPath {
|
||||
target_frontier,
|
||||
path,
|
||||
..
|
||||
} => {
|
||||
// Track cycles (new target = new cycle)
|
||||
let is_new_target = last_target
|
||||
.map(|last| last.distance(*target_frontier) > 0.3)
|
||||
.unwrap_or(true);
|
||||
|
||||
if is_new_target {
|
||||
cycles += 1;
|
||||
last_target = Some(*target_frontier);
|
||||
|
||||
let robot_pos = current_pose.position();
|
||||
println!(
|
||||
" Cycle {}: Path to ({:.2}, {:.2}), dist: {:.2}m, path: {:.2}m, {} waypoints",
|
||||
cycles,
|
||||
target_frontier.x,
|
||||
target_frontier.y,
|
||||
robot_pos.distance(*target_frontier),
|
||||
path.length,
|
||||
path.points.len()
|
||||
);
|
||||
|
||||
// Capture CBVG for visualization using visibility-based building
|
||||
// This ensures nodes are only in lidar-scanned areas
|
||||
last_cbvg = Some(harness.slam().build_cbvg());
|
||||
last_planned_path = Some(path.clone());
|
||||
}
|
||||
|
||||
// Execute velocity command from controller (or stop if none)
|
||||
let vel = step.velocity.unwrap_or(VelocityCommand::new(0.0, 0.0));
|
||||
let step_result = harness.step(vel.linear, vel.angular);
|
||||
|
||||
// Check for collision
|
||||
if step_result.any_bumper() {
|
||||
collisions += 1;
|
||||
|
||||
let collision_type =
|
||||
match (step_result.left_bumper, step_result.right_bumper) {
|
||||
(true, true) => CollisionType::BumperBoth,
|
||||
(true, false) => CollisionType::BumperLeft,
|
||||
(false, true) => CollisionType::BumperRight,
|
||||
_ => CollisionType::BumperBoth,
|
||||
};
|
||||
|
||||
let theta = step_result.physics_pose.theta;
|
||||
let radius = harness.config().robot.robot_radius;
|
||||
let point = Point2D::new(
|
||||
step_result.physics_pose.x + (radius + 0.02) * theta.cos(),
|
||||
step_result.physics_pose.y + (radius + 0.02) * theta.sin(),
|
||||
);
|
||||
|
||||
// Create collision event for controller
|
||||
pending_collision = Some(CollisionEvent::new(collision_type, point, theta));
|
||||
|
||||
// Also add virtual wall directly to map
|
||||
let wall = pending_collision
|
||||
.as_ref()
|
||||
.unwrap()
|
||||
.to_virtual_wall(config.virtual_wall_length);
|
||||
harness.slam_mut().add_line(wall.line);
|
||||
virtual_walls += 1;
|
||||
|
||||
println!(
|
||||
" Collision at ({:.2}, {:.2}) - added virtual wall",
|
||||
point.x, point.y
|
||||
);
|
||||
}
|
||||
|
||||
// Process lidar periodically
|
||||
if step_count % LIDAR_INTERVAL == 0 {
|
||||
harness.process_lidar();
|
||||
}
|
||||
|
||||
// Log progress periodically
|
||||
if cycles > last_cycle_print && step_count % 100 == 0 {
|
||||
let end_pos = harness.physics_pose().position();
|
||||
let current_frontiers = harness.slam().frontiers().len();
|
||||
println!(
|
||||
" Position: ({:.2}, {:.2}), frontiers: {}, unreachable: {}",
|
||||
end_pos.x,
|
||||
end_pos.y,
|
||||
current_frontiers,
|
||||
controller.unreachable_frontier_count()
|
||||
);
|
||||
last_cycle_print = cycles;
|
||||
}
|
||||
}
|
||||
ExplorationState::RecoveringFromCollision { .. } => {
|
||||
// Execute backoff velocity (or stop if none)
|
||||
let vel = step.velocity.unwrap_or(VelocityCommand::new(0.0, 0.0));
|
||||
harness.step(vel.linear, vel.angular);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Final log
|
||||
let end_pos = harness.physics_pose().position();
|
||||
let current_frontiers = harness.slam().frontiers().len();
|
||||
println!(
|
||||
" Final position: ({:.2}, {:.2}), frontiers: {}, unreachable: {}",
|
||||
end_pos.x,
|
||||
end_pos.y,
|
||||
current_frontiers,
|
||||
controller.unreachable_frontier_count()
|
||||
);
|
||||
|
||||
// Get line count before optimization
|
||||
let map_lines_before = harness.slam().lines().len();
|
||||
|
||||
// Optimize coplanar lines with tight thresholds
|
||||
let coplanar_config = CoplanarMergeConfig::default()
|
||||
.with_max_angle_diff(3.0 * std::f32::consts::PI / 180.0)
|
||||
.with_max_perpendicular_dist(0.05);
|
||||
|
||||
let lines_merged = harness.slam_mut().optimize_lines(&coplanar_config);
|
||||
|
||||
println!(
|
||||
"\n Line optimization: {} lines -> {} lines ({} merged)",
|
||||
map_lines_before,
|
||||
harness.slam().lines().len(),
|
||||
lines_merged
|
||||
);
|
||||
|
||||
// Finalize to generate visualization with CBVG
|
||||
let _test_result =
|
||||
harness.finalize_with_graph(last_cbvg.as_ref(), last_planned_path.as_ref());
|
||||
|
||||
ExplorationTestResult {
|
||||
completed: harness.slam().frontiers().is_empty(),
|
||||
sim_time: harness.sim_time(),
|
||||
cycles,
|
||||
collisions,
|
||||
virtual_walls,
|
||||
map_lines_before,
|
||||
map_lines: harness.slam().lines().len(),
|
||||
lines_merged,
|
||||
initial_frontiers,
|
||||
remaining_frontiers: harness.slam().frontiers().len(),
|
||||
}
|
||||
}
|
||||
|
||||
/// Wait for map update by running lidar scans for specified duration.
|
||||
fn wait_for_map_update(harness: &mut TestHarness, duration: f32) {
|
||||
let start_time = harness.sim_time();
|
||||
|
||||
while harness.sim_time() - start_time < duration {
|
||||
// Stop the robot
|
||||
harness.step(0.0, 0.0);
|
||||
// Process lidar
|
||||
harness.process_lidar();
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,17 +0,0 @@
|
||||
//! Test scenarios for integration tests
|
||||
//!
|
||||
//! Contains both YAML-based predefined path tests and
|
||||
//! autonomous exploration tests.
|
||||
//!
|
||||
//! # Running Tests
|
||||
//!
|
||||
//! ```bash
|
||||
//! # Run all integration tests
|
||||
//! cargo test --features integration-tests -- --nocapture
|
||||
//!
|
||||
//! # Run exploration test
|
||||
//! cargo test --features integration-tests exploration -- --nocapture
|
||||
//! ```
|
||||
|
||||
mod exploration_test;
|
||||
mod yaml_tests;
|
||||
@@ -1,189 +0,0 @@
|
||||
//! YAML-based scenario tests
|
||||
//!
|
||||
//! Runs all test scenarios defined in YAML files in the scenarios directory.
|
||||
//!
|
||||
//! # Usage
|
||||
//!
|
||||
//! ```bash
|
||||
//! # Run all scenarios
|
||||
//! cargo test --features integration-tests -- yaml_tests --nocapture
|
||||
//!
|
||||
//! # Run a single scenario by name
|
||||
//! cargo test --features integration-tests -- test_static_scan --nocapture
|
||||
//! cargo test --features integration-tests -- test_straight_line --nocapture
|
||||
//! cargo test --features integration-tests -- test_loop_closure --nocapture
|
||||
//! ```
|
||||
//!
|
||||
//! # Adding New Scenarios
|
||||
//!
|
||||
//! Create a new `.yaml` file in `tests/integration/scenarios/` with:
|
||||
//!
|
||||
//! ```yaml
|
||||
//! name: "my_scenario"
|
||||
//! map: "maps/medium_room.yaml"
|
||||
//! start_pose:
|
||||
//! x: 4.0
|
||||
//! y: 4.0
|
||||
//! theta: 0.0
|
||||
//! path:
|
||||
//! - {left: 0.5, right: 0.5} # forward 0.5m
|
||||
//! - {left: -0.183, right: 0.183} # turn left 90°
|
||||
//! ```
|
||||
|
||||
use crate::yaml_runner;
|
||||
|
||||
/// Run a single scenario by filename (without .yaml extension)
|
||||
fn run_single_scenario(name: &str) {
|
||||
env_logger::try_init().ok();
|
||||
|
||||
let scenario_path = format!(
|
||||
"{}/tests/integration/scenarios/{}.yaml",
|
||||
env!("CARGO_MANIFEST_DIR"),
|
||||
name
|
||||
);
|
||||
|
||||
println!("\n--- Running scenario: {} ---", name);
|
||||
|
||||
match yaml_runner::run_scenario(&scenario_path) {
|
||||
Ok(result) => {
|
||||
println!("{}", result.metrics.summary());
|
||||
println!(
|
||||
"Ground truth: ({:.2}, {:.2}, {:.1}°)",
|
||||
result.ground_truth_pose.x,
|
||||
result.ground_truth_pose.y,
|
||||
result.ground_truth_pose.theta.to_degrees()
|
||||
);
|
||||
println!(
|
||||
"SLAM pose: ({:.2}, {:.2}, {:.1}°)",
|
||||
result.final_pose.x,
|
||||
result.final_pose.y,
|
||||
result.final_pose.theta.to_degrees()
|
||||
);
|
||||
println!("Observations: {}", result.observations);
|
||||
println!("Sim time: {:.2}s", result.sim_time);
|
||||
}
|
||||
Err(e) => {
|
||||
panic!("Scenario {} failed: {}", name, e);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Individual scenario tests - run with: cargo test --features integration-tests -- test_<name>
|
||||
|
||||
#[test]
|
||||
fn test_static_scan() {
|
||||
run_single_scenario("static_scan_test");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_straight_line() {
|
||||
run_single_scenario("straight_line_test");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_pure_rotation() {
|
||||
run_single_scenario("pure_rotation_test");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_loop_closure() {
|
||||
run_single_scenario("loop_closure_test");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_medium_room_perimeter() {
|
||||
run_single_scenario("medium_room_perimeter");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_medium_room_exploration() {
|
||||
run_single_scenario("medium_room_exploration");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_medium_room_obstacles() {
|
||||
run_single_scenario("medium_room_obstacles");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_simple_room_straight_line() {
|
||||
run_single_scenario("simple_room_straight_line");
|
||||
}
|
||||
|
||||
// New stress-test scenarios for ICP convergence and drift validation
|
||||
|
||||
#[test]
|
||||
fn test_icp_sparse_corridor() {
|
||||
run_single_scenario("icp_sparse_corridor");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_drift_long_path() {
|
||||
run_single_scenario("drift_long_path");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_drift_continuous_curve() {
|
||||
run_single_scenario("drift_continuous_curve");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_yaml_scenarios() {
|
||||
env_logger::try_init().ok();
|
||||
|
||||
let scenarios_dir = concat!(env!("CARGO_MANIFEST_DIR"), "/tests/integration/scenarios");
|
||||
|
||||
println!("\n========================================");
|
||||
println!(" YAML Integration Test Scenarios");
|
||||
println!("========================================");
|
||||
|
||||
let results = yaml_runner::run_all_scenarios(scenarios_dir);
|
||||
|
||||
if results.is_empty() {
|
||||
println!("\nNo YAML scenarios found in: {}", scenarios_dir);
|
||||
println!("Create .yaml files in tests/integration/scenarios/");
|
||||
return;
|
||||
}
|
||||
|
||||
println!("\n========================================");
|
||||
println!(" Results Summary");
|
||||
println!("========================================\n");
|
||||
|
||||
let mut passed = 0;
|
||||
let mut failed = 0;
|
||||
let mut failures = Vec::new();
|
||||
|
||||
for (name, result) in &results {
|
||||
match result {
|
||||
Ok(r) => {
|
||||
passed += 1;
|
||||
println!("PASS: {}", name);
|
||||
println!(" Position error: {:.3}m", r.metrics.position_error);
|
||||
println!(
|
||||
" Wall accuracy: {:.0}%",
|
||||
r.metrics.wall_accuracy * 100.0
|
||||
);
|
||||
println!(" Lines detected: {}", r.metrics.detected_walls);
|
||||
println!(
|
||||
" Final pose: ({:.2}, {:.2}, {:.1}°)",
|
||||
r.final_pose.x,
|
||||
r.final_pose.y,
|
||||
r.final_pose.theta.to_degrees()
|
||||
);
|
||||
println!();
|
||||
}
|
||||
Err(e) => {
|
||||
failed += 1;
|
||||
println!("FAIL: {}", name);
|
||||
println!(" Error: {}\n", e);
|
||||
failures.push(name.clone());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
println!("========================================");
|
||||
println!(" Total: {} passed, {} failed", passed, failed);
|
||||
println!("========================================\n");
|
||||
|
||||
assert!(failures.is_empty(), "Failed scenarios: {:?}", failures);
|
||||
}
|
||||
@@ -0,0 +1,2 @@
|
||||
dataset/
|
||||
maps/
|
||||
Reference in New Issue
Block a user