diff --git a/dhruva/proposal.md b/dhruva/proposal.md new file mode 100644 index 0000000..ca419ee --- /dev/null +++ b/dhruva/proposal.md @@ -0,0 +1,455 @@ +# Dhruva: High-Level Application Layer Design + +## Design Decisions + +| Decision | Choice | Rationale | +|----------|--------|-----------| +| **Language** | Rust | Runs on robot ARM, consistent with SangamIO/VastuSLAM | +| **Initial Mode** | Auto Clean only | Focus on core systematic coverage | +| **Room Detection** | Automatic | Morphological operations on free space | +| **Docking** | Hybrid | Lidar for approach, IR for final precision | + +## Architecture + +``` +┌─────────────────────────────────────────────────────────────────┐ +│ Drishti (UI) │ +└─────────────────────────────┬───────────────────────────────────┘ + │ Status/Commands +┌─────────────────────────────▼───────────────────────────────────┐ +│ DHRUVA (on robot) │ +│ ┌─────────────────────────────────────────────────────────┐ │ +│ │ Mission Controller │ │ +│ │ • State machine (Idle/Clean/Dock/Charge/Error) │ │ +│ └─────────────────────────────────────────────────────────┘ │ +│ ┌─────────────────────────────────────────────────────────┐ │ +│ │ Coverage Planner │ │ +│ │ • Boustrophedon, wall-follow, room sequencing │ │ +│ └─────────────────────────────────────────────────────────┘ │ +│ ┌─────────────────────────────────────────────────────────┐ │ +│ │ Navigation Layer │ │ +│ │ • A* path planning, DWA avoidance, recovery behaviors │ │ +│ └─────────────────────────────────────────────────────────┘ │ +│ ┌─────────────────────────────────────────────────────────┐ │ +│ │ Sensor Fusion │ │ +│ │ • EKF (encoders 110Hz + lidar 5Hz + IMU), VastuSLAM │ │ +│ └─────────────────────────────────────────────────────────┘ │ +└─────────────────────────────┬───────────────────────────────────┘ + │ UDP/TCP (port 5555) +┌─────────────────────────────▼───────────────────────────────────┐ +│ SangamIO Daemon │ +└─────────────────────────────────────────────────────────────────┘ +``` + +--- + +## High-Level Functionalities + +### 1. State Estimation & Sensor Fusion + +**Purpose**: Create accurate, high-rate pose estimates from multiple sensors + +| Feature | Description | Priority | +|---------|-------------|----------| +| **Odometry Fusion** | Combine wheel encoders (110Hz) with lidar scan matching (5Hz) | Critical | +| **IMU Integration** | Use gyro for angular velocity, detect wheel slip | High | +| **EKF/UKF Filter** | Probabilistic state estimation with sensor noise models | High | +| **Timestamp Alignment** | Buffer and synchronize sensors with different rates | Critical | +| **Localization Watchdog** | Detect and recover from localization failures | High | + +**Key Challenges**: +- 22:1 rate difference between odometry and lidar +- Wheel slip on carpet transitions +- Initial pose uncertainty after dock departure + +--- + +### 2. Navigation Layer + +**Purpose**: Safe, efficient movement from A to B + +| Feature | Description | Priority | +|---------|-------------|----------| +| **Global Path Planning** | A*/D* Lite on occupancy grid for room-scale paths | Critical | +| **Local Obstacle Avoidance** | VFH+ or Dynamic Window Approach for reactive control | Critical | +| **Trajectory Following** | Pure pursuit or MPC for smooth path execution | High | +| **Cliff Response** | Immediate stop + backup when cliff detected | Critical | +| **Bumper Response** | Stop, backup, turn away from obstacle | Critical | +| **Narrow Passage Detection** | Identify tight spaces robot may not fit through | Medium | + +**Suggested Controllers**: +``` +┌────────────────────────────────────────────────────────┐ +│ Navigation Stack │ +│ │ +│ Global Planner ──────► Path ──────► Local Planner │ +│ (A* on grid) │ (DWA/VFH+) │ +│ │ │ │ +│ │ ▼ │ +│ Recovery Manager ◄─────┴───────► Velocity Commands │ +│ (stuck/cliff/bump) │ │ +│ ▼ │ +│ SangamIO Drive API │ +└────────────────────────────────────────────────────────┘ +``` + +--- + +### 3. Coverage Planning + +**Purpose**: Systematic cleaning that covers entire floor area + +| Feature | Description | Priority | +|---------|-------------|----------| +| **Boustrophedon (Lawn Mower)** | Parallel lines with U-turns at boundaries | Critical | +| **Room-Based Coverage** | Clean one room at a time, then move to next | High | +| **Spiral Pattern** | Outward spiral for spot cleaning mode | High | +| **Edge/Wall Following** | Hug walls and furniture edges | High | +| **Coverage Tracking** | Mark cells as swept, identify gaps | Critical | +| **Gap Filling** | Return to missed areas after main pass | Medium | + +**Coverage Strategy**: +``` +1. Exploration pass (build initial map) +2. Room segmentation (identify distinct rooms) +3. For each room: + a. Edge pass (wall following) + b. Interior pass (boustrophedon) + c. Gap filling (revisit missed spots) +4. Return to dock +``` + +--- + +### 4. Room & Zone Management + +**Purpose**: Semantic understanding of the environment + +| Feature | Description | Priority | +|---------|-------------|----------| +| **Room Segmentation** | Detect room boundaries from map topology | High | +| **Room Naming/Labeling** | User-defined room names (Kitchen, Bedroom) | Medium | +| **Virtual Walls** | Software barriers that robot won't cross | High | +| **No-Go Zones** | Rectangular regions to avoid (pet bowls, cables) | High | +| **Priority Zones** | Areas to clean first or more frequently | Medium | +| **Multi-Floor Maps** | Store separate maps per floor | Low | + +**Implementation Notes**: +- Room segmentation using morphological operations on free space +- Virtual walls stored as line segments in map metadata +- Zone data persisted with map (.vastu format) + +--- + +### 5. Cleaning Modes + +**Purpose**: Different behaviors for different use cases + +| Mode | Description | Actuator Settings | +|------|-------------|-------------------| +| **Auto Clean** | Full house coverage, systematic | Vacuum 80%, Brushes 100% | +| **Quick Clean** | Fast pass, main areas only | Vacuum 60%, Brushes 80% | +| **Deep Clean** | Two passes, slower speed | Vacuum 100%, Brushes 100% | +| **Spot Clean** | 1m² spiral at current location | Vacuum 100%, Brushes 100% | +| **Edge Clean** | Wall-following perimeter pass | Vacuum 80%, Side brush 100% | +| **Room Clean** | Clean selected room(s) only | User configurable | +| **Quiet Mode** | Reduced suction for noise-sensitive times | Vacuum 40%, Brushes 60% | +| **Mop Mode** | Water pump enabled, slower movement | Pump ON, Speed 50% | + +--- + +### 6. Docking & Charging + +**Purpose**: Autonomous charging management + +| Feature | Description | Priority | +|---------|-------------|----------| +| **Dock Detection** | Recognize dock signature in lidar scans | Critical | +| **Dock Approach** | Navigate to dock vicinity | Critical | +| **Final Docking** | Precision approach using IR alignment | Critical | +| **Charging Monitor** | Track battery state, detect full charge | High | +| **Resume After Charge** | Continue cleaning from where it stopped | High | +| **Low Battery Preemption** | Return to dock before battery dies | Critical | + +**Dock Approach Sequence**: +``` +1. Navigate to saved dock location (global path) +2. Detect dock signature in lidar (characteristic shape) +3. Align perpendicular to dock front +4. Slow approach with IR homing +5. Contact detection → stop +6. Verify `is_charging` status +``` + +--- + +### 7. Error Handling & Recovery + +**Purpose**: Graceful handling of real-world problems + +| Error Type | Detection | Recovery Action | +|------------|-----------|-----------------| +| **Stuck (no motion)** | Encoder stall + IMU confirms no movement | Wiggle, backup, turn, retry | +| **Stuck (spinning)** | Encoders moving but pose unchanged | Stop, backup, new path | +| **Cliff Triggered** | Any cliff sensor TRUE | Emergency stop, backup 10cm, turn 90° | +| **Bumper Collision** | Bumper sensor TRUE | Stop, backup 5cm, turn 30° away | +| **Wheel Lifted** | Wheel drop sensors (if available) | Stop all motors, wait for ground | +| **Battery Critical** | battery_level < 10% | Abort cleaning, emergency dock | +| **Localization Lost** | Scan match score too low | Stop, attempt relocalization | +| **Path Blocked** | No valid path to goal | Try alternate routes, report to user | +| **Dustbox Full** | dustbox_attached FALSE | Pause, notify user | + +**Recovery Escalation**: +``` +Level 1: Simple retry (same action) +Level 2: Local recovery (backup, turn) +Level 3: Replan path (find alternate route) +Level 4: Relocalization (scan-to-map matching) +Level 5: Return to dock (abort mission) +Level 6: Stop and wait (critical error) +``` + +--- + +### 8. Mission Orchestration + +**Purpose**: High-level task management + +| Feature | Description | Priority | +|---------|-------------|----------| +| **Mission State Machine** | Idle → Cleaning → Docking → Charging cycle | Critical | +| **Cleaning Schedules** | Time-based automatic cleaning | Medium | +| **Multi-Room Sequencing** | Optimal order for room cleaning | Medium | +| **Resume After Charge** | Remember progress, continue where stopped | High | +| **Pause/Resume** | User-triggered pause with state preservation | High | +| **Mission History** | Log of past cleaning sessions | Low | + +**State Machine**: +``` + ┌───────────────────────────────────────────────┐ + │ │ + ▼ │ + ┌─────────┐ Start ┌───────────┐ │ + │ IDLE │ ───────────────►│ CLEANING │ │ + └────┬────┘ └─────┬─────┘ │ + │ │ │ + │ Done/Low │Battery │ + │ ▼ │ + │ ┌───────────┐ │ + │ │ DOCKING │ │ + │ └─────┬─────┘ │ + │ │ │ + │ Docked │ │ + │ ▼ │ + │ ┌───────────┐ Charged │ + │ │ CHARGING │────────────┘ + │ └─────┬─────┘ (if resume) + │ │ + │ Full │Charge + │◄───────────────────────────┘ + │ + ┌────▼────┐ + │ ERROR │◄── (any state on critical error) + └─────────┘ +``` + +--- + +### 9. Communication & External Interface + +**Purpose**: Integration with user interfaces and external systems + +| Feature | Description | Priority | +|---------|-------------|----------| +| **Status Broadcasting** | Current state, battery, area cleaned | High | +| **Map Streaming** | Live occupancy grid updates for visualization | High | +| **Remote Commands** | Start, Stop, Pause, Dock, Go-To-Point | Critical | +| **Cleaning Reports** | Summary after each session | Medium | +| **Error Notifications** | Push alerts for stuck, dustbin full, etc. | High | +| **OTA Updates** | Firmware update mechanism | Medium | + +--- + +## Implementation Plan + +### Phase 1: Foundation (MVP) + +**Goal**: Basic autonomous cleaning loop + +| Step | Module | Description | Files | +|------|--------|-------------|-------| +| 1.1 | `interface/client.rs` | SangamIO UDP/TCP client | Connect, send commands, receive sensors | +| 1.2 | `fusion/sync.rs` | Sensor timestamp alignment | Buffer 110Hz encoders, align with 5Hz lidar | +| 1.3 | `fusion/odometry.rs` | Differential drive odometry | Convert encoder ticks to dx, dy, dtheta | +| 1.4 | `fusion/ekf.rs` | Extended Kalman Filter | Fuse odometry + lidar scan matching | +| 1.5 | `navigation/recovery.rs` | Cliff/bumper response | Emergency stop, backup, turn | +| 1.6 | `navigation/planner.rs` | A* path planner | Grid-based global planning | +| 1.7 | `navigation/local_planner.rs` | DWA obstacle avoidance | Reactive local control | +| 1.8 | `mission/state_machine.rs` | Mission FSM | Idle → Cleaning → Docking → Charging | + +### Phase 2: Coverage & Docking + +**Goal**: Systematic cleaning + autonomous charging + +| Step | Module | Description | +|------|--------|-------------| +| 2.1 | `coverage/boustrophedon.rs` | Lawn mower pattern generator | +| 2.2 | `coverage/wall_follow.rs` | Edge cleaning controller | +| 2.3 | `coverage/tracker.rs` | Coverage tracking (swept cells) | +| 2.4 | `docking/detector.rs` | Lidar dock signature detection | +| 2.5 | `docking/approach.rs` | Hybrid approach controller (lidar + IR) | +| 2.6 | `docking/charging.rs` | Battery management, resume logic | + +### Phase 3: Room Intelligence + +**Goal**: Semantic understanding of environment + +| Step | Module | Description | +|------|--------|-------------| +| 3.1 | `zones/segmentation.rs` | Automatic room detection | +| 3.2 | `zones/virtual_walls.rs` | No-go barriers | +| 3.3 | `coverage/room_sequencer.rs` | Optimal room order | + +### Phase 4: Polish + +**Goal**: Production readiness + +| Step | Module | Description | +|------|--------|-------------| +| 4.1 | `interface/api.rs` | Status API for Drishti | +| 4.2 | `mission/history.rs` | Mission logging | +| 4.3 | `modes/` | Additional cleaning modes | + +--- + +## Module Structure + +``` +dhruva/ +├── Cargo.toml +├── dhruva.toml # Configuration +├── proto/ +│ └── dhruva.proto # Extended protobuf (status, maps) +└── src/ + ├── lib.rs + ├── main.rs # Daemon entry + │ + ├── core/ + │ ├── mod.rs + │ ├── config.rs # TOML config loading + │ ├── state.rs # RobotState, MissionState + │ └── events.rs # Event channel types + │ + ├── fusion/ + │ ├── mod.rs + │ ├── sync.rs # Timestamp alignment + │ ├── odometry.rs # Wheel encoder → pose delta + │ ├── imu.rs # Gyro/accel integration + │ └── ekf.rs # Extended Kalman Filter + │ + ├── navigation/ + │ ├── mod.rs + │ ├── planner.rs # A* global planner + │ ├── local_planner.rs # DWA reactive control + │ ├── controller.rs # Velocity output + │ └── recovery.rs # Cliff/bumper/stuck recovery + │ + ├── coverage/ + │ ├── mod.rs + │ ├── boustrophedon.rs # Parallel line coverage + │ ├── wall_follow.rs # Edge cleaning + │ ├── tracker.rs # Coverage % tracking + │ └── room_sequencer.rs # Multi-room order + │ + ├── zones/ + │ ├── mod.rs + │ ├── segmentation.rs # Morphological room detection + │ └── virtual_walls.rs # No-go zones + │ + ├── docking/ + │ ├── mod.rs + │ ├── detector.rs # Lidar signature matching + │ ├── approach.rs # Hybrid lidar + IR homing + │ └── charging.rs # Battery management + │ + ├── mission/ + │ ├── mod.rs + │ ├── state_machine.rs # Mission FSM + │ └── history.rs # Session logging + │ + └── interface/ + ├── mod.rs + ├── client.rs # SangamIO UDP/TCP client + ├── api.rs # External status API + └── telemetry.rs # Map streaming +``` + +--- + +## Key Algorithms + +### Boustrophedon Coverage +``` +1. Decompose free space into cells using critical point method +2. Generate vertical sweeping lines within each cell +3. Connect lines with U-turns at boundaries +4. Track swept areas, mark remaining gaps +5. Fill gaps with targeted paths +``` + +### Hybrid Dock Approach +``` +1. Navigate to saved dock location (A* path) +2. Switch to lidar mode: detect dock signature (concave shape) +3. Align perpendicular to dock at 0.5m distance +4. Switch to IR mode: follow IR gradient for final 0.5m +5. Detect contact (bumper or charging status) +6. Verify is_charging = true +``` + +### Recovery Escalation +``` +Level 1: Retry (1-2 attempts) +Level 2: Backup 10cm + turn 30° +Level 3: Replan global path +Level 4: Relocalization scan +Level 5: Return to dock +Level 6: Stop + notify user +``` + +--- + +## Dependencies + +```toml +[dependencies] +vastu-slam = { path = "../vastu-slam" } # SLAM library +tokio = { version = "1", features = ["full"] } # Async runtime +prost = "0.12" # Protobuf +serde = { version = "1", features = ["derive"] } +toml = "0.8" # Config +nalgebra = "0.32" # Linear algebra (EKF) +crossbeam-channel = "0.5" # Lock-free channels +tracing = "0.1" # Logging + +[build-dependencies] +prost-build = "0.12" +``` + +--- + +## Critical Files Reference + +| Component | Path | Purpose | +|-----------|------|---------| +| VastuSLAM API | `vastu-slam/src/lib.rs` | Map, observe, scan matching | +| SangamIO Proto | `sangam-io/proto/sangamio.proto` | Sensor/command messages | +| Sensor Types | `sangam-io/src/core/types.rs` | SensorValue enum | +| GD32 Protocol | `sangam-io/src/devices/crl200s/gd32/` | Motor control reference | +| Map Config | `vastu-slam/src/config/` | Grid resolution, matching params | + +--- + +## Brainstorming Notes + + diff --git a/sangam-io/Cargo.toml b/sangam-io/Cargo.toml index 7f9a2c4..a36d03a 100644 --- a/sangam-io/Cargo.toml +++ b/sangam-io/Cargo.toml @@ -51,6 +51,3 @@ lto = true # Link-time optimization codegen-units = 1 # Single codegen unit for better optimization panic = "abort" # Abort on panic (smaller than unwinding) strip = true # Strip symbols from binary - -[profile.release.build-override] -opt-level = 3 diff --git a/vastu-slam/Cargo.toml b/vastu-slam/Cargo.toml index 3a853fb..7e7eca3 100644 --- a/vastu-slam/Cargo.toml +++ b/vastu-slam/Cargo.toml @@ -7,17 +7,28 @@ license = "MIT" # Note: This crate requires nightly Rust for portable_simd feature +[features] +default = [] +scenario = ["serde_yaml"] # Enable YAML scenario parsing for tests + [dependencies] log = "0.4" # Logging serde = { version = "1.0", features = ["derive"] } # Config serialization -serde_yaml = "0.9" # YAML config files -rayon = "1.10" # Data parallelism for scan matching +toml = { version = "0.8", default-features = false, features = ["parse"] } # TOML config files +rayon = "1.11" # Data parallelism for scan matching +serde_yaml = { version = "0.9", optional = true } # YAML scenario parsing (optional) [dev-dependencies] env_logger = "0.11" # Test logging approx = "0.5" # Approximate comparisons (tests only) image = { version = "0.25", default-features = false, features = ["png", "pnm"] } criterion = "0.5" # Benchmarking +serde_yaml = "0.9" # YAML for tests (always available in tests) +sangam-io = { path = "../sangam-io", features = ["mock"] } # Mock device for examples + +[[example]] +name = "scenario_slam" +required-features = ["scenario"] [[bench]] name = "scan_matching" @@ -34,3 +45,6 @@ harness = false [profile.release] opt-level = 3 lto = true +codegen-units = 1 # Single codegen unit for better optimization +panic = "abort" # Abort on panic (smaller than unwinding) +strip = true # Strip symbols from binary diff --git a/vastu-slam/configs/config.toml b/vastu-slam/configs/config.toml new file mode 100644 index 0000000..c71afa7 --- /dev/null +++ b/vastu-slam/configs/config.toml @@ -0,0 +1,165 @@ +# VastuSLAM Configuration +# Units: meters (m), radians (rad), cells (n) + +# ============================================================================ +# Grid Configuration +# ============================================================================ +[grid] +# Cell resolution in meters per cell +resolution = 0.025 # 2.5cm cells + +# Initial grid dimensions (cells) +initial_width = 800 # 20m at 2.5cm +initial_height = 800 # 20m at 2.5cm + +# Grid origin mode: "center" or "corner" +origin_mode = "center" + +# Custom origin offset (only used if origin_mode: corner) +origin_x = 0.0 +origin_y = 0.0 + +# Enable automatic grid expansion +auto_expand = true + +# ============================================================================ +# Sensor Configuration +# ============================================================================ +[sensor.lidar] +# Sensor offset from robot center (x forward, y left) +offset_x = -0.110 +offset_y = 0.0 + +# Range limits +min_range = 0.15 +max_range = 8.0 + +[sensor.robot] +radius = 0.17 # CRL-200S physical radius + +[sensor.cliff_sensors] +left_side = [0.12, 0.10] +left_front = [0.15, 0.05] +right_front = [0.15, -0.05] +right_side = [0.12, -0.10] + +[sensor.bumper] +# Bumper zones (angles from center in radians) +left_angle = 0.5 # ~30 degrees +right_angle = -0.5 + +# ============================================================================ +# SLAM Configuration +# ============================================================================ +[slam.log_odds] +# Log-odds occupancy model (Cartographer-style) +# Log-odds increment for hit (lidar endpoint) +# Cartographer default: ~20 (P=0.55) +l_hit = 20 + +# Log-odds decrement for miss (ray passes through) +# Asymmetric to make walls "stickier" than free space +l_miss = -4 + +# Cell classification thresholds +l_occupied_threshold = 50 +l_free_threshold = -50 + +# Clamping range (probability bounds) +l_min = -200 # P ~ 0.12 +l_max = 200 # P ~ 0.88 + +[slam.correlative] +# Correlative scan matcher +enabled = true + +# Search window +search_x = 0.3 # meters +search_y = 0.3 # meters +search_theta = 0.15 # radians (~8.6 degrees) + +# Search resolution +linear_resolution = 0.02 # 2cm +angular_resolution = 0.02 # ~1.1 degrees + +# Multi-resolution search (coarse-to-fine) +multi_resolution = true +coarse_factor = 2.0 + +# Gaussian scoring (uses distance field) +use_gaussian_scoring = true +gaussian_sigma = 0.10 # 10cm - matches 2x grid resolution + +# Gauss-Newton refinement for sub-pixel accuracy +gn_max_iterations = 10 +gn_convergence_threshold = 1e-5 +gn_damping = 1e-3 +gn_translation_weight = 500.0 # Trust encoder distance +gn_rotation_weight = 20.0 # Allow heading correction + +# Match quality threshold +min_score = 0.5 + +# Maximum points to use (0 = all) +max_points = 0 + +[slam.loop_closure] +# Loop closure detection +enabled = true + +# Keyframe creation thresholds +keyframe_distance = 0.5 # 50cm between keyframes +keyframe_rotation = 0.5 # ~28 degrees + +# LiDAR-IRIS descriptor matching +max_descriptor_distance = 100 # Out of 640 bits + +# Spatial constraints +min_loop_gap = 20 # Minimum pose index gap +max_loop_distance = 5.0 # Maximum spatial distance (m) + +# Verification via scan matching +min_verification_score = 0.6 +use_branch_bound = true # Use branch-and-bound instead of correlative + +# Verification search window +verification_search_window = 0.2 # 20cm +verification_angular_window = 0.2 # ~11 degrees + +[slam.pose_graph] +# Pose graph optimization +max_iterations = 20 +convergence_threshold = 1e-4 +damping = 1e-3 + +# Edge weights +odometry_weight = 100.0 # Strong trust in encoder odometry +loop_weight = 50.0 # Lower to handle noise + +# Huber loss for robust optimization (downweight outliers) +huber_scale = 0.1 # 10cm + +[slam.background_optimizer] +# Background optimizer +max_iterations = 50 +convergence_threshold = 1e-5 +huber_scale = 0.1 + +# Trigger optimization every N nodes (0 = only on loop closure) +trigger_every_n_nodes = 0 + +# Background thread (not recommended for ARM) +use_background_thread = false + +# ============================================================================ +# Map Persistence +# ============================================================================ +[persistence] +# Output format: "vastu" (binary), "pgm" (ROS), "both" +output_format = "both" + +# Output directory (relative to working directory) +output_dir = "./output" + +# Auto-save interval during exploration (seconds, 0=disabled) +auto_save_interval = 30 diff --git a/vastu-slam/configs/config.yaml b/vastu-slam/configs/config.yaml deleted file mode 100644 index 672057f..0000000 --- a/vastu-slam/configs/config.yaml +++ /dev/null @@ -1,170 +0,0 @@ -# VastuSLAM Configuration -# Units: meters (m), radians (rad), cells (n) - -# ============================================================================ -# Grid Configuration -# ============================================================================ -grid: - # Cell resolution in meters per cell - resolution: 0.025 # 2.5cm cells - - # Initial grid dimensions (cells) - initial_width: 800 # 20m at 2.5cm - initial_height: 800 # 20m at 2.5cm - - # Grid origin mode: "center" or "corner" - origin_mode: center - - # Custom origin offset (only used if origin_mode: corner) - origin_x: 0.0 - origin_y: 0.0 - - # Enable automatic grid expansion - auto_expand: true - -# ============================================================================ -# Sensor Configuration -# ============================================================================ -sensor: - # LiDAR configuration - lidar: - # Sensor offset from robot center (x forward, y left) - offset_x: -0.110 - offset_y: 0.0 - - # Range limits - min_range: 0.15 - max_range: 8.0 - - # Robot geometry - robot: - radius: 0.17 # CRL-200S physical radius - - # Cliff sensor positions (robot frame) - cliff_sensors: - left_side: [0.12, 0.10] - left_front: [0.15, 0.05] - right_front: [0.15, -0.05] - right_side: [0.12, -0.10] - - # Bumper zones (angles from center in radians) - bumper: - left_angle: 0.5 # ~30 degrees - right_angle: -0.5 - -# ============================================================================ -# SLAM Configuration -# ============================================================================ -slam: - # Log-odds occupancy model (Cartographer-style) - log_odds: - # Log-odds increment for hit (lidar endpoint) - # Cartographer default: ~20 (P=0.55) - l_hit: 20 - - # Log-odds decrement for miss (ray passes through) - # Asymmetric to make walls "stickier" than free space - l_miss: -4 - - # Cell classification thresholds - l_occupied_threshold: 50 - l_free_threshold: -50 - - # Clamping range (probability bounds) - l_min: -200 # P ~ 0.12 - l_max: 200 # P ~ 0.88 - - # Correlative scan matcher - correlative: - enabled: true - - # Search window - search_x: 0.3 # meters - search_y: 0.3 # meters - search_theta: 0.15 # radians (~8.6 degrees) - - # Search resolution - linear_resolution: 0.02 # 2cm - angular_resolution: 0.02 # ~1.1 degrees - - # Multi-resolution search (coarse-to-fine) - multi_resolution: true - coarse_factor: 2.0 - - # Gaussian scoring (uses distance field) - use_gaussian_scoring: true - gaussian_sigma: 0.10 # 10cm - matches 2x grid resolution - - # Gauss-Newton refinement for sub-pixel accuracy - gn_max_iterations: 10 - gn_convergence_threshold: 1e-5 - gn_damping: 1e-3 - gn_translation_weight: 500.0 # Trust encoder distance - gn_rotation_weight: 20.0 # Allow heading correction - - # Match quality threshold - min_score: 0.5 - - # Maximum points to use (0 = all) - max_points: 0 - - # Loop closure detection - loop_closure: - enabled: true - - # Keyframe creation thresholds - keyframe_distance: 0.5 # 50cm between keyframes - keyframe_rotation: 0.5 # ~28 degrees - - # LiDAR-IRIS descriptor matching - max_descriptor_distance: 100 # Out of 640 bits - - # Spatial constraints - min_loop_gap: 20 # Minimum pose index gap - max_loop_distance: 5.0 # Maximum spatial distance (m) - - # Verification via scan matching - min_verification_score: 0.6 - use_branch_bound: true # Use branch-and-bound instead of correlative - - # Verification search window - verification_search_window: 0.2 # 20cm - verification_angular_window: 0.2 # ~11 degrees - - # Pose graph optimization - pose_graph: - max_iterations: 20 - convergence_threshold: 1e-4 - damping: 1e-3 - - # Edge weights - odometry_weight: 100.0 # Strong trust in encoder odometry - loop_weight: 50.0 # Lower to handle noise - - # Huber loss for robust optimization (downweight outliers) - huber_scale: 0.1 # 10cm - - # Background optimizer - background_optimizer: - max_iterations: 50 - convergence_threshold: 1e-5 - huber_scale: 0.1 - - # Trigger optimization every N nodes (0 = only on loop closure) - trigger_every_n_nodes: 0 - - # Background thread (not recommended for ARM) - use_background_thread: false - -# ============================================================================ -# Map Persistence -# ============================================================================ -persistence: - # Output format: "vastu" (binary), "pgm" (ROS), "both" - output_format: both - - # Output directory (relative to working directory) - output_dir: ./output - - # Auto-save interval during exploration (seconds, 0=disabled) - auto_save_interval: 30 diff --git a/vastu-slam/examples/scenario_slam.rs b/vastu-slam/examples/scenario_slam.rs new file mode 100644 index 0000000..d1c5ca5 --- /dev/null +++ b/vastu-slam/examples/scenario_slam.rs @@ -0,0 +1,391 @@ +//! Scenario-based SLAM example using SangamIO mock device. +//! +//! This example demonstrates: +//! - Loading a scenario file (YAML) with movement commands +//! - Running the SangamIO mock device simulation in-process +//! - Building a map using VastuSLAM from simulated lidar scans +//! - Exporting the result as SVG visualization +//! +//! # Usage +//! +//! ```bash +//! cargo run --example scenario_slam -- scenarios/square_path.yaml +//! ``` +//! +//! The output SVG will be saved to the `output/` directory. + +use std::collections::HashMap; +use std::path::Path; +use std::sync::{Arc, Mutex}; +use std::time::{Duration, Instant}; + +use sangam_io::config::DeviceConfig; +use sangam_io::core::driver::DeviceDriver; +use sangam_io::core::types::{ + Command as SangamCommand, ComponentAction, SensorGroupData, SensorValue, +}; +use sangam_io::devices::mock::MockDriver; +use sangam_io::devices::mock::config::SimulationConfig; + +use vastu_slam::io::{Scenario, SvgConfig, SvgVisualizer, markers_by_distance}; +use vastu_slam::matching::CorrelativeMatcherConfig; +use vastu_slam::{LidarScan, MapConfig, OccupancyGridMap, Pose2D}; + +/// Convert SangamIO PointCloud2D to VastuSLAM LidarScan +fn to_lidar_scan(points: &[(f32, f32, u8)]) -> LidarScan { + let angles: Vec = points.iter().map(|(a, _, _)| *a).collect(); + let ranges: Vec = points.iter().map(|(_, d, _)| *d).collect(); + LidarScan::new(ranges, angles, 0.15, 8.0) +} + +/// Get lidar scan from sensor data if available +fn get_lidar_scan( + sensor_data: &HashMap>>, + last_seq: &mut u64, +) -> Option { + let lidar = sensor_data.get("lidar")?; + let data = lidar.lock().ok()?; + + // Only return if new data (sequence number changed) + if data.sequence_number == *last_seq { + return None; + } + *last_seq = data.sequence_number; + + if let Some(SensorValue::PointCloud2D(points)) = data.values.get("scan") { + if !points.is_empty() { + return Some(to_lidar_scan(points)); + } + } + None +} + +/// Compute odometry pose from wheel encoders +fn compute_odometry( + sensor_data: &HashMap>>, + prev_left: &mut u16, + prev_right: &mut u16, + pose: &mut Pose2D, + ticks_per_meter: f32, + wheel_base: f32, +) { + let sensor_status = match sensor_data.get("sensor_status") { + Some(s) => s, + None => return, + }; + + let data = match sensor_status.lock() { + Ok(d) => d, + Err(_) => return, + }; + + let left = match data.values.get("wheel_left") { + Some(SensorValue::U16(v)) => *v, + _ => return, + }; + let right = match data.values.get("wheel_right") { + Some(SensorValue::U16(v)) => *v, + _ => return, + }; + + // Calculate tick deltas (handle wraparound) + let dl = left.wrapping_sub(*prev_left) as i16; + let dr = right.wrapping_sub(*prev_right) as i16; + *prev_left = left; + *prev_right = right; + + // Skip if no movement + if dl == 0 && dr == 0 { + return; + } + + // Convert ticks to meters + let dist_left = dl as f32 / ticks_per_meter; + let dist_right = dr as f32 / ticks_per_meter; + + // Differential drive kinematics + let dist = (dist_left + dist_right) / 2.0; + let dtheta = (dist_right - dist_left) / wheel_base; + + // Update pose + pose.x += dist * pose.theta.cos(); + pose.y += dist * pose.theta.sin(); + pose.theta += dtheta; +} + +/// Create a command to enable lidar +fn lidar_enable() -> SangamCommand { + SangamCommand::ComponentControl { + id: "lidar".to_string(), + action: ComponentAction::Enable { config: None }, + } +} + +/// Create a command to enable drive +fn drive_enable() -> SangamCommand { + SangamCommand::ComponentControl { + id: "drive".to_string(), + action: ComponentAction::Enable { config: None }, + } +} + +/// Create a command to set velocity +fn set_velocity(linear: f32, angular: f32) -> SangamCommand { + let mut config = HashMap::new(); + config.insert("linear".to_string(), SensorValue::F32(linear)); + config.insert("angular".to_string(), SensorValue::F32(angular)); + SangamCommand::ComponentControl { + id: "drive".to_string(), + action: ComponentAction::Configure { config }, + } +} + +fn main() -> Result<(), Box> { + // Initialize logging + env_logger::Builder::from_env(env_logger::Env::default().default_filter_or("info")).init(); + + // Parse CLI args + let args: Vec = std::env::args().collect(); + if args.len() < 2 { + eprintln!("Usage: {} [--speed-factor N]", args[0]); + eprintln!( + "Example: {} scenarios/square_path.yaml --speed-factor 10", + args[0] + ); + std::process::exit(1); + } + + let scenario_path = Path::new(&args[1]); + let speed_factor = args + .iter() + .position(|a| a == "--speed-factor") + .and_then(|i| args.get(i + 1)) + .and_then(|s| s.parse::().ok()) + .unwrap_or(5.0); // Default 5x speed for faster simulation + + // Load scenario + log::info!("Loading scenario from {:?}", scenario_path); + let scenario = Scenario::load(scenario_path)?; + log::info!( + "Scenario: {} ({} commands, {:.1}s total)", + scenario.name, + scenario.commands.len(), + scenario.total_duration_secs() + ); + + // Resolve map path (scenario map name -> maps/NAME.yaml) + let maps_dir = scenario_path + .parent() + .unwrap_or(Path::new(".")) + .join("../maps"); + let map_file = maps_dir.join(format!("{}.yaml", scenario.map)); + log::info!("Using map: {:?}", map_file); + + // Get start pose from scenario or use defaults + let (start_x, start_y, start_theta) = scenario + .start_pose + .as_ref() + .map(|p| (p.x, p.y, p.theta)) + .unwrap_or((4.0, 4.0, 0.0)); + + // Create simulation config + let sim_config = SimulationConfig { + map_file: map_file.to_string_lossy().to_string(), + start_x, + start_y, + start_theta, + speed_factor, + random_seed: 42, // Deterministic for reproducibility + ..Default::default() + }; + + // Create device config + let device_config = DeviceConfig { + device_type: "mock".to_string(), + name: "Scenario SLAM Simulation".to_string(), + hardware: None, + simulation: Some(sim_config), + }; + + // Initialize mock driver + log::info!("Initializing mock driver (speed factor: {}x)", speed_factor); + let mut driver = MockDriver::new(device_config)?; + let init_result = driver.initialize()?; + let sensor_data = init_result.sensor_data; + + // Enable lidar and drive + driver.send_command(lidar_enable())?; + driver.send_command(drive_enable())?; + + // Wait for first lidar scan + log::info!("Waiting for first lidar scan..."); + std::thread::sleep(Duration::from_millis(500)); + + // Create SLAM map + let map_config = MapConfig::default(); + let mut map = OccupancyGridMap::new(map_config); + + // Configure matcher with correct lidar offset (must match SensorConfig) + let mut matcher_config = CorrelativeMatcherConfig::default(); + matcher_config.sensor_offset = (-0.110, 0.0); // Lidar 11cm behind robot center + + // Tracking state + let mut trajectory: Vec = vec![]; + let mut current_pose = Pose2D::new(start_x, start_y, start_theta); + let mut prev_wheel_left: u16 = 0; + let mut prev_wheel_right: u16 = 0; + let mut last_lidar_seq: u64 = 0; + + // Robot parameters (CRL-200S defaults) + let ticks_per_meter = 4464.0; + let wheel_base = 0.233; + + // Initialize wheel encoder baseline + if let Some(sensor_status) = sensor_data.get("sensor_status") { + if let Ok(data) = sensor_status.lock() { + if let Some(SensorValue::U16(l)) = data.values.get("wheel_left") { + prev_wheel_left = *l; + } + if let Some(SensorValue::U16(r)) = data.values.get("wheel_right") { + prev_wheel_right = *r; + } + } + } + + // Seed the map with initial scans (rotate to get 360° coverage) + log::info!("Seeding map with {} scans...", scenario.seed.scans); + if scenario.seed.rotation { + // Rotate slowly while collecting scans + driver.send_command(set_velocity(0.0, 0.3))?; + } + + let seed_start = Instant::now(); + let seed_duration_ms = (scenario.seed.scans as f32 * 200.0 / speed_factor) as u64; + + while seed_start.elapsed().as_millis() < seed_duration_ms as u128 { + // Update odometry + compute_odometry( + &sensor_data, + &mut prev_wheel_left, + &mut prev_wheel_right, + &mut current_pose, + ticks_per_meter, + wheel_base, + ); + + // During seeding, insert scans directly without matching + // (matching against empty/sparse map gives poor results) + if let Some(scan) = get_lidar_scan(&sensor_data, &mut last_lidar_seq) { + map.observe_lidar(&scan, current_pose); + trajectory.push(current_pose); + } + + std::thread::sleep(Duration::from_millis(20)); + } + + // Stop rotation + driver.send_command(set_velocity(0.0, 0.0))?; + std::thread::sleep(Duration::from_millis(100)); + + log::info!( + "Map seeded with {} scans, {} cells known", + trajectory.len(), + map.coverage_stats().known_cells + ); + + // Execute scenario commands + log::info!("Executing {} scenario commands...", scenario.commands.len()); + + for (i, cmd) in scenario.commands.iter().enumerate() { + let (linear, angular) = cmd.velocities(); + let duration_ms = cmd.duration_ms(); + let sim_duration_ms = (duration_ms as f32 / speed_factor) as u64; + + log::debug!( + "Command {}/{}: v={:.2} m/s, ω={:.2} rad/s, duration={}ms (sim: {}ms)", + i + 1, + scenario.commands.len(), + linear, + angular, + duration_ms, + sim_duration_ms + ); + + // Send velocity command + driver.send_command(set_velocity(linear, angular))?; + + let cmd_start = Instant::now(); + while cmd_start.elapsed().as_millis() < sim_duration_ms as u128 { + // Update odometry + compute_odometry( + &sensor_data, + &mut prev_wheel_left, + &mut prev_wheel_right, + &mut current_pose, + ticks_per_meter, + wheel_base, + ); + + // Process lidar scan with matching + if let Some(scan) = get_lidar_scan(&sensor_data, &mut last_lidar_seq) { + let (_result, corrected_pose, match_result) = + map.observe_lidar_with_matching(&scan, current_pose, &matcher_config); + + if match_result.converged && match_result.score >= 0.5 { + current_pose = corrected_pose; + log::trace!("Match accepted: score={:.3}", match_result.score); + } else { + log::debug!( + "Match rejected: score={:.3}, converged={}", + match_result.score, + match_result.converged + ); + } + trajectory.push(current_pose); + } + + std::thread::sleep(Duration::from_millis(20)); + } + } + + // Stop robot + driver.send_command(set_velocity(0.0, 0.0))?; + + // Report results + let stats = map.coverage_stats(); + log::info!( + "SLAM complete: {} poses, {:.2}m² explored, {} wall cells", + trajectory.len(), + stats.explored_area_m2, + stats.wall_cells + ); + + // Create output directory + let output_dir = Path::new("output"); + std::fs::create_dir_all(output_dir)?; + + // Export SVG + let svg_filename = scenario.svg_filename(); + let svg_path = output_dir.join(&svg_filename); + + let svg_config = SvgConfig { + scale: scenario.svg_output.scale, + trajectory_width: scenario.svg_output.trajectory_width, + ..Default::default() + }; + + let markers = markers_by_distance( + &trajectory, + scenario.svg_output.marker_interval_m.unwrap_or(0.5), + ); + + let visualizer = SvgVisualizer::new(map.storage().clone(), svg_config) + .with_title(&scenario.name) + .with_estimated(trajectory, markers); + + visualizer.save(&svg_path)?; + + log::info!("SVG saved to: {:?}", svg_path); + + Ok(()) +} diff --git a/vastu-slam/src/config/mod.rs b/vastu-slam/src/config/mod.rs index 04f15b6..024cf84 100644 --- a/vastu-slam/src/config/mod.rs +++ b/vastu-slam/src/config/mod.rs @@ -1,13 +1,13 @@ //! Unified configuration loading for VastuSLAM. //! -//! Loads all configuration from a single YAML file with sensible defaults. +//! Loads all configuration from a single TOML file with sensible defaults. //! //! ## Quick Start //! //! ```rust,ignore //! use vastu_slam::config::VastuConfig; //! -//! // Load from default path (configs/config.yaml) +//! // Load from default path (configs/config.toml) //! let config = VastuConfig::load_default()?; //! //! // Or use built-in defaults (no file needed) @@ -30,37 +30,36 @@ //! | [`SlamSection`] | Matcher, loop closure, pose graph, motion filtering | //! | [`PersistenceSection`] | Output format and directory | //! -//! ## Example YAML +//! ## Example TOML //! -//! ```yaml -//! grid: -//! resolution: 0.025 # 2.5cm cells -//! initial_width: 800 # 20m +//! ```toml +//! [grid] +//! resolution = 0.025 # 2.5cm cells +//! initial_width = 800 # 20m //! -//! sensor: -//! robot: -//! radius: 0.17 -//! lidar: -//! offset_x: -0.110 -//! max_range: 8.0 +//! [sensor.robot] +//! radius = 0.17 //! -//! slam: -//! correlative: -//! enabled: true -//! multi_resolution: true +//! [sensor.lidar] +//! offset_x = -0.110 +//! max_range = 8.0 //! -//! # Optional: IMU fusion for improved pose estimation -//! pose_extrapolator: -//! pose_queue_duration: 0.5 # seconds -//! odom_queue_duration: 0.5 # seconds -//! imu_gravity_time_constant: 10.0 # seconds -//! imu_rotation_weight: 0.3 # 0.0=odom only, 1.0=IMU only +//! [slam.correlative] +//! enabled = true +//! multi_resolution = true //! -//! # Optional: Scan insertion throttling -//! motion_filter: -//! max_time_seconds: 5.0 # insert at least every 5s -//! max_distance_meters: 0.2 # insert if moved 20cm -//! max_angle_radians: 0.035 # insert if rotated ~2° +//! # Optional: IMU fusion for improved pose estimation +//! [slam.pose_extrapolator] +//! pose_queue_duration = 0.5 # seconds +//! odom_queue_duration = 0.5 # seconds +//! imu_gravity_time_constant = 10.0 # seconds +//! imu_rotation_weight = 0.3 # 0.0=odom only, 1.0=IMU only +//! +//! # Optional: Scan insertion throttling +//! [slam.motion_filter] +//! max_time_seconds = 5.0 # insert at least every 5s +//! max_distance_meters = 0.2 # insert if moved 20cm +//! max_angle_radians = 0.035 # insert if rotated ~2deg //! ``` //! //! ## Motion Filtering diff --git a/vastu-slam/src/config/vastu.rs b/vastu-slam/src/config/vastu.rs index 6b1b1a6..3052071 100644 --- a/vastu-slam/src/config/vastu.rs +++ b/vastu-slam/src/config/vastu.rs @@ -17,7 +17,7 @@ use super::persistence::PersistenceSection; use super::sensor::SensorSection; use super::slam::SlamSection; -/// Full VastuSLAM configuration loaded from YAML +/// Full VastuSLAM configuration loaded from TOML #[derive(Clone, Debug, Serialize, Deserialize, Default)] pub struct VastuConfig { /// Grid settings @@ -38,16 +38,16 @@ pub struct VastuConfig { } impl VastuConfig { - /// Load configuration from a YAML file + /// Load configuration from a TOML file pub fn load(path: &Path) -> Result { let contents = std::fs::read_to_string(path).map_err(|e| ConfigLoadError::Io(e.to_string()))?; - Self::from_yaml(&contents) + Self::from_toml(&contents) } - /// Load from default config path (configs/config.yaml) + /// Load from default config path (configs/config.toml) pub fn load_default() -> Result { - let path = Path::new("configs/config.yaml"); + let path = Path::new("configs/config.toml"); if path.exists() { Self::load(path) } else { @@ -55,9 +55,9 @@ impl VastuConfig { } } - /// Parse from YAML string - pub fn from_yaml(yaml: &str) -> Result { - serde_yaml::from_str(yaml).map_err(|e| ConfigLoadError::Parse(e.to_string())) + /// Parse from TOML string + pub fn from_toml(toml_str: &str) -> Result { + toml::from_str(toml_str).map_err(|e| ConfigLoadError::Parse(e.to_string())) } /// Convert to MapConfig for OccupancyGridMap @@ -138,11 +138,17 @@ mod tests { } #[test] - fn test_yaml_roundtrip() { - let config = VastuConfig::default(); - let yaml = serde_yaml::to_string(&config).unwrap(); - let parsed: VastuConfig = serde_yaml::from_str(&yaml).unwrap(); - assert_eq!(parsed.grid.resolution, config.grid.resolution); + fn test_toml_parsing() { + let toml_str = r#" +[grid] +resolution = 0.05 + +[sensor.robot] +radius = 0.20 +"#; + let config = VastuConfig::from_toml(toml_str).unwrap(); + assert_eq!(config.grid.resolution, 0.05); + assert_eq!(config.sensor.robot.radius, 0.20); } #[test] @@ -162,19 +168,19 @@ mod tests { } #[test] - fn test_motion_filtering_from_yaml() { - let yaml = r#" -slam: - pose_extrapolator: - pose_queue_duration: 0.5 - imu_gravity_time_constant: 10.0 - imu_rotation_weight: 0.3 - motion_filter: - max_time_seconds: 5.0 - max_distance_meters: 0.2 - max_angle_radians: 0.035 + fn test_motion_filtering_from_toml() { + let toml_str = r#" +[slam.pose_extrapolator] +pose_queue_duration = 0.5 +imu_gravity_time_constant = 10.0 +imu_rotation_weight = 0.3 + +[slam.motion_filter] +max_time_seconds = 5.0 +max_distance_meters = 0.2 +max_angle_radians = 0.035 "#; - let config = VastuConfig::from_yaml(yaml).unwrap(); + let config = VastuConfig::from_toml(toml_str).unwrap(); assert!(config.has_motion_filtering()); let extrapolator = config.pose_extrapolator_config().unwrap(); @@ -186,17 +192,18 @@ slam: #[test] fn test_to_localizer_config() { - let yaml = r#" -slam: - correlative: - enabled: true - linear_search_window: 0.5 - pose_extrapolator: - imu_rotation_weight: 0.4 - motion_filter: - max_distance_meters: 0.15 + let toml_str = r#" +[slam.correlative] +enabled = true +linear_search_window = 0.5 + +[slam.pose_extrapolator] +imu_rotation_weight = 0.4 + +[slam.motion_filter] +max_distance_meters = 0.15 "#; - let config = VastuConfig::from_yaml(yaml).unwrap(); + let config = VastuConfig::from_toml(toml_str).unwrap(); let localizer_config = config.to_localizer_config(); assert!(localizer_config.matcher.enabled); diff --git a/vastu-slam/src/grid/config.rs b/vastu-slam/src/grid/config.rs index 3fb28bc..665f5a5 100644 --- a/vastu-slam/src/grid/config.rs +++ b/vastu-slam/src/grid/config.rs @@ -249,51 +249,6 @@ pub struct MapConfig { pub log_odds: LogOddsConfig, } -impl MapConfig { - /// Load configuration from a YAML file - pub fn from_yaml_file(path: &std::path::Path) -> Result { - let contents = - std::fs::read_to_string(path).map_err(|e| ConfigError::IoError(e.to_string()))?; - Self::from_yaml(&contents) - } - - /// Load configuration from a YAML string - pub fn from_yaml(yaml: &str) -> Result { - serde_yaml::from_str(yaml).map_err(|e| ConfigError::ParseError(e.to_string())) - } - - /// Save configuration to a YAML file - pub fn to_yaml_file(&self, path: &std::path::Path) -> Result<(), ConfigError> { - let yaml = self.to_yaml()?; - std::fs::write(path, yaml).map_err(|e| ConfigError::IoError(e.to_string())) - } - - /// Serialize to YAML string - pub fn to_yaml(&self) -> Result { - serde_yaml::to_string(self).map_err(|e| ConfigError::ParseError(e.to_string())) - } -} - -/// Configuration error type -#[derive(Debug, Clone)] -pub enum ConfigError { - /// File I/O error - IoError(String), - /// YAML parsing error - ParseError(String), -} - -impl std::fmt::Display for ConfigError { - fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { - match self { - ConfigError::IoError(msg) => write!(f, "IO error: {}", msg), - ConfigError::ParseError(msg) => write!(f, "Parse error: {}", msg), - } - } -} - -impl std::error::Error for ConfigError {} - #[cfg(test)] mod tests { use super::*; @@ -319,12 +274,4 @@ mod tests { // 800 * 800 * 4 = 2,560,000 bytes = 2.44 MB assert_eq!(mem, 2560000); } - - #[test] - fn test_yaml_serialization() { - let config = MapConfig::default(); - let yaml = config.to_yaml().unwrap(); - let parsed: MapConfig = MapConfig::from_yaml(&yaml).unwrap(); - assert_eq!(parsed.grid.resolution, config.grid.resolution); - } } diff --git a/vastu-slam/src/grid/mod.rs b/vastu-slam/src/grid/mod.rs index 38a13b5..44d8d25 100644 --- a/vastu-slam/src/grid/mod.rs +++ b/vastu-slam/src/grid/mod.rs @@ -86,5 +86,5 @@ pub mod lidar_update; pub mod raycaster; mod storage; -pub use config::{ConfigError, GridConfig, LogOddsConfig, MapConfig, SensorConfig}; +pub use config::{GridConfig, LogOddsConfig, MapConfig, SensorConfig}; pub use storage::{CellCounts, CellMut, GridStorage}; diff --git a/vastu-slam/src/io/mod.rs b/vastu-slam/src/io/mod.rs index 6fc1b45..9acf93b 100644 --- a/vastu-slam/src/io/mod.rs +++ b/vastu-slam/src/io/mod.rs @@ -5,7 +5,7 @@ //! - **Native .vastu format**: Binary format preserving all cell data //! - **PGM export**: ROS-compatible occupancy grid images //! - **SVG export**: Visualization with trajectories for auditing -//! - **Scenario parsing**: YAML-based test scenario definitions +//! - **Scenario parsing**: YAML-based test scenario definitions (requires `scenario` feature) //! //! ## Saving and Loading Maps //! @@ -47,6 +47,7 @@ //! ``` pub mod pgm; +#[cfg(any(test, feature = "scenario"))] pub mod scenario; pub mod svg; pub mod vastu_format; @@ -55,6 +56,7 @@ pub use vastu_format::{IoError, load_vastu, read_vastu, save_vastu, write_vastu} pub use pgm::{export_pgm, export_ros_map, export_yaml, write_pgm}; +#[cfg(any(test, feature = "scenario"))] pub use scenario::{Command, Scenario, ScenarioError, SeedConfig, StartPose, SvgOutputConfig}; pub use svg::{SvgConfig, SvgVisualizer, markers_by_distance, markers_by_time}; diff --git a/vastu-slam/src/lib.rs b/vastu-slam/src/lib.rs index aa6dc33..0b0e7f5 100644 --- a/vastu-slam/src/lib.rs +++ b/vastu-slam/src/lib.rs @@ -100,7 +100,7 @@ pub use core::{ PoseExtrapolator, PoseExtrapolatorConfig, SensorObservation, WorldPoint, }; -pub use grid::{CellCounts, ConfigError, GridConfig, GridStorage, MapConfig, SensorConfig}; +pub use grid::{CellCounts, GridConfig, GridStorage, MapConfig, SensorConfig}; pub use config::{ConfigLoadError, VastuConfig};