Add timing instrumentation to VectorMap observe pipeline
Add TimingBreakdown struct to track per-stage timing (ICP matching, line extraction, corner detection, association, merging, new features, loop closure) with microsecond precision. Integration test harness now collects and prints timing summaries. Also clean up integration tests by removing unused odometry module and simplifying path_generator to only include PathSegment (unused PathGenerator methods removed).
This commit is contained in:
codetiger
@@ -187,6 +187,44 @@ impl Default for Path {
|
||||
}
|
||||
}
|
||||
|
||||
/// Timing breakdown for a single observation (all times in microseconds).
|
||||
#[derive(Debug, Clone, Default)]
|
||||
pub struct TimingBreakdown {
|
||||
/// Time spent on ICP matching (µs).
|
||||
pub icp_matching_us: u64,
|
||||
/// Time spent on line extraction (µs).
|
||||
pub line_extraction_us: u64,
|
||||
/// Time spent on corner detection (µs).
|
||||
pub corner_detection_us: u64,
|
||||
/// Time spent on line association (µs).
|
||||
pub association_us: u64,
|
||||
/// Time spent on line merging (µs).
|
||||
pub merging_us: u64,
|
||||
/// Time spent finding new features (µs).
|
||||
pub new_features_us: u64,
|
||||
/// Time spent on loop closure detection (µs).
|
||||
pub loop_closure_us: u64,
|
||||
/// Total observation time (µs).
|
||||
pub total_us: u64,
|
||||
}
|
||||
|
||||
impl TimingBreakdown {
|
||||
/// Create a new timing breakdown with all zeros.
|
||||
pub fn new() -> Self {
|
||||
Self::default()
|
||||
}
|
||||
|
||||
/// Get total feature extraction time (line + corner).
|
||||
pub fn extraction_total_us(&self) -> u64 {
|
||||
self.line_extraction_us + self.corner_detection_us
|
||||
}
|
||||
|
||||
/// Get total integration time (association + merging + new features).
|
||||
pub fn integration_total_us(&self) -> u64 {
|
||||
self.association_us + self.merging_us + self.new_features_us
|
||||
}
|
||||
}
|
||||
|
||||
/// Result of observing a scan.
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct ObserveResult {
|
||||
@@ -206,6 +244,8 @@ pub struct ObserveResult {
|
||||
pub icp_converged: bool,
|
||||
/// Whether a loop closure was detected.
|
||||
pub loop_closure_detected: bool,
|
||||
/// Timing breakdown for this observation.
|
||||
pub timing: TimingBreakdown,
|
||||
}
|
||||
|
||||
impl ObserveResult {
|
||||
@@ -220,6 +260,7 @@ impl ObserveResult {
|
||||
icp_iterations: 0,
|
||||
icp_converged: false,
|
||||
loop_closure_detected: false,
|
||||
timing: TimingBreakdown::default(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,5 +1,7 @@
|
||||
//! VectorMap: The main SLAM map implementation.
|
||||
|
||||
use std::time::Instant;
|
||||
|
||||
use crate::core::{Bounds, Point2D, PointCloud2D, Pose2D};
|
||||
use crate::extraction::{detect_corners, extract_lines, extract_lines_hybrid};
|
||||
use crate::features::{Corner2D, FeatureSet, Line2D};
|
||||
@@ -9,7 +11,7 @@ use crate::integration::{
|
||||
use crate::loop_closure::{LoopClosure, LoopClosureDetector};
|
||||
use crate::matching::PointToLineIcp;
|
||||
use crate::query::{detect_frontiers, query_occupancy, raycast};
|
||||
use crate::{Frontier, Map, ObserveResult, Occupancy, Path};
|
||||
use crate::{Frontier, Map, ObserveResult, Occupancy, Path, TimingBreakdown};
|
||||
|
||||
use super::config::VectorMapConfig;
|
||||
use super::line_store::LineStore;
|
||||
@@ -157,6 +159,9 @@ impl VectorMap {
|
||||
/// 3. Updates the map with new features (mapping)
|
||||
/// 4. Checks for loop closure (if enabled)
|
||||
fn process_scan(&mut self, scan: &PointCloud2D, odometry: Pose2D) -> ObserveResult {
|
||||
let total_start = Instant::now();
|
||||
let mut timing = TimingBreakdown::new();
|
||||
|
||||
self.observation_count += 1;
|
||||
|
||||
// If map is empty, use odometry and add all features
|
||||
@@ -170,12 +175,14 @@ impl VectorMap {
|
||||
// Get robot-frame scan points for weighted ICP matching
|
||||
let robot_frame_points: Vec<Point2D> = scan.iter().collect();
|
||||
|
||||
// Try scan matching using persistent ICP matcher with weighted correspondences
|
||||
// ─── ICP Matching ───
|
||||
let icp_start = Instant::now();
|
||||
let match_result = self.icp_matcher.match_scan_robot_frame(
|
||||
&robot_frame_points,
|
||||
self.line_store.lines(),
|
||||
predicted_pose,
|
||||
);
|
||||
timing.icp_matching_us = icp_start.elapsed().as_micros() as u64;
|
||||
|
||||
// Update ICP matcher's confidence state for adaptive coarse search triggering
|
||||
self.icp_matcher
|
||||
@@ -234,23 +241,24 @@ impl VectorMap {
|
||||
|
||||
self.current_pose = final_pose;
|
||||
|
||||
// Extract features from robot-frame scan, transform to world using final pose
|
||||
let features = self.extract_features(scan, &final_pose);
|
||||
// ─── Feature Extraction ───
|
||||
let features = self.extract_features_timed(scan, &final_pose, &mut timing);
|
||||
|
||||
// Update map if mapping is enabled
|
||||
// ─── Feature Integration ───
|
||||
let (features_added, features_merged) = if self.config.mapping_enabled {
|
||||
self.integrate_features(&features)
|
||||
self.integrate_features_timed(&features, &mut timing)
|
||||
} else {
|
||||
(0, 0)
|
||||
};
|
||||
|
||||
// Check for loop closure if enabled
|
||||
// ─── Loop Closure ───
|
||||
let loop_closure_detected = if self.config.loop_closure_enabled {
|
||||
let lc_start = Instant::now();
|
||||
let robot_points: Vec<Point2D> = scan.iter().collect();
|
||||
let feature_lines = features.lines();
|
||||
let feature_corners = features.corners();
|
||||
|
||||
if let Some(loop_closure) = self.loop_detector.process(
|
||||
let detected = if let Some(loop_closure) = self.loop_detector.process(
|
||||
final_pose,
|
||||
&feature_lines,
|
||||
&feature_corners,
|
||||
@@ -260,11 +268,15 @@ impl VectorMap {
|
||||
true
|
||||
} else {
|
||||
false
|
||||
}
|
||||
};
|
||||
timing.loop_closure_us = lc_start.elapsed().as_micros() as u64;
|
||||
detected
|
||||
} else {
|
||||
false
|
||||
};
|
||||
|
||||
timing.total_us = total_start.elapsed().as_micros() as u64;
|
||||
|
||||
ObserveResult {
|
||||
pose: final_pose,
|
||||
confidence,
|
||||
@@ -274,15 +286,19 @@ impl VectorMap {
|
||||
icp_iterations,
|
||||
icp_converged,
|
||||
loop_closure_detected,
|
||||
timing,
|
||||
}
|
||||
}
|
||||
|
||||
/// Initialize map from first scan.
|
||||
fn initialize_from_scan(&mut self, scan: &PointCloud2D, odometry: Pose2D) -> ObserveResult {
|
||||
let total_start = Instant::now();
|
||||
let mut timing = TimingBreakdown::new();
|
||||
|
||||
self.current_pose = odometry;
|
||||
|
||||
// Extract features from robot-frame scan, transform to world using odometry
|
||||
let features = self.extract_features(scan, &odometry);
|
||||
let features = self.extract_features_timed(scan, &odometry, &mut timing);
|
||||
|
||||
// Add all features to map
|
||||
let num_lines = features.lines().len();
|
||||
@@ -295,6 +311,7 @@ impl VectorMap {
|
||||
|
||||
// Initialize first keyframe if loop closure is enabled
|
||||
if self.config.loop_closure_enabled {
|
||||
let lc_start = Instant::now();
|
||||
let robot_points: Vec<Point2D> = scan.iter().collect();
|
||||
let feature_lines = features.lines();
|
||||
let feature_corners = features.corners();
|
||||
@@ -304,8 +321,11 @@ impl VectorMap {
|
||||
&feature_corners,
|
||||
&robot_points,
|
||||
);
|
||||
timing.loop_closure_us = lc_start.elapsed().as_micros() as u64;
|
||||
}
|
||||
|
||||
timing.total_us = total_start.elapsed().as_micros() as u64;
|
||||
|
||||
ObserveResult {
|
||||
pose: odometry,
|
||||
confidence: 0.0,
|
||||
@@ -315,14 +335,21 @@ impl VectorMap {
|
||||
icp_iterations: 0,
|
||||
icp_converged: false,
|
||||
loop_closure_detected: false,
|
||||
timing,
|
||||
}
|
||||
}
|
||||
|
||||
/// Extract features from a scan.
|
||||
fn extract_features(&self, scan_robot_frame: &PointCloud2D, pose: &Pose2D) -> FeatureSet {
|
||||
/// Extract features from a scan with timing.
|
||||
fn extract_features_timed(
|
||||
&self,
|
||||
scan_robot_frame: &PointCloud2D,
|
||||
pose: &Pose2D,
|
||||
timing: &mut TimingBreakdown,
|
||||
) -> FeatureSet {
|
||||
let points: Vec<Point2D> = scan_robot_frame.iter().collect();
|
||||
|
||||
// Extract lines in robot frame
|
||||
// ─── Line Extraction ───
|
||||
let line_start = Instant::now();
|
||||
let lines_robot = if self.config.use_hybrid_extraction {
|
||||
// Hybrid: RANSAC for dominant lines, then split-merge for remaining
|
||||
extract_lines_hybrid(
|
||||
@@ -337,30 +364,40 @@ impl VectorMap {
|
||||
|
||||
// Transform lines to world frame
|
||||
let lines_world: Vec<Line2D> = lines_robot.iter().map(|l| l.transform(pose)).collect();
|
||||
timing.line_extraction_us = line_start.elapsed().as_micros() as u64;
|
||||
|
||||
// Extract corners from world-frame lines
|
||||
// ─── Corner Detection ───
|
||||
let corner_start = Instant::now();
|
||||
let corners = detect_corners(&lines_world, &self.config.corner);
|
||||
timing.corner_detection_us = corner_start.elapsed().as_micros() as u64;
|
||||
|
||||
FeatureSet::from_features(&lines_world, &corners)
|
||||
}
|
||||
|
||||
/// Integrate new features into the map.
|
||||
fn integrate_features(&mut self, features: &FeatureSet) -> (usize, usize) {
|
||||
/// Integrate new features into the map with timing.
|
||||
fn integrate_features_timed(
|
||||
&mut self,
|
||||
features: &FeatureSet,
|
||||
timing: &mut TimingBreakdown,
|
||||
) -> (usize, usize) {
|
||||
let scan_lines: Vec<Line2D> = features.lines().to_vec();
|
||||
|
||||
if scan_lines.is_empty() {
|
||||
return (0, 0);
|
||||
}
|
||||
|
||||
// Find associations
|
||||
// ─── Association ───
|
||||
let assoc_start = Instant::now();
|
||||
let associations = find_associations(
|
||||
&scan_lines,
|
||||
self.line_store.lines(),
|
||||
Some(self.line_store.index()),
|
||||
&self.config.association,
|
||||
);
|
||||
timing.association_us = assoc_start.elapsed().as_micros() as u64;
|
||||
|
||||
// Merge matched lines
|
||||
// ─── Merging ───
|
||||
let merge_start = Instant::now();
|
||||
let merged = batch_merge(
|
||||
&scan_lines,
|
||||
self.line_store.lines_mut(),
|
||||
@@ -368,8 +405,10 @@ impl VectorMap {
|
||||
&self.config.merger,
|
||||
);
|
||||
let features_merged = merged.len();
|
||||
timing.merging_us = merge_start.elapsed().as_micros() as u64;
|
||||
|
||||
// Find and add unmatched lines
|
||||
// ─── New Features ───
|
||||
let new_start = Instant::now();
|
||||
let unmatched = find_unmatched_scan_lines(
|
||||
&scan_lines,
|
||||
self.line_store.lines(),
|
||||
@@ -389,6 +428,7 @@ impl VectorMap {
|
||||
|
||||
// Update corners
|
||||
self.update_corners();
|
||||
timing.new_features_us = new_start.elapsed().as_micros() as u64;
|
||||
|
||||
(features_added, features_merged)
|
||||
}
|
||||
|
||||
@@ -0,0 +1,184 @@
|
||||
# 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.
|
||||
@@ -22,23 +22,6 @@ pub fn lidar_to_point_cloud(
|
||||
min_quality: u8,
|
||||
min_range: f32,
|
||||
max_range: f32,
|
||||
) -> PointCloud2D {
|
||||
// Use full 360° scan data - the split-merge algorithm handles this correctly
|
||||
// after the infinite recursion fix in split_recursive()
|
||||
let polar = PolarScan { points: lidar_data };
|
||||
polar.to_cartesian(min_quality, min_range, max_range)
|
||||
}
|
||||
|
||||
/// Convert lidar scan data without subsampling (for testing with known small datasets).
|
||||
///
|
||||
/// # Warning
|
||||
/// Only use this with small point clouds (<100 points) or datasets that
|
||||
/// don't form continuous loops.
|
||||
pub fn lidar_to_point_cloud_full(
|
||||
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)
|
||||
@@ -50,15 +33,14 @@ mod tests {
|
||||
use std::f32::consts::FRAC_PI_2;
|
||||
|
||||
#[test]
|
||||
fn test_lidar_to_point_cloud_full() {
|
||||
// Test the full (unfiltered) conversion
|
||||
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_full(lidar_data, 60, 0.1, 10.0);
|
||||
let cloud = lidar_to_point_cloud(lidar_data, 60, 0.1, 10.0);
|
||||
|
||||
// Third point filtered by quality
|
||||
assert_eq!(cloud.len(), 2);
|
||||
@@ -73,7 +55,7 @@ mod tests {
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_lidar_to_point_cloud_full_360() {
|
||||
fn test_lidar_to_point_cloud_360() {
|
||||
// Test that full 360 degree scan is preserved
|
||||
let lidar_data: Vec<_> = (0..360)
|
||||
.map(|i| {
|
||||
|
||||
@@ -13,7 +13,7 @@ use vastu_map::core::Pose2D;
|
||||
use vastu_map::{Map, VectorMap, VectorMapConfig};
|
||||
|
||||
use crate::adapters::lidar_to_point_cloud;
|
||||
use crate::metrics::{ConvergenceStats, TestMetrics};
|
||||
use crate::metrics::{ConvergenceStats, TestMetrics, TimingStats};
|
||||
use crate::path_generator::PathSegment;
|
||||
use crate::visualization::Visualizer;
|
||||
|
||||
@@ -200,6 +200,8 @@ pub struct TestHarness {
|
||||
convergence_stats: ConvergenceStats,
|
||||
/// Trajectory history for enhanced visualization
|
||||
trajectory: TrajectoryHistory,
|
||||
/// Algorithm timing statistics across all observations
|
||||
timing_stats: TimingStats,
|
||||
}
|
||||
|
||||
impl TestHarness {
|
||||
@@ -242,6 +244,7 @@ impl TestHarness {
|
||||
last_scan_robot_frame: None,
|
||||
convergence_stats: ConvergenceStats::new(),
|
||||
trajectory: TrajectoryHistory::new(),
|
||||
timing_stats: TimingStats::new(),
|
||||
})
|
||||
}
|
||||
|
||||
@@ -355,6 +358,9 @@ impl TestHarness {
|
||||
self.convergence_stats
|
||||
.record(result.icp_iterations, result.icp_converged);
|
||||
|
||||
// Record timing stats
|
||||
self.timing_stats.record(&result.timing);
|
||||
|
||||
// Record to trajectory history for visualization
|
||||
// Store scan at intervals to avoid excessive memory usage
|
||||
let scan_to_store = if self.observations % SCAN_STORAGE_INTERVAL == 0 {
|
||||
@@ -427,6 +433,9 @@ impl TestHarness {
|
||||
);
|
||||
}
|
||||
|
||||
// Print timing summary
|
||||
self.timing_stats.print_summary();
|
||||
|
||||
TestResult {
|
||||
metrics,
|
||||
final_pose,
|
||||
|
||||
@@ -3,8 +3,10 @@
|
||||
//! Computes error metrics comparing SLAM output against ground truth:
|
||||
//! - Position and orientation error
|
||||
//! - Wall accuracy (how close detected lines are to actual walls in map)
|
||||
//! - 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;
|
||||
@@ -50,6 +52,143 @@ impl ConvergenceStats {
|
||||
}
|
||||
}
|
||||
|
||||
/// Single stage timing accumulator.
|
||||
#[derive(Debug, Clone, Default)]
|
||||
struct StageTiming {
|
||||
total_us: u64,
|
||||
min_us: u64,
|
||||
max_us: u64,
|
||||
count: usize,
|
||||
}
|
||||
|
||||
impl StageTiming {
|
||||
fn new() -> Self {
|
||||
Self {
|
||||
total_us: 0,
|
||||
min_us: u64::MAX,
|
||||
max_us: 0,
|
||||
count: 0,
|
||||
}
|
||||
}
|
||||
|
||||
fn record(&mut self, us: u64) {
|
||||
self.total_us += us;
|
||||
self.min_us = self.min_us.min(us);
|
||||
self.max_us = self.max_us.max(us);
|
||||
self.count += 1;
|
||||
}
|
||||
|
||||
fn avg_us(&self) -> f64 {
|
||||
if self.count == 0 {
|
||||
0.0
|
||||
} else {
|
||||
self.total_us as f64 / self.count as f64
|
||||
}
|
||||
}
|
||||
|
||||
fn min_display(&self) -> u64 {
|
||||
if self.min_us == u64::MAX {
|
||||
0
|
||||
} else {
|
||||
self.min_us
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Accumulated timing statistics across all observations.
|
||||
#[derive(Debug, Clone, Default)]
|
||||
pub struct TimingStats {
|
||||
icp_matching: StageTiming,
|
||||
line_extraction: StageTiming,
|
||||
corner_detection: StageTiming,
|
||||
association: StageTiming,
|
||||
merging: StageTiming,
|
||||
new_features: StageTiming,
|
||||
loop_closure: StageTiming,
|
||||
total: StageTiming,
|
||||
}
|
||||
|
||||
impl TimingStats {
|
||||
/// Create a new empty timing stats tracker.
|
||||
pub fn new() -> Self {
|
||||
Self {
|
||||
icp_matching: StageTiming::new(),
|
||||
line_extraction: StageTiming::new(),
|
||||
corner_detection: StageTiming::new(),
|
||||
association: StageTiming::new(),
|
||||
merging: StageTiming::new(),
|
||||
new_features: StageTiming::new(),
|
||||
loop_closure: StageTiming::new(),
|
||||
total: StageTiming::new(),
|
||||
}
|
||||
}
|
||||
|
||||
/// Record timing from a single observation.
|
||||
pub fn record(&mut self, timing: &TimingBreakdown) {
|
||||
self.icp_matching.record(timing.icp_matching_us);
|
||||
self.line_extraction.record(timing.line_extraction_us);
|
||||
self.corner_detection.record(timing.corner_detection_us);
|
||||
self.association.record(timing.association_us);
|
||||
self.merging.record(timing.merging_us);
|
||||
self.new_features.record(timing.new_features_us);
|
||||
self.loop_closure.record(timing.loop_closure_us);
|
||||
self.total.record(timing.total_us);
|
||||
}
|
||||
|
||||
/// Get number of observations recorded.
|
||||
pub fn observation_count(&self) -> usize {
|
||||
self.total.count
|
||||
}
|
||||
|
||||
/// Print a formatted timing summary.
|
||||
pub fn print_summary(&self) {
|
||||
let count = self.observation_count();
|
||||
if count == 0 {
|
||||
println!("No timing data collected.");
|
||||
return;
|
||||
}
|
||||
|
||||
println!("\n=== Timing Summary ({} observations) ===", count);
|
||||
println!(
|
||||
"{:<24} | {:>10} | {:>10} | {:>10} | {:>10}",
|
||||
"Stage", "Total", "Avg", "Min", "Max"
|
||||
);
|
||||
println!("{}", "-".repeat(73));
|
||||
|
||||
self.print_stage("ICP Matching", &self.icp_matching);
|
||||
self.print_stage("Line Extraction", &self.line_extraction);
|
||||
self.print_stage("Corner Detection", &self.corner_detection);
|
||||
self.print_stage("Association", &self.association);
|
||||
self.print_stage("Merging", &self.merging);
|
||||
self.print_stage("New Features", &self.new_features);
|
||||
self.print_stage("Loop Closure", &self.loop_closure);
|
||||
|
||||
println!("{}", "-".repeat(73));
|
||||
self.print_stage("Total per Observe", &self.total);
|
||||
}
|
||||
|
||||
fn print_stage(&self, name: &str, stage: &StageTiming) {
|
||||
println!(
|
||||
"{:<24} | {:>10} | {:>10} | {:>10} | {:>10}",
|
||||
name,
|
||||
Self::format_us(stage.total_us),
|
||||
Self::format_us(stage.avg_us() as u64),
|
||||
Self::format_us(stage.min_display()),
|
||||
Self::format_us(stage.max_us),
|
||||
);
|
||||
}
|
||||
|
||||
fn format_us(us: u64) -> String {
|
||||
if us >= 1_000_000 {
|
||||
format!("{:.2} s", us as f64 / 1_000_000.0)
|
||||
} else if us >= 1_000 {
|
||||
format!("{:.2} ms", us as f64 / 1_000.0)
|
||||
} else {
|
||||
format!("{} µs", us)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Test metrics results.
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct TestMetrics {
|
||||
|
||||
@@ -19,7 +19,6 @@
|
||||
mod adapters;
|
||||
mod harness;
|
||||
mod metrics;
|
||||
mod odometry;
|
||||
mod path_generator;
|
||||
mod scenarios;
|
||||
mod visualization;
|
||||
@@ -28,5 +27,5 @@ mod yaml_runner;
|
||||
|
||||
// Re-export for test convenience
|
||||
pub use harness::{HarnessConfig, TestHarness, TestResult};
|
||||
pub use metrics::{AcceptanceCriteria, TestMetrics};
|
||||
pub use path_generator::{PathGenerator, PathSegment};
|
||||
pub use metrics::{AcceptanceCriteria, TestMetrics, TimingStats};
|
||||
pub use path_generator::PathSegment;
|
||||
|
||||
@@ -1,192 +0,0 @@
|
||||
//! Odometry simulation from encoder ticks
|
||||
//!
|
||||
//! Simulates odometry estimation using differential drive kinematics,
|
||||
//! with configurable drift and noise for testing SLAM robustness.
|
||||
|
||||
use vastu_map::core::Pose2D;
|
||||
|
||||
/// Odometry drift configuration.
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct OdometryConfig {
|
||||
/// Systematic drift per meter traveled (e.g., 0.02 = 2% drift)
|
||||
pub drift_per_meter: f32,
|
||||
/// Angular drift per radian rotated
|
||||
pub angular_drift_per_rad: f32,
|
||||
}
|
||||
|
||||
impl Default for OdometryConfig {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
drift_per_meter: 0.0,
|
||||
angular_drift_per_rad: 0.0,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl OdometryConfig {
|
||||
/// Create a clean odometry config with no drift.
|
||||
pub fn clean() -> Self {
|
||||
Self::default()
|
||||
}
|
||||
}
|
||||
|
||||
/// Odometry estimator that tracks robot pose from encoder ticks.
|
||||
///
|
||||
/// Uses differential drive kinematics to compute pose changes from
|
||||
/// left/right wheel encoder tick differences.
|
||||
pub struct OdometryEstimator {
|
||||
config: OdometryConfig,
|
||||
accumulated_pose: Pose2D,
|
||||
last_left_ticks: u16,
|
||||
last_right_ticks: u16,
|
||||
wheel_base: f32,
|
||||
ticks_per_meter: f32,
|
||||
initialized: bool,
|
||||
}
|
||||
|
||||
impl OdometryEstimator {
|
||||
/// Create a new odometry estimator.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `wheel_base` - Distance between wheels in meters
|
||||
/// * `ticks_per_meter` - Encoder ticks per meter of travel
|
||||
/// * `config` - Drift/noise configuration
|
||||
pub fn new(wheel_base: f32, ticks_per_meter: f32, config: OdometryConfig) -> Self {
|
||||
Self {
|
||||
config,
|
||||
accumulated_pose: Pose2D::identity(),
|
||||
last_left_ticks: 0,
|
||||
last_right_ticks: 0,
|
||||
wheel_base,
|
||||
ticks_per_meter,
|
||||
initialized: false,
|
||||
}
|
||||
}
|
||||
|
||||
/// Initialize with starting pose and encoder values.
|
||||
pub fn initialize(&mut self, start_pose: Pose2D, left_ticks: u16, right_ticks: u16) {
|
||||
self.accumulated_pose = start_pose;
|
||||
self.last_left_ticks = left_ticks;
|
||||
self.last_right_ticks = right_ticks;
|
||||
self.initialized = true;
|
||||
}
|
||||
|
||||
/// Update with new encoder ticks, returns the accumulated pose.
|
||||
///
|
||||
/// Handles u16 wrapping correctly.
|
||||
pub fn update(&mut self, left_ticks: u16, right_ticks: u16) -> Pose2D {
|
||||
if !self.initialized {
|
||||
self.last_left_ticks = left_ticks;
|
||||
self.last_right_ticks = right_ticks;
|
||||
self.initialized = true;
|
||||
return self.accumulated_pose;
|
||||
}
|
||||
|
||||
// Handle u16 wrapping
|
||||
let delta_left = left_ticks.wrapping_sub(self.last_left_ticks) as i16;
|
||||
let delta_right = right_ticks.wrapping_sub(self.last_right_ticks) as i16;
|
||||
|
||||
self.last_left_ticks = left_ticks;
|
||||
self.last_right_ticks = right_ticks;
|
||||
|
||||
// Convert ticks to meters
|
||||
let left_dist = delta_left as f32 / self.ticks_per_meter;
|
||||
let right_dist = delta_right as f32 / self.ticks_per_meter;
|
||||
|
||||
// Apply drift
|
||||
let left_dist = left_dist * (1.0 + self.config.drift_per_meter);
|
||||
let right_dist = right_dist * (1.0 + self.config.drift_per_meter);
|
||||
|
||||
// Differential drive kinematics
|
||||
let linear = (left_dist + right_dist) / 2.0;
|
||||
let angular = (right_dist - left_dist) / self.wheel_base;
|
||||
|
||||
// Apply angular drift
|
||||
let angular = angular * (1.0 + self.config.angular_drift_per_rad);
|
||||
|
||||
// Compute pose delta in current robot frame
|
||||
let (dx, dy, dtheta) = if angular.abs() < 1e-6 {
|
||||
// Straight line motion
|
||||
(linear, 0.0, 0.0)
|
||||
} else {
|
||||
// Arc motion
|
||||
let radius = linear / angular;
|
||||
(
|
||||
radius * angular.sin(),
|
||||
radius * (1.0 - angular.cos()),
|
||||
angular,
|
||||
)
|
||||
};
|
||||
|
||||
// Transform to world frame and accumulate
|
||||
let cos_theta = self.accumulated_pose.theta.cos();
|
||||
let sin_theta = self.accumulated_pose.theta.sin();
|
||||
|
||||
self.accumulated_pose.x += dx * cos_theta - dy * sin_theta;
|
||||
self.accumulated_pose.y += dx * sin_theta + dy * cos_theta;
|
||||
self.accumulated_pose.theta += dtheta;
|
||||
|
||||
// Normalize theta to [-PI, PI]
|
||||
self.accumulated_pose.theta = normalize_angle(self.accumulated_pose.theta);
|
||||
|
||||
self.accumulated_pose
|
||||
}
|
||||
}
|
||||
|
||||
/// Normalize angle to [-PI, PI] range.
|
||||
fn normalize_angle(angle: f32) -> f32 {
|
||||
let mut a = angle;
|
||||
while a > std::f32::consts::PI {
|
||||
a -= 2.0 * std::f32::consts::PI;
|
||||
}
|
||||
while a < -std::f32::consts::PI {
|
||||
a += 2.0 * std::f32::consts::PI;
|
||||
}
|
||||
a
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use std::f32::consts::PI;
|
||||
|
||||
#[test]
|
||||
fn test_straight_motion() {
|
||||
let mut odom = OdometryEstimator::new(0.233, 4464.0, OdometryConfig::clean());
|
||||
odom.initialize(Pose2D::identity(), 0, 0);
|
||||
|
||||
// Move forward 1 meter (4464 ticks on each wheel)
|
||||
let pose = odom.update(4464, 4464);
|
||||
|
||||
assert!((pose.x - 1.0).abs() < 0.01);
|
||||
assert!(pose.y.abs() < 0.01);
|
||||
assert!(pose.theta.abs() < 0.01);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_rotation() {
|
||||
let mut odom = OdometryEstimator::new(0.233, 4464.0, OdometryConfig::clean());
|
||||
odom.initialize(Pose2D::identity(), 0, 0);
|
||||
|
||||
// Rotate in place (opposite wheel motion)
|
||||
// For 90 degree turn: arc_length = wheel_base/2 * PI/2
|
||||
let arc_ticks = (0.233 / 2.0 * PI / 2.0 * 4464.0) as u16;
|
||||
let pose = odom.update(65535 - arc_ticks + 1, arc_ticks); // Left backward, right forward
|
||||
|
||||
// Should have rotated approximately PI/2
|
||||
assert!((pose.theta - PI / 2.0).abs() < 0.1);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_wrapping() {
|
||||
let mut odom = OdometryEstimator::new(0.233, 4464.0, OdometryConfig::clean());
|
||||
odom.initialize(Pose2D::identity(), 65530, 65530);
|
||||
|
||||
// Wrap around
|
||||
let pose = odom.update(10, 10);
|
||||
|
||||
// Should have moved forward ~16 ticks worth
|
||||
let expected_dist = 16.0 / 4464.0;
|
||||
assert!((pose.x - expected_dist).abs() < 0.001);
|
||||
}
|
||||
}
|
||||
@@ -1,12 +1,7 @@
|
||||
//! Robot path generation utilities
|
||||
//! Robot path segment for integration tests
|
||||
//!
|
||||
//! Provides predefined path patterns for integration tests:
|
||||
//! - Rectangle (wall following)
|
||||
//! - Spiral outward (coverage)
|
||||
//! - Random walk (exploration)
|
||||
//! - Loop closure (return to start)
|
||||
//! Provides PathSegment for defining robot motion commands in YAML scenarios.
|
||||
|
||||
use rand::prelude::*;
|
||||
use std::f32::consts::PI;
|
||||
|
||||
/// A path segment with velocity commands and duration.
|
||||
@@ -67,170 +62,34 @@ impl PathSegment {
|
||||
}
|
||||
}
|
||||
|
||||
/// Path generator utilities.
|
||||
pub struct PathGenerator;
|
||||
|
||||
impl PathGenerator {
|
||||
/// Generate a spiral outward coverage pattern.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `initial_radius` - Starting radius in meters
|
||||
/// * `expansion_rate` - Radius increase per loop in meters
|
||||
/// * `num_loops` - Number of complete loops
|
||||
/// * `linear_speed` - Linear velocity in m/s
|
||||
pub fn spiral_outward(
|
||||
initial_radius: f32,
|
||||
expansion_rate: f32,
|
||||
num_loops: usize,
|
||||
linear_speed: f32,
|
||||
) -> Vec<PathSegment> {
|
||||
let mut segments = Vec::new();
|
||||
let mut radius = initial_radius;
|
||||
|
||||
for _ in 0..num_loops {
|
||||
// Arc for one loop
|
||||
let circumference = 2.0 * PI * radius;
|
||||
let duration = circumference / linear_speed;
|
||||
let angular_vel = linear_speed / radius;
|
||||
|
||||
segments.push(PathSegment::arc(linear_speed, angular_vel, duration));
|
||||
|
||||
// Expand radius
|
||||
radius += expansion_rate;
|
||||
}
|
||||
|
||||
segments
|
||||
}
|
||||
|
||||
/// Generate a rectangle pattern (wall following).
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `width` - Rectangle width in meters
|
||||
/// * `height` - Rectangle height in meters
|
||||
/// * `linear_speed` - Linear velocity in m/s
|
||||
/// * `turn_speed` - Angular velocity during turns in rad/s
|
||||
pub fn rectangle(
|
||||
width: f32,
|
||||
height: f32,
|
||||
linear_speed: f32,
|
||||
turn_speed: f32,
|
||||
) -> Vec<PathSegment> {
|
||||
let mut segments = Vec::new();
|
||||
|
||||
for i in 0..4 {
|
||||
let length = if i % 2 == 0 { width } else { height };
|
||||
|
||||
// Forward segment
|
||||
segments.push(PathSegment::forward(linear_speed, length / linear_speed));
|
||||
|
||||
// Turn 90 degrees left
|
||||
segments.push(PathSegment::turn_left(turn_speed, PI / 2.0));
|
||||
}
|
||||
|
||||
segments
|
||||
}
|
||||
|
||||
/// Generate a random exploration pattern.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `num_segments` - Number of forward-turn pairs
|
||||
/// * `linear_speed` - Linear velocity in m/s
|
||||
/// * `segment_length` - Base forward distance in meters
|
||||
/// * `turn_speed` - Angular velocity during turns in rad/s
|
||||
/// * `seed` - Random seed for reproducibility
|
||||
pub fn random_walk(
|
||||
num_segments: usize,
|
||||
linear_speed: f32,
|
||||
segment_length: f32,
|
||||
turn_speed: f32,
|
||||
seed: u64,
|
||||
) -> Vec<PathSegment> {
|
||||
let mut rng = StdRng::seed_from_u64(seed);
|
||||
let mut segments = Vec::new();
|
||||
|
||||
for _ in 0..num_segments {
|
||||
// Random forward distance (0.5x to 1.5x base length)
|
||||
let distance = segment_length * (0.5 + rng.random::<f32>());
|
||||
segments.push(PathSegment::forward(linear_speed, distance / linear_speed));
|
||||
|
||||
// Random turn (-90 to +90 degrees)
|
||||
let angle = (rng.random::<f32>() - 0.5) * PI;
|
||||
if angle > 0.0 {
|
||||
segments.push(PathSegment::turn_left(turn_speed, angle));
|
||||
} else {
|
||||
segments.push(PathSegment::turn_right(turn_speed, -angle));
|
||||
}
|
||||
}
|
||||
|
||||
segments
|
||||
}
|
||||
|
||||
/// Generate a loop closure path (exploration + return to start).
|
||||
///
|
||||
/// Reverses the given exploration path to return to the starting position.
|
||||
pub fn loop_closure(exploration_path: Vec<PathSegment>) -> Vec<PathSegment> {
|
||||
let mut full_path = exploration_path.clone();
|
||||
|
||||
// Reverse the path to return to start
|
||||
for segment in exploration_path.into_iter().rev() {
|
||||
full_path.push(PathSegment {
|
||||
linear_vel: -segment.linear_vel,
|
||||
angular_vel: -segment.angular_vel,
|
||||
duration: segment.duration,
|
||||
});
|
||||
}
|
||||
|
||||
full_path
|
||||
}
|
||||
|
||||
/// Generate a full rotation path for observing surrounding walls.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `turn_speed` - Angular velocity in rad/s
|
||||
/// * `pause_duration` - Pause at start and end for observations
|
||||
pub fn full_rotation(turn_speed: f32, pause_duration: f32) -> Vec<PathSegment> {
|
||||
vec![
|
||||
PathSegment::stop(pause_duration),
|
||||
PathSegment::turn_left(turn_speed, 2.0 * PI),
|
||||
PathSegment::stop(pause_duration),
|
||||
]
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn test_rectangle_path() {
|
||||
let path = PathGenerator::rectangle(2.0, 1.0, 0.5, 1.0);
|
||||
assert_eq!(path.len(), 8); // 4 forward + 4 turn segments
|
||||
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_spiral_path() {
|
||||
let path = PathGenerator::spiral_outward(0.5, 0.2, 3, 0.2);
|
||||
assert_eq!(path.len(), 3); // 3 loops
|
||||
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_random_walk_deterministic() {
|
||||
let path1 = PathGenerator::random_walk(5, 0.2, 0.5, 0.5, 42);
|
||||
let path2 = PathGenerator::random_walk(5, 0.2, 0.5, 0.5, 42);
|
||||
assert_eq!(path1.len(), path2.len());
|
||||
// Same seed should produce identical paths
|
||||
for (s1, s2) in path1.iter().zip(path2.iter()) {
|
||||
assert_eq!(s1.duration, s2.duration);
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_loop_closure() {
|
||||
let exploration = vec![
|
||||
PathSegment::forward(0.2, 1.0),
|
||||
PathSegment::turn_left(0.5, PI / 2.0),
|
||||
];
|
||||
let full = PathGenerator::loop_closure(exploration.clone());
|
||||
assert_eq!(full.len(), exploration.len() * 2);
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user