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

Add SLAM round-trip test framework and update docs for UDP/TCP architecture

Browse Source 
- Add slam-test/ framework for testing DhruvaSLAM mapping accuracy
- Includes test orchestrator, map analysis, and command scripts
- Update CLAUDE.md to reflect hybrid UDP/TCP streaming architecture
This commit is contained in:
codetiger
2025-12-17 08:08:33 +05:30
parent 2d15609fb9
commit d6c029825d
10 changed files with 1383 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CLAUDE.md
+93 -52
View File
@@ -12,7 +12,7 @@ VacuumTiger is an open-source robotic vacuum firmware project for the CRL-200S h
**Target Platform**: Embedded Linux (ARM) - Allwinner A33 running Tina Linux
**Primary Language**: Rust (edition 2024) for SangamIO, Python 3.8+ for Drishti
**Cross-compilation Target**: `armv7-unknown-linux-musleabihf`
**Architecture**: Daemon-based with TCP streaming (not a library)
**Architecture**: Daemon-based with UDP/TCP streaming (not a library)
## System Architecture
@@ -22,19 +22,20 @@ VacuumTiger is an open-source robotic vacuum firmware project for the CRL-200S h
┌─────────────────────────────────────────────────┐
│ Client Applications (SLAM/Drishti) │
└─────────────┬───────────────┬───────────────────┘
│ TCP 5555/5556 │
│ UDP 5555 │ TCP 5555
│ (sensors) │ (commands)
┌─────────────▼───────────────▼───────────────────┐
│ SangamIO Daemon (Robot) │
│ • Telemetry streaming @ 110Hz (sensors) │
│ • Telemetry streaming @ 110Hz (UDP unicast) │
│ • Lidar streaming @ 5Hz (360° scans) │
│ • Command processing (motion/actuators) │
│ • Real-time control loop (50Hz) │
│ • Command processing (TCP, reliable) │
│ • Real-time control loop (20ms heartbeat) │
└─────────────┬───────────────┬───────────────────┘
│ /dev/ttyS3 │ /dev/ttyS1
┌──────▼─────────┐ ┌──▼────────────┐
│ GD32F103 MCU │ │ Delta-2D Lidar │
│ (Motor Control)│ │ (360° Scanning)│
└────────────────┘ └────────────────┘
│ GD32F103 MCU │ │ Delta-2D Lidar│
│ (Motor Control)│ │ (360° Scan) │
└────────────────┘ └───────────────┘
```
### Key Design Principles
@@ -43,7 +44,7 @@ VacuumTiger is an open-source robotic vacuum firmware project for the CRL-200S h
2. **Concrete Over Abstract**: Direct implementations instead of trait objects (except LidarDriver)
3. **Lock-Free Streaming**: Uses lock-free queues for sensor data flow
4. **Real-Time Guarantees**: OS threads for heartbeat (20ms) instead of async
5. **TCP Protocol**: Protobuf over TCP for all communication
5. **Hybrid Protocol**: UDP for sensor streaming (low latency), TCP for commands (reliable)
## Build and Deployment
@@ -103,7 +104,13 @@ sshpass -p "vacuum@123" ssh root@vacuum "RUST_LOG=info /usr/sbin/sangamio"
**CRITICAL**: Always overwrite `/usr/sbin/sangamio` directly. Do NOT write to `/tmp` folder.
## TCP Protocol
## Network Protocol
SangamIO uses a hybrid UDP/TCP architecture:
- **UDP** for sensor streaming (low latency, no head-of-line blocking)
- **TCP** for commands (reliable delivery)
Both use the same port (5555) for simpler configuration.
### Message Format
@@ -120,41 +127,61 @@ All messages use length-prefixed framing with Protobuf payloads:
See `sangam-io/proto/sangamio.proto` for SangamIO messages (sensors, commands) and `dhruva-slam/proto/dhruva.proto` for SLAM output messages (odometry, map, scan).
### Topics
### Topics (UDP Streaming)
**Outbound (Daemon → Client)**:
**Outbound (Daemon → Client via UDP)**:
- `sensors/sensor_status`: Sensor data @ 110Hz (all 13 sensors)
- `sensors/device_version`: Version info (one-time)
- `sensors/lidar`: LidarScan @ 5Hz (point cloud)
**Inbound (Client → Daemon)**:
**Inbound (Client → Daemon via TCP)**:
- `command`: RobotCommand (SetVelocity, Stop, actuator control, etc.)
### Client Registration
UDP uses unicast (not broadcast). Clients are registered automatically:
1. Client connects TCP to port 5555
2. Server records client IP address
3. UDP packets are sent to client on same port
4. When TCP disconnects, UDP streaming stops
### Example Python Client
```python
import socket
import struct
import threading
from proto import sangamio_pb2
# Connect to daemon
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sock.connect(('192.168.68.101', 5555))
ROBOT_IP = '192.168.68.101'
PORT = 5555
# Read messages
while True:
# Read length prefix
length = struct.unpack('>I', sock.recv(4))[0]
# TCP for commands
tcp_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
tcp_sock.connect((ROBOT_IP, PORT))
# Read Protobuf message
data = sock.recv(length)
msg = sangamio_pb2.Message()
msg.ParseFromString(data)
# UDP for sensor data (same port)
udp_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
udp_sock.bind(('0.0.0.0', PORT))
if msg.topic == 'sensors/sensor_status':
sg = msg.sensor_group
charging = sg.values['is_charging'].bool_val
print(f"Charging: {charging}")
def receive_sensors():
while True:
data, addr = udp_sock.recvfrom(4096)
length = struct.unpack('>I', data[:4])[0]
msg = sangamio_pb2.Message()
msg.ParseFromString(data[4:4+length])
if msg.topic == 'sensors/sensor_status':
sg = msg.sensor_group
print(f"Battery: {sg.values['battery_level'].u8_val}%")
# Start receiving in background
threading.Thread(target=receive_sensors, daemon=True).start()
# Send commands via TCP
def send_command(cmd):
payload = cmd.SerializeToString()
tcp_sock.send(struct.pack('>I', len(payload)) + payload)
```
## Configuration
@@ -180,40 +207,54 @@ bind_address = "0.0.0.0:5555"
VacuumTiger/
├── sangam-io/ # Hardware abstraction daemon
│ ├── src/
│ │ ├── main.rs # Entry point, arg parsing
│ │ ├── config.rs # Configuration structures
│ │ ├── main.rs # Entry point, TCP/UDP listener
│ │ ├── config.rs # TOML configuration loading
│ │ ├── error.rs # Error types
│ │ ├── core/ # Core abstractions
│ │ │ ├── mod.rs
│ │ │ ├── driver.rs # DeviceDriver trait
│ │ │ └── types.rs # SensorValue, Command types
│ │ ├── devices/ # Hardware drivers
│ │ │ ├── mod.rs # Device factory
│ │ │ └── crl200s/ # CRL-200S robot platform
│ │ │ ├── mod.rs
│ │ │ ├── constants.rs # Offsets, flags, timing
│ │ │ ├── gd32/ # Motor controller driver
│ │ │ │ ├── mod.rs # Driver orchestration
│ │ │ │ ├── protocol.rs # RxPacket, PacketReader
│ │ │ │ ├── packet.rs # TxPacket, checksum
│ │ │ │ ├── ring_buffer.rs # Zero-copy ring buffer
│ │ │ │ ├── heartbeat.rs # 20ms watchdog loop
│ │ │ │ ├── reader.rs # Status parsing
│ │ │ │ ├── commands.rs # Command handlers
│ │ │ │ └── state.rs # Shared atomic state
│ │ │ └── delta2d/ # 3iRobotix lidar
│ │ │ ├── mod.rs # LidarDriver impl
│ │ │ └── protocol.rs # Packet parsing
│ │ └── streaming/ # TCP communication
│ │ │ ├── mod.rs # Device factory (crl200s, mock)
│ │ │ ├── crl200s/ # Real hardware driver
│ │ │ │ ├── mod.rs
│ │ │ │ ├── constants.rs # Offsets, flags, timing
│ │ │ │ ├── COMMANDS.md # GD32 command reference
│ │ │ │ ├── SENSORSTATUS.md # Sensor packet docs
│ │ │ │ ├── gd32/ # Motor controller driver
│ │ │ │ │ ├── mod.rs # Driver orchestration
│ │ │ │ │ ├── protocol.rs # RxPacket, PacketReader
│ │ │ │ │ ├── packet.rs # TxPacket, checksum
│ │ │ │ │ ├── ring_buffer.rs # Zero-copy ring buffer
│ │ │ │ │ ├── heartbeat.rs # 20ms watchdog loop
│ │ │ │ │ ├── reader.rs # Status parsing
│ │ │ │ │ ├── commands.rs # Command handlers
│ │ │ │ │ └── state.rs # Shared atomic state
│ │ │ │ └── delta2d/ # 3iRobotix lidar
│ │ │ │ ├── mod.rs # LidarDriver impl
│ │ │ │ └── protocol.rs # Packet parsing
│ │ │ └── mock/ # Simulation driver (--features mock)
│ │ │ ├── mod.rs # MockDriver, simulation loop
│ │ │ ├── config.rs # SimulationConfig structs
│ │ │ ├── physics.rs # Differential drive kinematics
│ │ │ ├── lidar_sim.rs # Ray-casting lidar
│ │ │ ├── imu_sim.rs # IMU with noise
│ │ │ ├── encoder_sim.rs # Encoder with slip
│ │ │ ├── sensor_sim.rs # Bumpers, cliffs
│ │ │ ├── map_loader.rs # PGM+YAML loader
│ │ │ └── noise.rs # Noise generator
│ │ └── streaming/ # Network communication
│ │ ├── mod.rs # Re-exports
│ │ ├── wire.rs # Protobuf serialization
│ │ ├── tcp_publisher.rs # Outbound stream
│ │ └── tcp_receiver.rs # Command handler
│ │ ├── udp_publisher.rs # UDP sensor streaming
│ │ └── tcp_receiver.rs # TCP command handler
│ ├── proto/ # Protobuf schema
│ │ └── sangamio.proto # Message definitions
│ ├── sangamio.toml # Configuration file
│ ├── COMMANDS.md # GD32 command reference
│ └── SENSORSTATUS.md # Sensor packet documentation
│ ├── maps/ # Example simulation maps
│ ├── docs/ # Documentation
│ │ └── mock-device-guide.md
│ ├── sangamio.toml # Hardware configuration
│ └── mock.toml # Simulation configuration
│
└── drishti/ # Python visualization client
├── drishti.py # Console client
slam-test/.gitignore
+17
View File
@@ -0,0 +1,17 @@
# Test results
results/
# Python
__pycache__/
*.pyc
.venv/
venv/
# Generated proto files
scripts/proto/*.py
!scripts/proto/__init__.py
# Editor
.vscode/
.idea/
*.swp
slam-test/Makefile
+40
View File
@@ -0,0 +1,40 @@
# SLAM Test Framework Makefile
.PHONY: proto test clean help
# Default target
help:
@echo "SLAM Round-Trip Test Framework"
@echo ""
@echo "Targets:"
@echo " make proto - Compile protobuf files"
@echo " make test - Run test with defaults (2 min)"
@echo " make test-quick - Run quick test (30 sec)"
@echo " make test-long - Run long test (5 min)"
@echo " make clean - Clean results directory"
@echo ""
@echo "Manual run:"
@echo " ./test_slam.sh --duration 60 --output ./results/mytest"
# Compile protobuf
proto:
@echo "Compiling protobuf..."
protoc --python_out=scripts/proto \
-I ../dhruva-slam/proto \
../dhruva-slam/proto/dhruva.proto
@echo "Done: scripts/proto/dhruva_pb2.py"
# Run tests
test:
./test_slam.sh --duration 120
test-quick:
./test_slam.sh --duration 30
test-long:
./test_slam.sh --duration 300
# Clean results
clean:
rm -rf results/*
@echo "Cleaned results/"
slam-test/README.md
+154
View File
@@ -0,0 +1,154 @@
# SLAM Round-Trip Test Framework
Integration testing for DhruvaSLAM mapping accuracy against mock simulation ground truth.
## Quick Start
```bash
cd slam-test
# First time: compile protobuf
make proto
# Run with defaults (2 min duration)
make test
# Or run directly with custom options
./test_slam.sh --output ./results/mytest --duration 60
```
## Requirements
- Rust toolchain (for building sangam-io and dhruva-slam)
- Python 3.8+ with packages:
```bash
pip install protobuf numpy pillow matplotlib
```
- protoc (protocol buffer compiler):
```bash
# macOS
brew install protobuf
# Linux
apt install protobuf-compiler
```
## Available Commands
```bash
make proto # Compile protobuf files (required first time)
make test # Run test (2 min duration)
make test-quick # Run quick test (30 sec)
make test-long # Run long test (5 min)
make clean # Clean results directory
```
## Manual Test Options
```bash
./test_slam.sh [OPTIONS]
Options:
--output DIR Output directory (default: results/TIMESTAMP)
--duration SEC Test duration in seconds (default: 120)
--config FILE DhruvaSLAM config file (default: dhruva-test.toml)
--help Show help message
```
## Structure
```
slam-test/
├── README.md # This file
├── Makefile # Build and test commands
├── test_slam.sh # Main test orchestrator
├── test_config.toml # Test parameters and thresholds
├── dhruva-test.toml # SLAM config (resolution=0.05m)
├── round-trip-test-proposal.md # Detailed design doc
├── scripts/
│ ├── send_command.py # Send commands to DhruvaSLAM
│ ├── analyze_map.py # Compare maps, generate reports
│ └── proto/
│ └── dhruva_pb2.py # Generated protobuf (run make proto)
└── results/ # Test outputs (gitignored)
```
## Test Flow
1. **Setup**: Start SangamIO (mock) and DhruvaSLAM
2. **Mapping**: Send StartMapping, robot explores autonomously
3. **Capture**: Stop mapping, save output map
4. **Analysis**: Compare against ground truth, generate reports
## Outputs
Each test run creates a timestamped directory with:
| File | Description |
|------|-------------|
| `sangamio.log` | SangamIO process logs |
| `dhruva.log` | DhruvaSLAM process logs |
| `ground_truth.pgm` | Input map copy |
| `slam_output.pgm` | SLAM generated map |
| `comparison.png` | Visual diff overlay |
| `analysis.json` | Numeric metrics |
| `summary.txt` | Human-readable results |
## Visual Diff Colors
- **White**: Free space (both agree)
- **Green**: Occupied (both agree - correct detection)
- **Red**: False positive (SLAM occupied, ground truth free)
- **Blue**: False negative (SLAM free, ground truth occupied)
- **Gray**: Unknown in SLAM output
## Success Criteria
| Metric | Pass | Warning |
|--------|------|---------|
| Occupied overlap | > 70% | > 60% |
| Free space overlap | > 85% | > 75% |
| Coverage ratio | > 75% | > 60% |
## Configuration
### Test Config (`test_config.toml`)
Test parameters and pass/fail thresholds.
### SLAM Config (`dhruva-test.toml`)
DhruvaSLAM configuration with **resolution matching ground truth** (0.05m = 5cm).
Key settings that must match `sangam-io/mock.toml`:
- `[map] resolution = 0.05`
- `[lidar]` mounting offsets
- `[odometry]` encoder parameters
## Troubleshooting
### "Proto files not compiled"
```bash
make proto
```
### "Could not connect to port 5557"
Check if DhruvaSLAM started:
```bash
tail -f results/*/dhruva.log
```
### "SangamIO failed to start"
Check if port 5555 is in use:
```bash
lsof -i :5555
```
## See Also
- [Detailed Proposal](round-trip-test-proposal.md)
- [DhruvaSLAM README](../dhruva-slam/README.md)
- [SangamIO Mock Guide](../sangam-io/docs/mock-device-guide.md)
slam-test/dhruva-test.toml
+147
View File
@@ -0,0 +1,147 @@
# DhruvaSLAM Test Configuration
#
# This config is used for round-trip testing against mock simulation.
# Key difference: resolution matches ground truth map (0.05m = 5cm)
[source]
sangam_address = "localhost:5555"
[output]
bind_port = 5557
odometry_rate_hz = 50
map_rate_hz = 5.0
feature_rate_hz = 1.0
[stream]
robot_status_hz = 10.0
sensor_status_hz = 10.0
map_hz = 2.0
navigation_hz = 5.0
[map_storage]
# Use temp directory for test isolation
path = "/tmp/dhruva_test_maps"
[odometry]
# Use wheel-only odometry to avoid Mahony calibration issues
algorithm = "wheel"
ticks_per_meter = 4464.0
wheel_base = 0.233
[lidar]
# Must match mock.toml lidar configuration
mounting_x = -0.110
mounting_y = 0.0
optical_offset = 0.025
angle_offset = 0.2182
[preprocessing]
min_range = 0.15
max_range = 5.0
target_points = 180
min_angle_step = 0.0175
[slam]
min_match_score = 0.3
lost_threshold = 0.1
min_scan_points = 50
encoder_weight = 0.8
use_submap_matching = false
[matcher]
algorithm = "hybrid_p2l"
always_correlative = true
encoder_weight = 1.0
search_window_x = 0.3
search_window_y = 0.3
search_window_theta = 0.5
linear_resolution = 0.02
angular_resolution = 0.01
grid_resolution = 0.05
correlative_min_score = 0.4
icp_max_iterations = 50
icp_translation_epsilon = 0.001
icp_rotation_epsilon = 0.001
icp_max_correspondence_distance = 0.5
icp_min_correspondences = 10
icp_outlier_ratio = 0.1
icp_robust_kernel = "welsch"
icp_kernel_scale = 0.1
[keyframe]
min_translation = 0.5
min_rotation = 0.5
max_keyframes = 1000
min_interval_ms = 500
[submap]
scans_per_submap = 100
overlap_scans = 10
max_active_submaps = 2
[map]
# CRITICAL: Must match ground truth resolution (5cm)
resolution = 0.05
initial_width = 20.0
initial_height = 20.0
log_odds_occupied = 0.9
log_odds_free = -0.7
log_odds_max = 50.0
log_odds_min = -50.0
occupied_threshold = 0.5
free_threshold = -0.5
[loop_closure]
enabled = true
detection_interval = 5
min_keyframe_gap = 20
optimization_threshold = 3
max_candidates = 5
[loop_closure.iris]
num_rows = 80
num_cols = 360
max_range = 8.0
signature_bits = 640
[loop_closure.matching]
max_hamming_distance = 100
min_icp_score = 0.5
[optimization]
solver = "sparse_cg"
max_iterations = 50
tolerance = 1e-6
[exploration]
# Enabled for autonomous frontier-based exploration
enabled = true
strategy = "frontier"
loop_rate_hz = 10.0
[exploration.motion]
max_linear_vel = 0.3
max_angular_vel = 0.5
obstacle_clearance = 0.15
linear_vel = 0.2
angular_vel = 0.3
[exploration.frontier]
min_frontier_size = 3 # Smaller frontiers still worth exploring
detection_range = 10.0
blocked_radius = 0.15 # Reduced to allow closer exploration to obstacles
clustering_threshold = 0.5
[navigation]
enabled = true
update_rate_hz = 10.0
robot_radius = 0.18
max_linear_vel = 0.3
max_angular_vel = 0.5
waypoint_threshold = 0.15
goal_threshold = 0.08
heading_threshold = 0.15
[session]
state_path = "/tmp/dhruva_test_session.json"
auto_load_last_map = false
slam-test/scripts/analyze_map.py
Executable
+463
View File
@@ -0,0 +1,463 @@
#!/usr/bin/env python3
"""
Analyze SLAM output map against ground truth.
Generates:
- comparison.png: Visual diff overlay
- trajectory.png: Robot path (if available)
- analysis.json: Numeric metrics
- summary.txt: Human-readable summary
"""
import argparse
import json
import sys
from dataclasses import dataclass
from datetime import datetime
from pathlib import Path
from typing import Optional, Tuple
import numpy as np
from PIL import Image
# Try to import matplotlib for visualization
try:
import matplotlib.pyplot as plt
import matplotlib.patches as mpatches
HAS_MATPLOTLIB = True
except ImportError:
HAS_MATPLOTLIB = False
print("Warning: matplotlib not available, skipping visualizations")
@dataclass
class MapData:
"""Loaded map data."""
pixels: np.ndarray # 2D array of pixel values (0-255)
resolution: float # meters per pixel
origin_x: float # world X of pixel (0,0)
origin_y: float # world Y of pixel (0,0)
width: int
height: int
@dataclass
class ComparisonResult:
"""Map comparison metrics."""
# Cell counts
total_gt_occupied: int
total_gt_free: int
total_slam_occupied: int
total_slam_free: int
total_slam_unknown: int
# Overlap metrics
true_positive: int # Both say occupied
true_negative: int # Both say free
false_positive: int # SLAM occupied, GT free
false_negative: int # SLAM free, GT occupied
# Computed metrics
occupied_overlap_pct: float
free_overlap_pct: float
false_positive_pct: float
false_negative_pct: float
# Coverage
explored_cells: int
coverage_ratio: float
def load_pgm(path: Path, resolution: float = 0.05,
origin_x: float = 0.0, origin_y: float = 0.0) -> MapData:
"""Load PGM map file."""
img = Image.open(path).convert('L')
pixels = np.array(img)
return MapData(
pixels=pixels,
resolution=resolution,
origin_x=origin_x,
origin_y=origin_y,
width=pixels.shape[1],
height=pixels.shape[0]
)
def load_yaml_metadata(yaml_path: Path) -> dict:
"""Load map metadata from YAML file."""
metadata = {}
try:
import yaml
with open(yaml_path) as f:
data = yaml.safe_load(f)
if 'resolution' in data:
metadata['resolution'] = float(data['resolution'])
if 'origin' in data:
origin = data['origin']
if isinstance(origin, list) and len(origin) >= 2:
metadata['origin_x'] = float(origin[0])
metadata['origin_y'] = float(origin[1])
except ImportError:
# Fallback to simple parsing if PyYAML not available
try:
with open(yaml_path) as f:
for line in f:
if ':' in line:
key, value = line.split(':', 1)
key = key.strip()
value = value.strip()
if key == 'resolution':
metadata['resolution'] = float(value)
elif key == 'origin' and value:
# Parse [x, y, theta] format
value = value.strip('[]')
parts = [float(x.strip()) for x in value.split(',')]
metadata['origin_x'] = parts[0]
metadata['origin_y'] = parts[1]
except Exception as e:
print(f"Warning: Could not parse YAML metadata: {e}")
except Exception as e:
print(f"Warning: Could not parse YAML metadata: {e}")
return metadata
def classify_pixel(value: int) -> str:
"""Classify pixel as free, occupied, or unknown."""
if value >= 240: # White = free
return 'free'
elif value <= 20: # Black = occupied
return 'occupied'
else: # Gray = unknown
return 'unknown'
def align_maps(ground_truth: MapData, slam_map: MapData,
robot_start: Tuple[float, float, float]) -> Tuple[np.ndarray, np.ndarray]:
"""
Align SLAM map to ground truth using robot start position.
The SLAM system builds maps relative to the robot's starting position,
treating it as (0,0) in SLAM coordinates. The robot_start parameter
provides the robot's actual starting position in world coordinates,
allowing us to translate between coordinate systems:
- SLAM (0,0) = World (robot_start.x, robot_start.y)
- SLAM origin (-10,-10) in SLAM coords = World origin + robot_start
Returns aligned arrays of same dimensions as ground truth.
"""
# Robot start position in world coordinates
start_x, start_y, _ = robot_start
# Convert SLAM origin from SLAM-local to world coordinates
# SLAM's (0,0) corresponds to world's (start_x, start_y)
slam_world_origin_x = slam_map.origin_x + start_x
slam_world_origin_y = slam_map.origin_y + start_y
# Create output array matching ground truth dimensions
aligned_slam = np.full_like(ground_truth.pixels, 128, dtype=np.uint8) # Unknown
# For each pixel in ground truth, find corresponding SLAM pixel
for gt_py in range(ground_truth.height):
for gt_px in range(ground_truth.width):
# Ground truth pixel to world coordinates
# Note: PGM has Y=0 at top, but world has Y=0 at bottom
world_x = ground_truth.origin_x + gt_px * ground_truth.resolution
world_y = ground_truth.origin_y + (ground_truth.height - 1 - gt_py) * ground_truth.resolution
# World to SLAM pixel coordinates (using world-adjusted origin)
slam_px = int((world_x - slam_world_origin_x) / slam_map.resolution)
slam_py = slam_map.height - 1 - int((world_y - slam_world_origin_y) / slam_map.resolution)
# Check bounds
if 0 <= slam_px < slam_map.width and 0 <= slam_py < slam_map.height:
aligned_slam[gt_py, gt_px] = slam_map.pixels[slam_py, slam_px]
return ground_truth.pixels, aligned_slam
def compare_maps(gt_pixels: np.ndarray, slam_pixels: np.ndarray) -> ComparisonResult:
"""Compare aligned maps and compute metrics."""
# Classify all pixels
gt_free = gt_pixels >= 240
gt_occupied = gt_pixels <= 20
slam_free = slam_pixels >= 240
slam_occupied = slam_pixels <= 20
slam_unknown = (slam_pixels > 20) & (slam_pixels < 240)
# Count cells
total_gt_occupied = np.sum(gt_occupied)
total_gt_free = np.sum(gt_free)
total_slam_occupied = np.sum(slam_occupied)
total_slam_free = np.sum(slam_free)
total_slam_unknown = np.sum(slam_unknown)
# Compute overlaps (only where SLAM has data)
slam_has_data = ~slam_unknown
true_positive = np.sum(gt_occupied & slam_occupied)
true_negative = np.sum(gt_free & slam_free)
false_positive = np.sum(gt_free & slam_occupied)
false_negative = np.sum(gt_occupied & slam_free)
# Compute percentages
if total_gt_occupied > 0:
occupied_overlap_pct = 100.0 * true_positive / total_gt_occupied
false_negative_pct = 100.0 * false_negative / total_gt_occupied
else:
occupied_overlap_pct = 0.0
false_negative_pct = 0.0
if total_gt_free > 0:
free_overlap_pct = 100.0 * true_negative / total_gt_free
false_positive_pct = 100.0 * false_positive / total_gt_free
else:
free_overlap_pct = 0.0
false_positive_pct = 0.0
# Coverage ratio
explored_cells = np.sum(slam_has_data)
total_navigable = total_gt_free + total_gt_occupied
coverage_ratio = explored_cells / total_navigable if total_navigable > 0 else 0.0
return ComparisonResult(
total_gt_occupied=int(total_gt_occupied),
total_gt_free=int(total_gt_free),
total_slam_occupied=int(total_slam_occupied),
total_slam_free=int(total_slam_free),
total_slam_unknown=int(total_slam_unknown),
true_positive=int(true_positive),
true_negative=int(true_negative),
false_positive=int(false_positive),
false_negative=int(false_negative),
occupied_overlap_pct=occupied_overlap_pct,
free_overlap_pct=free_overlap_pct,
false_positive_pct=false_positive_pct,
false_negative_pct=false_negative_pct,
explored_cells=int(explored_cells),
coverage_ratio=coverage_ratio
)
def create_diff_image(gt_pixels: np.ndarray, slam_pixels: np.ndarray,
output_path: Path) -> None:
"""Create color-coded diff image."""
if not HAS_MATPLOTLIB:
return
height, width = gt_pixels.shape
diff_rgb = np.zeros((height, width, 3), dtype=np.uint8)
# Classify pixels
gt_free = gt_pixels >= 240
gt_occupied = gt_pixels <= 20
slam_free = slam_pixels >= 240
slam_occupied = slam_pixels <= 20
slam_unknown = (slam_pixels > 20) & (slam_pixels < 240)
# White: Both free
both_free = gt_free & slam_free
diff_rgb[both_free] = [255, 255, 255]
# Green: Both occupied (correct detection)
both_occupied = gt_occupied & slam_occupied
diff_rgb[both_occupied] = [0, 200, 0]
# Red: False positive (SLAM occupied, GT free)
false_pos = gt_free & slam_occupied
diff_rgb[false_pos] = [255, 0, 0]
# Blue: False negative (SLAM free, GT occupied)
false_neg = gt_occupied & slam_free
diff_rgb[false_neg] = [0, 0, 255]
# Gray: Unknown in SLAM
diff_rgb[slam_unknown] = [180, 180, 180]
# Light gray: GT unknown (neither free nor occupied)
gt_unknown = ~gt_free & ~gt_occupied
diff_rgb[gt_unknown & ~slam_unknown] = [220, 220, 220]
# Save image
fig, ax = plt.subplots(figsize=(12, 10))
ax.imshow(diff_rgb)
ax.set_title("Map Comparison: SLAM vs Ground Truth")
ax.axis('off')
# Legend
legend_elements = [
mpatches.Patch(color='white', label='Free (both agree)'),
mpatches.Patch(color='green', label='Occupied (both agree)'),
mpatches.Patch(color='red', label='False positive'),
mpatches.Patch(color='blue', label='False negative'),
mpatches.Patch(color='gray', label='Unknown (SLAM)'),
]
ax.legend(handles=legend_elements, loc='upper right')
plt.tight_layout()
plt.savefig(output_path, dpi=150)
plt.close()
print(f" Saved: {output_path}")
def write_analysis_json(result: ComparisonResult, resolution: float,
duration: int, output_path: Path) -> None:
"""Write analysis results to JSON."""
data = {
"test_info": {
"timestamp": datetime.now().isoformat(),
"duration_seconds": duration,
"resolution_m": resolution
},
"geometric_accuracy": {
"occupied_overlap_pct": round(result.occupied_overlap_pct, 2),
"free_overlap_pct": round(result.free_overlap_pct, 2),
"false_positive_pct": round(result.false_positive_pct, 2),
"false_negative_pct": round(result.false_negative_pct, 2)
},
"cell_counts": {
"gt_occupied": result.total_gt_occupied,
"gt_free": result.total_gt_free,
"slam_occupied": result.total_slam_occupied,
"slam_free": result.total_slam_free,
"slam_unknown": result.total_slam_unknown
},
"coverage": {
"explored_cells": result.explored_cells,
"coverage_ratio": round(result.coverage_ratio, 3)
},
"confusion_matrix": {
"true_positive": result.true_positive,
"true_negative": result.true_negative,
"false_positive": result.false_positive,
"false_negative": result.false_negative
}
}
with open(output_path, 'w') as f:
json.dump(data, f, indent=2)
print(f" Saved: {output_path}")
def write_summary(result: ComparisonResult, resolution: float,
duration: int, output_path: Path) -> None:
"""Write human-readable summary."""
# Determine pass/fail
pass_threshold = 70.0
status = "PASS" if result.occupied_overlap_pct >= pass_threshold else "FAIL"
area_m2 = result.explored_cells * (resolution ** 2)
lines = [
"SLAM Round-Trip Test Summary",
"=" * 40,
f"Date: {datetime.now().strftime('%Y-%m-%d %H:%M:%S')}",
f"Duration: {duration} seconds",
f"Result: {status} ({result.occupied_overlap_pct:.1f}% overlap)",
"",
"Accuracy Metrics:",
f" - Occupied overlap: {result.occupied_overlap_pct:.1f}%",
f" - Free space overlap: {result.free_overlap_pct:.1f}%",
f" - False positive rate: {result.false_positive_pct:.1f}%",
f" - False negative rate: {result.false_negative_pct:.1f}%",
"",
"Coverage:",
f" - Explored area: {area_m2:.2f} m²",
f" - Coverage ratio: {result.coverage_ratio * 100:.1f}%",
"",
"Cell Counts:",
f" - Ground truth occupied: {result.total_gt_occupied}",
f" - Ground truth free: {result.total_gt_free}",
f" - SLAM occupied: {result.total_slam_occupied}",
f" - SLAM free: {result.total_slam_free}",
f" - SLAM unknown: {result.total_slam_unknown}",
]
with open(output_path, 'w') as f:
f.write('\n'.join(lines))
print(f" Saved: {output_path}")
# Also print to console
print("")
for line in lines:
print(line)
def main():
parser = argparse.ArgumentParser(description="Analyze SLAM map vs ground truth")
parser.add_argument("--ground-truth", required=True, help="Ground truth PGM file")
parser.add_argument("--slam-output", required=True, help="SLAM output PGM file")
parser.add_argument("--start-pose", default="1.5,3.5,0",
help="Robot start pose: x,y,theta")
parser.add_argument("--resolution", type=float, default=0.05,
help="Map resolution (meters/pixel)")
parser.add_argument("--output-dir", required=True, help="Output directory")
parser.add_argument("--duration", type=int, default=120,
help="Test duration for reporting")
args = parser.parse_args()
gt_path = Path(args.ground_truth)
slam_path = Path(args.slam_output)
output_dir = Path(args.output_dir)
# Parse robot start pose
start_parts = [float(x) for x in args.start_pose.split(',')]
robot_start = (start_parts[0], start_parts[1], start_parts[2] if len(start_parts) > 2 else 0.0)
print("Loading maps...")
# Load ground truth
if not gt_path.exists():
print(f"ERROR: Ground truth not found: {gt_path}")
sys.exit(1)
gt_yaml = gt_path.with_suffix('.yaml')
gt_meta = load_yaml_metadata(gt_yaml) if gt_yaml.exists() else {}
gt_resolution = gt_meta.get('resolution', args.resolution)
gt_origin_x = gt_meta.get('origin_x', 0.0)
gt_origin_y = gt_meta.get('origin_y', 0.0)
ground_truth = load_pgm(gt_path, gt_resolution, gt_origin_x, gt_origin_y)
print(f" Ground truth: {ground_truth.width}x{ground_truth.height} @ {gt_resolution}m/px")
# Load SLAM output
if not slam_path.exists():
print(f"ERROR: SLAM output not found: {slam_path}")
sys.exit(1)
slam_yaml = slam_path.with_suffix('.yaml')
slam_meta = load_yaml_metadata(slam_yaml) if slam_yaml.exists() else {}
slam_resolution = slam_meta.get('resolution', args.resolution)
slam_origin_x = slam_meta.get('origin_x', robot_start[0])
slam_origin_y = slam_meta.get('origin_y', robot_start[1])
slam_map = load_pgm(slam_path, slam_resolution, slam_origin_x, slam_origin_y)
print(f" SLAM output: {slam_map.width}x{slam_map.height} @ {slam_resolution}m/px")
print("Aligning maps...")
gt_aligned, slam_aligned = align_maps(ground_truth, slam_map, robot_start)
print("Comparing maps...")
result = compare_maps(gt_aligned, slam_aligned)
print("Generating outputs...")
output_dir.mkdir(parents=True, exist_ok=True)
# Create diff image
create_diff_image(gt_aligned, slam_aligned, output_dir / "comparison.png")
# Write JSON report
write_analysis_json(result, args.resolution, args.duration,
output_dir / "analysis.json")
# Write summary
write_summary(result, args.resolution, args.duration,
output_dir / "summary.txt")
if __name__ == "__main__":
main()
slam-test/scripts/proto/__init__.py
+1
View File
@@ -0,0 +1 @@
# Proto package
slam-test/scripts/send_command.py
Executable
+202
View File
@@ -0,0 +1,202 @@
#!/usr/bin/env python3
"""
Send commands to DhruvaSLAM via TCP.
Usage:
python send_command.py start_mapping [--name NAME] [--port PORT]
python send_command.py stop_mapping [--save] [--port PORT]
python send_command.py emergency_stop [--port PORT]
"""
import argparse
import socket
import struct
import sys
import uuid
from pathlib import Path
# Add proto directory to path
SCRIPT_DIR = Path(__file__).parent
PROTO_DIR = SCRIPT_DIR / "proto"
sys.path.insert(0, str(PROTO_DIR))
try:
import dhruva_pb2
except ImportError:
print("ERROR: Proto files not compiled. Run:")
print(f" protoc --python_out={PROTO_DIR} -I ../dhruva-slam/proto dhruva.proto")
print("")
print("Or install protobuf: pip install protobuf")
sys.exit(1)
def send_message(sock: socket.socket, msg: bytes) -> None:
"""Send length-prefixed message."""
length = struct.pack(">I", len(msg))
sock.sendall(length + msg)
def recv_message(sock: socket.socket, timeout: float = 5.0) -> bytes:
"""Receive length-prefixed message."""
sock.settimeout(timeout)
# Read length (4 bytes)
length_data = b""
while len(length_data) < 4:
chunk = sock.recv(4 - len(length_data))
if not chunk:
raise ConnectionError("Connection closed")
length_data += chunk
length = struct.unpack(">I", length_data)[0]
# Read message
data = b""
while len(data) < length:
chunk = sock.recv(length - len(data))
if not chunk:
raise ConnectionError("Connection closed")
data += chunk
return data
def start_mapping(host: str, port: int, name: str) -> bool:
"""Send StartMapping command."""
request_id = str(uuid.uuid4())[:8]
cmd = dhruva_pb2.DhruvaCommand()
cmd.request_id = request_id
cmd.start_mapping.map_name = name
print(f"Connecting to {host}:{port}...")
with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as sock:
sock.connect((host, port))
print(f"Sending StartMapping (name={name})...")
send_message(sock, cmd.SerializeToString())
# Wait for response
try:
response_data = recv_message(sock)
response = dhruva_pb2.DhruvaResponse()
response.ParseFromString(response_data)
if response.success:
print(f"SUCCESS: Mapping started (map_id={response.mapping_started.map_id})")
return True
else:
print(f"FAILED: {response.error_message}")
return False
except Exception as e:
print(f"No response received (may still have started): {e}")
return True # Assume success if no error
def stop_mapping(host: str, port: int, save: bool) -> bool:
"""Send StopMapping command."""
request_id = str(uuid.uuid4())[:8]
cmd = dhruva_pb2.DhruvaCommand()
cmd.request_id = request_id
cmd.stop_mapping.save = save
print(f"Connecting to {host}:{port}...")
with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as sock:
sock.connect((host, port))
print(f"Sending StopMapping (save={save})...")
send_message(sock, cmd.SerializeToString())
try:
response_data = recv_message(sock)
response = dhruva_pb2.DhruvaResponse()
response.ParseFromString(response_data)
if response.success:
if response.HasField("mapping_stopped"):
ms = response.mapping_stopped
print(f"SUCCESS: Mapping stopped (saved={ms.saved}, area={ms.area_m2:.2f}m²)")
else:
print("SUCCESS: Mapping stopped")
return True
else:
print(f"FAILED: {response.error_message}")
return False
except Exception as e:
print(f"No response received: {e}")
return True
def emergency_stop(host: str, port: int) -> bool:
"""Send EmergencyStop command."""
request_id = str(uuid.uuid4())[:8]
cmd = dhruva_pb2.DhruvaCommand()
cmd.request_id = request_id
cmd.emergency_stop.CopyFrom(dhruva_pb2.EmergencyStopCommand())
print(f"Connecting to {host}:{port}...")
with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as sock:
sock.connect((host, port))
print("Sending EmergencyStop...")
send_message(sock, cmd.SerializeToString())
try:
response_data = recv_message(sock)
response = dhruva_pb2.DhruvaResponse()
response.ParseFromString(response_data)
if response.success:
print("SUCCESS: Emergency stop executed")
return True
else:
print(f"FAILED: {response.error_message}")
return False
except Exception as e:
print(f"No response received: {e}")
return True
def main():
parser = argparse.ArgumentParser(description="Send commands to DhruvaSLAM")
parser.add_argument("--host", default="localhost", help="DhruvaSLAM host")
parser.add_argument("--port", type=int, default=5557, help="DhruvaSLAM port")
subparsers = parser.add_subparsers(dest="command", required=True)
# start_mapping
start_parser = subparsers.add_parser("start_mapping", help="Start mapping")
start_parser.add_argument("--name", default="test_map", help="Map name")
# stop_mapping
stop_parser = subparsers.add_parser("stop_mapping", help="Stop mapping")
stop_parser.add_argument("--save", action="store_true", help="Save map before stopping")
# emergency_stop
subparsers.add_parser("emergency_stop", help="Emergency stop")
args = parser.parse_args()
try:
if args.command == "start_mapping":
success = start_mapping(args.host, args.port, args.name)
elif args.command == "stop_mapping":
success = stop_mapping(args.host, args.port, args.save)
elif args.command == "emergency_stop":
success = emergency_stop(args.host, args.port)
else:
print(f"Unknown command: {args.command}")
sys.exit(1)
sys.exit(0 if success else 1)
except ConnectionRefusedError:
print(f"ERROR: Could not connect to {args.host}:{args.port}")
print("Is DhruvaSLAM running?")
sys.exit(1)
except Exception as e:
print(f"ERROR: {e}")
sys.exit(1)
if __name__ == "__main__":
main()
slam-test/test_config.toml
+40
View File
@@ -0,0 +1,40 @@
# SLAM Round-Trip Test Configuration
[test]
name = "round_trip_slam"
# Test duration in seconds (configurable via CLI --duration)
duration_seconds = 120
# Coverage target (0.0-1.0) - stop when this % of area explored
coverage_target = 0.8
[paths]
# All paths relative to VacuumTiger root
sangamio_config = "sangam-io/mock.toml"
dhruva_config = "slam-test/dhruva-test.toml"
ground_truth_map = "sangam-io/maps/example.pgm"
ground_truth_yaml = "sangam-io/maps/example.yaml"
[ground_truth]
# Map resolution in meters per pixel (from example.yaml)
resolution = 0.05
# Robot start position in world coordinates (from mock.toml)
robot_start_x = 1.5
robot_start_y = 3.5
robot_start_theta = 0.0
[network]
sangamio_port = 5555
dhruva_port = 5557
[thresholds]
# Pass/fail thresholds for automated testing
occupied_overlap_pass = 0.70
occupied_overlap_warn = 0.60
free_overlap_pass = 0.85
free_overlap_warn = 0.75
wall_rms_error_pass = 0.05 # meters
wall_rms_error_warn = 0.10
coverage_pass = 0.75
coverage_warn = 0.60
max_drift_pass = 0.20 # meters
max_drift_warn = 0.30
slam-test/test_slam.sh
Executable
+226
View File
@@ -0,0 +1,226 @@
#!/bin/bash
#
# SLAM Round-Trip Test Script
# Tests DhruvaSLAM mapping against mock simulation ground truth
#
# Usage:
# ./test_slam.sh [OPTIONS]
#
# Options:
# --output DIR Output directory (default: results/TIMESTAMP)
# --duration SEC Test duration in seconds (default: 120)
# --config FILE DhruvaSLAM config file (default: dhruva-test.toml)
# --help Show this help message
set -e
# Default values
DURATION=120
CONFIG="dhruva-test.toml"
OUTPUT_DIR=""
# Parse arguments
while [[ $# -gt 0 ]]; do
case $1 in
--output|-o)
OUTPUT_DIR="$2"
shift 2
;;
--duration|-d)
DURATION="$2"
shift 2
;;
--config|-c)
CONFIG="$2"
shift 2
;;
--help|-h)
head -20 "$0" | tail -15
exit 0
;;
*)
echo "Unknown option: $1"
exit 1
;;
esac
done
# Get script directory and project root
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
PROJECT_ROOT="$(dirname "$SCRIPT_DIR")"
# Set output directory with timestamp if not specified (use absolute path)
if [[ -z "$OUTPUT_DIR" ]]; then
OUTPUT_DIR="$SCRIPT_DIR/results/$(date +%Y%m%d_%H%M%S)"
else
# Convert relative path to absolute
OUTPUT_DIR="$(cd "$(dirname "$OUTPUT_DIR")" 2>/dev/null && pwd)/$(basename "$OUTPUT_DIR")" || OUTPUT_DIR="$SCRIPT_DIR/$OUTPUT_DIR"
fi
# Create output directory
mkdir -p "$OUTPUT_DIR"
# Use drishti venv Python (has protobuf installed)
PYTHON="$PROJECT_ROOT/drishti/venv/bin/python3"
if [[ ! -x "$PYTHON" ]]; then
echo "ERROR: Python venv not found at $PYTHON"
echo "Create it with: cd ../drishti && python3 -m venv venv && source venv/bin/activate && pip install protobuf numpy pillow matplotlib"
exit 1
fi
echo "=============================================="
echo " SLAM Round-Trip Test"
echo "=============================================="
echo "Output: $OUTPUT_DIR"
echo "Duration: ${DURATION}s"
echo "Config: $CONFIG"
echo "=============================================="
# Cleanup function
cleanup() {
echo ""
echo "Cleaning up..."
if [[ -n "$SANGAM_PID" ]]; then
kill $SANGAM_PID 2>/dev/null || true
fi
if [[ -n "$DHRUVA_PID" ]]; then
kill $DHRUVA_PID 2>/dev/null || true
fi
wait 2>/dev/null || true
}
trap cleanup EXIT
# 1. Start SangamIO (mock mode)
echo ""
echo "[1/7] Starting SangamIO (mock mode)..."
(cd "$PROJECT_ROOT/sangam-io" && cargo run --release --features mock -- --config mock.toml) \
> "$OUTPUT_DIR/sangamio.log" 2>&1 &
SANGAM_PID=$!
echo " PID: $SANGAM_PID"
sleep 2
# Check if SangamIO started
if ! kill -0 $SANGAM_PID 2>/dev/null; then
echo "ERROR: SangamIO failed to start. Check $OUTPUT_DIR/sangamio.log"
exit 1
fi
# 2. Start DhruvaSLAM
echo ""
echo "[2/7] Starting DhruvaSLAM..."
(cd "$PROJECT_ROOT/dhruva-slam" && RUST_LOG=info cargo run --release -- --config "../slam-test/$CONFIG") \
> "$OUTPUT_DIR/dhruva.log" 2>&1 &
DHRUVA_PID=$!
echo " PID: $DHRUVA_PID"
sleep 3
# Check if DhruvaSLAM started
if ! kill -0 $DHRUVA_PID 2>/dev/null; then
echo "ERROR: DhruvaSLAM failed to start. Check $OUTPUT_DIR/dhruva.log"
exit 1
fi
# 3. Send start mapping command
echo ""
echo "[3/7] Sending StartMapping command..."
$PYTHON "$SCRIPT_DIR/scripts/send_command.py" --port 5557 start_mapping \
--name "test_$(date +%H%M%S)"
if [[ $? -ne 0 ]]; then
echo "ERROR: Failed to send StartMapping command"
exit 1
fi
# 4. Wait for duration with progress
echo ""
echo "[4/7] Mapping in progress (${DURATION}s)..."
ELAPSED=0
while [[ $ELAPSED -lt $DURATION ]]; do
# Check processes are still running
if ! kill -0 $SANGAM_PID 2>/dev/null; then
echo "ERROR: SangamIO died unexpectedly"
exit 1
fi
if ! kill -0 $DHRUVA_PID 2>/dev/null; then
echo "ERROR: DhruvaSLAM died unexpectedly"
exit 1
fi
# Progress indicator
REMAINING=$((DURATION - ELAPSED))
printf "\r Time remaining: %3ds " $REMAINING
sleep 5
ELAPSED=$((ELAPSED + 5))
done
echo ""
# 5. Stop mapping
echo ""
echo "[5/7] Stopping mapping..."
$PYTHON "$SCRIPT_DIR/scripts/send_command.py" --port 5557 stop_mapping --save
sleep 2
# 6. Collect outputs
echo ""
echo "[6/7] Collecting outputs..."
# Copy ground truth (use the map from mock.toml - currently large_room)
cp "$PROJECT_ROOT/sangam-io/maps/large_room.pgm" "$OUTPUT_DIR/ground_truth.pgm"
cp "$PROJECT_ROOT/sangam-io/maps/large_room.yaml" "$OUTPUT_DIR/ground_truth.yaml"
echo " Copied ground truth map"
# Find and copy SLAM output
SLAM_MAP_DIR="/tmp/dhruva_test_maps"
if [[ -d "$SLAM_MAP_DIR" ]]; then
# Find most recent map file (macOS compatible)
LATEST_MAP=$(find "$SLAM_MAP_DIR" -name "*.pgm" -type f -exec stat -f '%m %N' {} \; 2>/dev/null | sort -rn | head -1 | cut -d' ' -f2-)
if [[ -n "$LATEST_MAP" ]]; then
cp "$LATEST_MAP" "$OUTPUT_DIR/slam_output.pgm"
# Copy associated yaml if exists
YAML_FILE="${LATEST_MAP%.pgm}.yaml"
if [[ -f "$YAML_FILE" ]]; then
cp "$YAML_FILE" "$OUTPUT_DIR/slam_output.yaml"
fi
echo " Copied SLAM output: $(basename "$LATEST_MAP")"
else
echo " WARNING: No SLAM map file found in $SLAM_MAP_DIR"
fi
else
echo " WARNING: SLAM map directory not found: $SLAM_MAP_DIR"
fi
# Kill processes before analysis
echo " Stopping processes..."
kill $SANGAM_PID $DHRUVA_PID 2>/dev/null || true
wait 2>/dev/null || true
unset SANGAM_PID DHRUVA_PID
# 7. Run analysis
echo ""
echo "[7/7] Analyzing results..."
if [[ -f "$OUTPUT_DIR/slam_output.pgm" ]]; then
$PYTHON "$SCRIPT_DIR/scripts/analyze_map.py" \
--ground-truth "$OUTPUT_DIR/ground_truth.pgm" \
--slam-output "$OUTPUT_DIR/slam_output.pgm" \
--start-pose 1.0,1.0,0 \
--resolution 0.05 \
--output-dir "$OUTPUT_DIR"
else
echo " SKIPPED: No SLAM output to analyze"
echo " Check $OUTPUT_DIR/dhruva.log for errors"
fi
# Print summary
echo ""
echo "=============================================="
echo " Test Complete"
echo "=============================================="
echo "Results: $OUTPUT_DIR"
echo ""
if [[ -f "$OUTPUT_DIR/summary.txt" ]]; then
cat "$OUTPUT_DIR/summary.txt"
else
echo "Logs available:"
echo " - $OUTPUT_DIR/sangamio.log"
echo " - $OUTPUT_DIR/dhruva.log"
fi