Migrate VastuSLAM config from YAML to TOML
- Replace serde_yaml with toml crate for config parsing - Make serde_yaml optional behind "scenario" feature flag - Add config.toml with full documentation - Add scenario_slam example for running test scenarios - Clean up release profile settings - Add dhruva proposal document
This commit is contained in:
codetiger
@@ -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
|
||||
|
||||
<!-- Add your ideas and discussions here -->
|
||||
@@ -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
|
||||
|
||||
+16
-2
@@ -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
|
||||
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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<f32> = points.iter().map(|(a, _, _)| *a).collect();
|
||||
let ranges: Vec<f32> = 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<String, Arc<Mutex<SensorGroupData>>>,
|
||||
last_seq: &mut u64,
|
||||
) -> Option<LidarScan> {
|
||||
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<String, Arc<Mutex<SensorGroupData>>>,
|
||||
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<dyn std::error::Error>> {
|
||||
// Initialize logging
|
||||
env_logger::Builder::from_env(env_logger::Env::default().default_filter_or("info")).init();
|
||||
|
||||
// Parse CLI args
|
||||
let args: Vec<String> = std::env::args().collect();
|
||||
if args.len() < 2 {
|
||||
eprintln!("Usage: {} <scenario.yaml> [--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::<f32>().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<Pose2D> = 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(())
|
||||
}
|
||||
@@ -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
|
||||
|
||||
@@ -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<Self, ConfigLoadError> {
|
||||
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<Self, ConfigLoadError> {
|
||||
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<Self, ConfigLoadError> {
|
||||
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<Self, ConfigLoadError> {
|
||||
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);
|
||||
|
||||
@@ -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<Self, ConfigError> {
|
||||
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<Self, ConfigError> {
|
||||
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<String, ConfigError> {
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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};
|
||||
|
||||
@@ -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};
|
||||
|
||||
@@ -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};
|
||||
|
||||
|
||||
Reference in New Issue
Block a user