Remove bag file system and enhance frontier exploration
- Remove bag file recording/playback infrastructure (no longer needed) - Add recently-visited cooldown to prevent frontier re-selection - Add trapped frontier detection to skip unreachable enclosed areas - Increase A* safety margin from 5cm to 10cm for better wall clearance - Update default config: wheel odometry, hybrid_p2l matching, higher velocities - Add navigation config section with robot radius and safety margins - Add sangam-io lib.rs for library usage
This commit is contained in:
codetiger
+3
-1
@@ -55,4 +55,6 @@ Thumbs.db
|
||||
analysis/
|
||||
.claude/
|
||||
|
||||
*.log
|
||||
*.log
|
||||
|
||||
results/
|
||||
|
||||
@@ -13,7 +13,6 @@ ctrlc = "3.5"
|
||||
env_logger = { version = "0.11", default-features = false, features = ["humantime"] }
|
||||
kiddo = "4.2" # K-d tree for nearest neighbor queries
|
||||
log = "0.4"
|
||||
postcard = { version = "1.1", features = ["alloc"] } # For bag file format (internal)
|
||||
prost = { version = "0.13", default-features = false, features = ["std", "derive"] }
|
||||
rand = { version = "0.9", default-features = false, features = ["std", "std_rng", "os_rng"] }
|
||||
serde = { version = "1.0", default-features = false, features = ["derive", "std"] }
|
||||
|
||||
+3
-15
@@ -60,7 +60,7 @@ DhruvaSLAM is organized as a binary crate (not a library) with 5 logical layers:
|
||||
│
|
||||
┌─────────────────────────────────────────────────────┐
|
||||
│ io/ │ ← Infrastructure
|
||||
│ (sangam_client, bag, streaming) │
|
||||
│ (sangam_client, streaming) │
|
||||
└─────────────────────────────────────────────────────┘
|
||||
│
|
||||
┌─────────────────────────────────────────────────────┐
|
||||
@@ -89,7 +89,7 @@ See individual module READMEs for details:
|
||||
- [sensors/](src/sensors/README.md) - Odometry fusion and lidar preprocessing
|
||||
- [algorithms/](src/algorithms/README.md) - Matching, mapping, localization, descriptors
|
||||
- [engine/](src/engine/README.md) - SLAM orchestration, keyframes, pose graph
|
||||
- [io/](src/io/README.md) - SangamIO client, bag files, TCP streaming
|
||||
- [io/](src/io/README.md) - SangamIO client, TCP streaming
|
||||
|
||||
## Building
|
||||
|
||||
@@ -133,22 +133,12 @@ cargo run --release -- --sangam 192.168.68.101:5555 --port 5557
|
||||
|
||||
# With debug logging
|
||||
RUST_LOG=debug cargo run --release
|
||||
|
||||
# Offline playback from bag file
|
||||
cargo run --release -- --bag bags/recording.bag
|
||||
|
||||
# Loop bag file playback
|
||||
cargo run --release -- --bag bags/recording.bag --loop
|
||||
```
|
||||
|
||||
**Arguments:**
|
||||
| Argument | Description | Default |
|
||||
|----------|-------------|---------|
|
||||
| `-c, --config` | Configuration file | `dhruva-slam.toml` |
|
||||
| `-s, --sangam` | SangamIO address | `192.168.68.101:5555` |
|
||||
| `-p, --port` | TCP publish port | `5557` |
|
||||
| `-b, --bag` | Bag file for offline playback | (none) |
|
||||
| `-l, --loop` | Loop bag file playback | false |
|
||||
|
||||
## Configuration
|
||||
|
||||
@@ -256,7 +246,6 @@ dhruva-slam/
|
||||
│ │
|
||||
│ ├── io/ # I/O infrastructure
|
||||
│ │ ├── sangam_client.rs # SangamIO TCP/UDP client
|
||||
│ │ ├── bag/ # BagRecorder, BagPlayer
|
||||
│ │ ├── streaming/ # TCP/UDP publishers
|
||||
│ │ ├── map_manager.rs # Map persistence
|
||||
│ │ └── motion_controller.rs # Velocity commands
|
||||
@@ -285,7 +274,6 @@ dhruva-slam/
|
||||
├── proto/ # Protobuf schemas
|
||||
│ └── dhruva.proto # Output message definitions
|
||||
│
|
||||
├── bags/ # Recorded sensor data
|
||||
├── results/ # Benchmark results
|
||||
├── docs/ # Design documents
|
||||
│
|
||||
@@ -389,7 +377,7 @@ cargo test -- --nocapture
|
||||
| **Binary, Not Library** | Standalone daemon, not imported as a library |
|
||||
| **Runtime Configuration** | Algorithm selection via TOML config |
|
||||
| **Embedded-First** | Optimized for Allwinner A33 (ARM Cortex-A7) |
|
||||
| **Testability** | Components unit-testable with bag file playback |
|
||||
| **Testability** | Components unit-testable with mock data |
|
||||
|
||||
### Platform Considerations (Allwinner A33)
|
||||
|
||||
|
||||
@@ -52,28 +52,17 @@ navigation_hz = 5.0
|
||||
# Directory to store saved maps
|
||||
path = "/var/lib/dhruva/maps"
|
||||
|
||||
# ============================================================================
|
||||
# Bag File Configuration (offline playback)
|
||||
# ============================================================================
|
||||
|
||||
[bag]
|
||||
# Bag file path for offline playback (comment out for live mode)
|
||||
# file = "bags/test.bag"
|
||||
|
||||
# Loop bag file playback continuously
|
||||
loop_playback = false
|
||||
|
||||
# ============================================================================
|
||||
# Odometry Configuration
|
||||
# ============================================================================
|
||||
|
||||
[odometry]
|
||||
# Odometry algorithm: "wheel", "complementary", "eskf", "mahony"
|
||||
# - wheel: Pure encoder odometry (no sensor fusion)
|
||||
# - wheel: Pure encoder odometry (no sensor fusion, simpler, no calibration)
|
||||
# - complementary: Simple encoder + gyro complementary filter
|
||||
# - eskf: Error-State Kalman Filter (full IMU fusion)
|
||||
# - mahony: Mahony AHRS with auto gyro bias calibration (recommended)
|
||||
algorithm = "mahony"
|
||||
# - mahony: Mahony AHRS with auto gyro bias calibration
|
||||
algorithm = "wheel"
|
||||
|
||||
# Encoder ticks per meter of wheel travel
|
||||
ticks_per_meter = 4464.0
|
||||
@@ -143,6 +132,11 @@ encoder_weight = 0.8
|
||||
# Currently disabled due to transform semantic issues
|
||||
use_submap_matching = false
|
||||
|
||||
# Sanity check thresholds - reject matches that deviate too much from odometry
|
||||
# Helps reject false positive matches in symmetric environments
|
||||
max_theta_deviation = 0.35 # ~20 degrees - reject if rotation differs too much
|
||||
max_translation_deviation = 0.5 # 50cm - reject if translation differs too much
|
||||
|
||||
# ============================================================================
|
||||
# Scan Matching Configuration
|
||||
# ============================================================================
|
||||
@@ -155,7 +149,7 @@ use_submap_matching = false
|
||||
# - multi_res: Multi-resolution correlative (recommended for speed/robustness)
|
||||
# - hybrid_icp: Correlative + P2P ICP
|
||||
# - hybrid_p2l: Correlative + P2L ICP (best accuracy, recommended)
|
||||
algorithm = "multi_res"
|
||||
algorithm = "hybrid_p2l"
|
||||
|
||||
# Always run correlative search first (true) or only on ICP failure (false)
|
||||
always_correlative = true
|
||||
@@ -170,8 +164,8 @@ search_window_y = 0.3
|
||||
search_window_theta = 0.5
|
||||
|
||||
# Resolution for correlative search
|
||||
linear_resolution = 0.03
|
||||
angular_resolution = 0.03
|
||||
linear_resolution = 0.02
|
||||
angular_resolution = 0.01
|
||||
grid_resolution = 0.05
|
||||
|
||||
# Minimum correlative score to accept match
|
||||
@@ -239,6 +233,7 @@ max_active_submaps = 2
|
||||
|
||||
[map]
|
||||
# Cell size in meters (smaller = more detail, more memory)
|
||||
# NOTE: Must match ground truth map resolution (5cm for mock maps)
|
||||
resolution = 0.02
|
||||
|
||||
# Initial map dimensions in meters (grows automatically)
|
||||
@@ -288,7 +283,7 @@ num_rows = 80
|
||||
num_cols = 360
|
||||
|
||||
# Maximum range to consider (meters)
|
||||
max_range = 8.0
|
||||
max_range = 6.0
|
||||
|
||||
# Number of bits in binary signature (must be multiple of 64)
|
||||
# 640 bits = 80 bytes per keyframe
|
||||
@@ -337,33 +332,68 @@ loop_rate_hz = 10.0
|
||||
|
||||
[exploration.motion]
|
||||
# Maximum linear velocity (m/s)
|
||||
max_linear_vel = 0.3
|
||||
max_linear_vel = 0.4
|
||||
|
||||
# Maximum angular velocity (rad/s)
|
||||
max_angular_vel = 0.5
|
||||
max_angular_vel = 0.8
|
||||
|
||||
# Minimum safe distance from obstacles (m)
|
||||
obstacle_clearance = 0.15
|
||||
|
||||
# Normal linear velocity for forward movement (m/s)
|
||||
linear_vel = 0.2
|
||||
linear_vel = 0.3
|
||||
|
||||
# Normal angular velocity for rotation (rad/s)
|
||||
angular_vel = 0.3
|
||||
angular_vel = 0.5
|
||||
|
||||
[exploration.frontier]
|
||||
# Minimum number of cells to consider a valid frontier
|
||||
min_frontier_size = 5
|
||||
min_frontier_size = 3
|
||||
|
||||
# Maximum range to search for frontiers (m)
|
||||
detection_range = 10.0
|
||||
|
||||
# Radius around blocked locations to avoid (m)
|
||||
blocked_radius = 0.3
|
||||
blocked_radius = 0.15
|
||||
|
||||
# Distance threshold for grouping frontier cells (m)
|
||||
clustering_threshold = 0.5
|
||||
|
||||
# ============================================================================
|
||||
# Navigation Configuration
|
||||
# ============================================================================
|
||||
|
||||
[navigation]
|
||||
# Enable goal-based navigation thread
|
||||
enabled = true
|
||||
|
||||
# Navigation update rate (Hz)
|
||||
update_rate_hz = 10.0
|
||||
|
||||
# Robot radius for path planning obstacle inflation (m)
|
||||
robot_radius = 0.18
|
||||
|
||||
# Safety margin beyond robot radius (m)
|
||||
# Adds extra clearance to prevent getting too close to walls
|
||||
# Total inflation = robot_radius + safety_margin
|
||||
safety_margin = 0.10
|
||||
|
||||
# Maximum linear velocity for navigation (m/s)
|
||||
max_linear_vel = 0.4
|
||||
|
||||
# Maximum angular velocity for navigation (rad/s)
|
||||
max_angular_vel = 0.8
|
||||
|
||||
# Distance to consider waypoint reached (m)
|
||||
waypoint_threshold = 0.15
|
||||
|
||||
# Angle error threshold for rotation (rad) - ~20 degrees
|
||||
heading_threshold = 0.35
|
||||
|
||||
# Treat unknown cells as passable (required for exploration)
|
||||
# Set to true only for navigation in fully mapped environments
|
||||
unknown_is_obstacle = false
|
||||
|
||||
# ============================================================================
|
||||
# Session Configuration
|
||||
# ============================================================================
|
||||
|
||||
@@ -55,7 +55,7 @@ impl Default for AStarConfig {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
robot_radius: 0.18, // CRL-200S robot radius
|
||||
safety_margin: 0.05, // 5cm extra clearance from walls
|
||||
safety_margin: 0.10, // 10cm extra clearance from walls
|
||||
allow_diagonal: true,
|
||||
max_iterations: 100_000,
|
||||
simplification_tolerance: 0.05, // 5cm
|
||||
|
||||
@@ -30,6 +30,10 @@ pub struct FrontierConfig {
|
||||
pub surrounded_check_directions: usize,
|
||||
/// Minimum clearance required in a direction to not be considered blocked (meters).
|
||||
pub min_clearance_m: f32,
|
||||
/// Robot radius for path planning (meters).
|
||||
pub robot_radius: f32,
|
||||
/// Safety margin beyond robot radius (meters).
|
||||
pub safety_margin: f32,
|
||||
}
|
||||
|
||||
impl Default for FrontierConfig {
|
||||
@@ -43,6 +47,8 @@ impl Default for FrontierConfig {
|
||||
recovery_rotation_rad: 0.175, // ~10 degrees
|
||||
surrounded_check_directions: 8, // Check 8 directions (every 45°)
|
||||
min_clearance_m: 0.25, // Need at least 25cm clearance
|
||||
robot_radius: 0.18, // CRL-200S robot radius
|
||||
safety_margin: 0.10, // 10cm extra clearance from walls
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -84,6 +90,10 @@ pub struct FrontierExploration {
|
||||
last_frontiers: Vec<Frontier>,
|
||||
/// Locations blocked by bumper (world coordinates).
|
||||
blocked_locations: Vec<(f32, f32)>,
|
||||
/// Recently visited locations with timestamps (cooldown to prevent re-selection).
|
||||
recently_visited: Vec<((f32, f32), std::time::Instant)>,
|
||||
/// Cooldown duration for recently visited locations (seconds).
|
||||
visited_cooldown_secs: f32,
|
||||
/// Current target frontier (if any).
|
||||
current_target: Option<Frontier>,
|
||||
/// Current navigation target point (may differ from centroid if edge-point fallback used).
|
||||
@@ -113,8 +123,8 @@ impl FrontierExploration {
|
||||
pub fn new(config: FrontierConfig) -> Self {
|
||||
// Configure A* for exploration: allow routing through unknown space
|
||||
let planner_config = AStarConfig {
|
||||
robot_radius: 0.18, // CRL-200S robot radius
|
||||
safety_margin: 0.05, // 5cm extra clearance from walls
|
||||
robot_radius: config.robot_radius,
|
||||
safety_margin: config.safety_margin,
|
||||
allow_diagonal: true,
|
||||
max_iterations: 50_000, // Lower limit for quick reachability check
|
||||
simplification_tolerance: 0.0, // Don't need simplified paths for reachability check
|
||||
@@ -126,6 +136,8 @@ impl FrontierExploration {
|
||||
planner: AStarPlanner::new(planner_config),
|
||||
last_frontiers: Vec::new(),
|
||||
blocked_locations: Vec::new(),
|
||||
recently_visited: Vec::new(),
|
||||
visited_cooldown_secs: 10.0, // 10 second cooldown before re-visiting
|
||||
current_target: None,
|
||||
current_target_point: None,
|
||||
path_history: Vec::with_capacity(100),
|
||||
@@ -140,6 +152,31 @@ impl FrontierExploration {
|
||||
}
|
||||
}
|
||||
|
||||
/// Check if a frontier was recently visited (in cooldown period).
|
||||
fn is_recently_visited(&self, frontier: &Frontier) -> bool {
|
||||
let radius_sq = self.config.blocked_radius * self.config.blocked_radius;
|
||||
let now = std::time::Instant::now();
|
||||
|
||||
self.recently_visited.iter().any(|((vx, vy), visited_time)| {
|
||||
// Check if still in cooldown
|
||||
if now.duration_since(*visited_time).as_secs_f32() > self.visited_cooldown_secs {
|
||||
return false;
|
||||
}
|
||||
// Check if frontier is near this visited location
|
||||
let dx = frontier.centroid.x - vx;
|
||||
let dy = frontier.centroid.y - vy;
|
||||
(dx * dx + dy * dy) < radius_sq
|
||||
})
|
||||
}
|
||||
|
||||
/// Clean up expired entries from recently_visited list.
|
||||
fn cleanup_recently_visited(&mut self) {
|
||||
let now = std::time::Instant::now();
|
||||
self.recently_visited.retain(|(_, visited_time)| {
|
||||
now.duration_since(*visited_time).as_secs_f32() < self.visited_cooldown_secs
|
||||
});
|
||||
}
|
||||
|
||||
/// Record current position in path history.
|
||||
fn record_position(&mut self, x: f32, y: f32) {
|
||||
// Only record if significantly different from last position
|
||||
@@ -234,6 +271,61 @@ impl FrontierExploration {
|
||||
cell_value >= 100 && cell_value != 255
|
||||
}
|
||||
|
||||
/// Check if a frontier is trapped inside an enclosed area.
|
||||
///
|
||||
/// A frontier is considered "trapped" if obstacles surround it from all directions
|
||||
/// within a short distance. This typically happens when there's a small pocket of
|
||||
/// unknown cells completely enclosed by walls - the frontier exists but is unreachable.
|
||||
fn is_frontier_trapped(&self, frontier: &Frontier, map: &CurrentMapData) -> bool {
|
||||
let num_directions = 8; // Check every 45 degrees
|
||||
let angle_step = 2.0 * std::f32::consts::PI / num_directions as f32;
|
||||
let check_distance = 0.5; // Check 50cm out from frontier
|
||||
let step_size = 0.05; // 5cm steps
|
||||
|
||||
let fx = frontier.centroid.x;
|
||||
let fy = frontier.centroid.y;
|
||||
|
||||
let mut blocked_directions = 0;
|
||||
|
||||
for i in 0..num_directions {
|
||||
let angle = i as f32 * angle_step;
|
||||
let mut hit_obstacle = false;
|
||||
|
||||
// Ray-cast in this direction
|
||||
let mut dist = step_size;
|
||||
while dist <= check_distance {
|
||||
let check_x = fx + dist * angle.cos();
|
||||
let check_y = fy + dist * angle.sin();
|
||||
|
||||
if self.is_point_blocked(map, check_x, check_y) {
|
||||
hit_obstacle = true;
|
||||
break;
|
||||
}
|
||||
dist += step_size;
|
||||
}
|
||||
|
||||
if hit_obstacle {
|
||||
blocked_directions += 1;
|
||||
}
|
||||
}
|
||||
|
||||
// Frontier is trapped if ALL or almost all directions hit obstacles
|
||||
// Allow at most 1 unblocked direction (could be the frontier edge itself)
|
||||
let trapped = blocked_directions >= num_directions - 1;
|
||||
|
||||
if trapped {
|
||||
log::debug!(
|
||||
"Frontier at ({:.2}, {:.2}) is TRAPPED ({}/{} directions blocked)",
|
||||
fx,
|
||||
fy,
|
||||
blocked_directions,
|
||||
num_directions
|
||||
);
|
||||
}
|
||||
|
||||
trapped
|
||||
}
|
||||
|
||||
/// Find the direction with most clearance from obstacles.
|
||||
fn find_best_escape_direction(&self, map: &CurrentMapData, robot_pose: &Pose2D) -> f32 {
|
||||
let num_directions = 16; // Check every 22.5 degrees
|
||||
@@ -635,10 +727,20 @@ impl FrontierExploration {
|
||||
robot_pose: &Pose2D,
|
||||
map: &CurrentMapData,
|
||||
) -> Option<(&'a Frontier, (f32, f32))> {
|
||||
// Filter non-blocked frontiers and sort by distance (closest first)
|
||||
// Clean up expired cooldown entries
|
||||
self.cleanup_recently_visited();
|
||||
|
||||
// Filter frontiers that are:
|
||||
// - Not blocked (previous navigation failures)
|
||||
// - Not recently visited (cooldown period)
|
||||
// - Not trapped inside enclosed areas
|
||||
let mut candidates: Vec<&Frontier> = frontiers
|
||||
.iter()
|
||||
.filter(|f| !self.is_frontier_blocked(f))
|
||||
.filter(|f| {
|
||||
!self.is_frontier_blocked(f)
|
||||
&& !self.is_recently_visited(f)
|
||||
&& !self.is_frontier_trapped(f, map)
|
||||
})
|
||||
.collect();
|
||||
|
||||
// Sort by distance (ascending - closest first)
|
||||
@@ -971,6 +1073,11 @@ impl ExplorationStrategy for FrontierExploration {
|
||||
target.centroid.x,
|
||||
target.centroid.y
|
||||
);
|
||||
|
||||
// Add to recently visited cooldown list to prevent immediate re-selection
|
||||
// This handles the case where we reached "close enough" but didn't fully clear the frontier
|
||||
self.recently_visited
|
||||
.push(((target.centroid.x, target.centroid.y), std::time::Instant::now()));
|
||||
}
|
||||
|
||||
// Clear current target - navigation completed
|
||||
|
||||
@@ -6,7 +6,6 @@ External communication and data persistence infrastructure.
|
||||
|
||||
The I/O module provides:
|
||||
- **SangamClient** - TCP client for the SangamIO daemon
|
||||
- **Bag** - Recording and playback of sensor data
|
||||
- **Streaming** - TCP publishing for visualization clients
|
||||
|
||||
## Module Structure
|
||||
@@ -15,12 +14,9 @@ The I/O module provides:
|
||||
io/
|
||||
├── mod.rs # Module exports
|
||||
├── sangam_client.rs # SangamIO TCP client
|
||||
├── bag/
|
||||
│ ├── mod.rs # Bag exports
|
||||
│ ├── format.rs # File format constants
|
||||
│ ├── recorder.rs # BagRecorder
|
||||
│ ├── player.rs # BagPlayer
|
||||
│ └── simulated.rs # SimulatedClient
|
||||
├── sangam_udp_receiver.rs # UDP sensor data receiver
|
||||
├── map_manager.rs # Map persistence
|
||||
├── motion_controller.rs # Velocity commands
|
||||
└── streaming/
|
||||
├── mod.rs # Streaming exports
|
||||
├── publisher.rs # OdometryPublisher
|
||||
@@ -100,99 +96,6 @@ Length-prefixed Protobuf framing over TCP:
|
||||
└──────────────────┴─────────────────────┘
|
||||
```
|
||||
|
||||
## Bag Recording and Playback
|
||||
|
||||
Record sensor data for offline testing and development.
|
||||
|
||||
### File Format
|
||||
|
||||
```
|
||||
┌─────────────────────────────────────────────────────────────┐
|
||||
│ Header (64 bytes) │
|
||||
│ • Magic: "DBAG" │
|
||||
│ • Version: 1 │
|
||||
│ • Flags │
|
||||
│ • Start timestamp (μs) │
|
||||
│ • End timestamp (μs) │
|
||||
│ • Message count │
|
||||
│ • Index offset │
|
||||
│ • Reserved │
|
||||
├─────────────────────────────────────────────────────────────┤
|
||||
│ Messages │
|
||||
│ ┌──────────────┬─────────────────────────────────────────┐ │
|
||||
│ │ Length (4B) │ Postcard-encoded BagMessage │ │
|
||||
│ └──────────────┴─────────────────────────────────────────┘ │
|
||||
│ ... │
|
||||
└─────────────────────────────────────────────────────────────┘
|
||||
```
|
||||
|
||||
### BagRecorder
|
||||
|
||||
Record live sensor data to file:
|
||||
|
||||
```rust
|
||||
use crate::io::bag::BagRecorder;
|
||||
|
||||
// Record for specific duration
|
||||
let recorder = BagRecorder::connect(
|
||||
"192.168.68.101:5555",
|
||||
"bags/recording.bag",
|
||||
)?;
|
||||
|
||||
let info = recorder.record_duration(Duration::from_secs(60));
|
||||
println!("Recorded {} messages", info.message_count);
|
||||
|
||||
// Or record until stopped (e.g., via Ctrl-C)
|
||||
let recorder = BagRecorder::connect(address, path)?;
|
||||
// ... in signal handler:
|
||||
recorder.stop();
|
||||
```
|
||||
|
||||
### BagPlayer
|
||||
|
||||
Read recorded bag files:
|
||||
|
||||
```rust
|
||||
use crate::io::bag::BagPlayer;
|
||||
|
||||
let player = BagPlayer::open("bags/recording.bag")?;
|
||||
|
||||
// Get metadata
|
||||
let info = player.info();
|
||||
println!("Duration: {:.1}s", info.duration_secs());
|
||||
println!("Messages: {}", info.message_count);
|
||||
|
||||
// Iterate messages
|
||||
for msg in player.messages() {
|
||||
let msg = msg?;
|
||||
println!("[{}] {}", msg.timestamp_us, msg.topic);
|
||||
}
|
||||
```
|
||||
|
||||
### SimulatedClient
|
||||
|
||||
Drop-in replacement for SangamClient that reads from bag file:
|
||||
|
||||
```rust
|
||||
use crate::io::bag::SimulatedClient;
|
||||
|
||||
// Same API as SangamClient!
|
||||
let mut client = SimulatedClient::open("bags/recording.bag")?;
|
||||
|
||||
loop {
|
||||
match client.recv() {
|
||||
Ok(msg) => process_message(&msg),
|
||||
Err(ClientError::EndOfFile) => break,
|
||||
Err(e) => return Err(e),
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
**Features:**
|
||||
- Preserves original timing (uses inter-message delays)
|
||||
- Can be looped for continuous playback
|
||||
- Identical API to SangamClient for easy switching
|
||||
|
||||
## Streaming
|
||||
|
||||
TCP publishing for visualization clients (Drishti).
|
||||
@@ -289,72 +192,6 @@ let current = pipeline.pose();
|
||||
|
||||
## Usage Examples
|
||||
|
||||
### Complete Recording Session
|
||||
|
||||
```rust
|
||||
use crate::io::bag::BagRecorder;
|
||||
use std::time::Duration;
|
||||
|
||||
fn record_session(address: &str, output: &str, duration_secs: u64) -> Result<()> {
|
||||
println!("Recording from {} to {}", address, output);
|
||||
|
||||
let recorder = BagRecorder::connect(address, output)?;
|
||||
let info = recorder.record_duration(Duration::from_secs(duration_secs));
|
||||
|
||||
println!("Recording complete:");
|
||||
println!(" Messages: {}", info.message_count);
|
||||
println!(" Duration: {:.1}s", info.duration_secs());
|
||||
println!(" Size: {} bytes", info.file_size);
|
||||
|
||||
Ok(())
|
||||
}
|
||||
```
|
||||
|
||||
### Offline SLAM Processing
|
||||
|
||||
```rust
|
||||
use crate::io::bag::SimulatedClient;
|
||||
use crate::io::sangam_client::ClientError;
|
||||
use crate::engine::slam::{OnlineSlam, SlamEngine};
|
||||
|
||||
fn process_bag(bag_path: &str) -> Result<()> {
|
||||
let mut client = SimulatedClient::open(bag_path)?;
|
||||
let mut slam = OnlineSlam::new(slam_config);
|
||||
let mut odom_pipeline = OdometryPipeline::new(odom_config);
|
||||
|
||||
loop {
|
||||
match client.recv() {
|
||||
Ok(msg) => {
|
||||
match msg.topic.as_str() {
|
||||
"sensors/sensor_status" => {
|
||||
let (left, right, gyro) = extract_sensors(&msg);
|
||||
odom_pipeline.process(left, right, gyro, msg.timestamp_us);
|
||||
}
|
||||
"sensors/lidar" => {
|
||||
let scan = extract_lidar(&msg);
|
||||
let odom_delta = odom_pipeline.delta_since_last_scan();
|
||||
let result = slam.process_scan(&scan, &odom_delta, msg.timestamp_us);
|
||||
|
||||
println!("Scan {}: pose ({:.2}, {:.2}, {:.2}°), score {:.2}",
|
||||
slam.status().num_scans,
|
||||
result.pose.x, result.pose.y,
|
||||
result.pose.theta.to_degrees(),
|
||||
result.match_score);
|
||||
}
|
||||
_ => {}
|
||||
}
|
||||
}
|
||||
Err(ClientError::EndOfFile) => break,
|
||||
Err(e) => return Err(e.into()),
|
||||
}
|
||||
}
|
||||
|
||||
println!("Processed {} scans, {} keyframes",
|
||||
slam.status().num_scans, slam.status().num_keyframes);
|
||||
Ok(())
|
||||
}
|
||||
```
|
||||
|
||||
### Live SLAM with Publishing
|
||||
|
||||
```rust
|
||||
@@ -399,7 +236,6 @@ fn run_slam_node(sangam_addr: &str, publish_port: &str) -> Result<()> {
|
||||
| Operation | Time | Notes |
|
||||
|-----------|------|-------|
|
||||
| SangamClient.recv | <1ms | Network-bound |
|
||||
| BagPlayer iteration | ~10μs | Per message |
|
||||
| OdometryPipeline.process | <10μs | Per sample |
|
||||
| Publisher.publish_pose | <100μs | Per client |
|
||||
| Publisher.publish_map | ~5ms | Depends on map size |
|
||||
|
||||
@@ -1,61 +0,0 @@
|
||||
//! Bag file recording and playback for offline testing.
|
||||
//!
|
||||
//! This module provides tools for recording sensor data from SangamIO
|
||||
//! and replaying it for algorithm development without hardware.
|
||||
//!
|
||||
//! # File Format
|
||||
//!
|
||||
//! Bag files use a simple binary format with Postcard serialization:
|
||||
//!
|
||||
//! ```text
|
||||
//! ┌──────────────────────────────────────────────────┐
|
||||
//! │ Header (64 bytes) │
|
||||
//! │ - Magic: "DBAG" (4 bytes) │
|
||||
//! │ - Version: u16 │
|
||||
//! │ - Flags: u16 │
|
||||
//! │ - Start time: u64 (microseconds) │
|
||||
//! │ - End time: u64 (microseconds) │
|
||||
//! │ - Message count: u64 │
|
||||
//! │ - Index offset: u64 (0 if no index) │
|
||||
//! │ - Reserved: 24 bytes │
|
||||
//! ├──────────────────────────────────────────────────┤
|
||||
//! │ Message Stream │
|
||||
//! │ [len:u32][postcard payload]... │
|
||||
//! └──────────────────────────────────────────────────┘
|
||||
//! ```
|
||||
//!
|
||||
//! # Example: Recording
|
||||
//!
|
||||
//! ```ignore
|
||||
//! use dhruva_slam::bag::SangamRecorder;
|
||||
//! use std::time::Duration;
|
||||
//!
|
||||
//! let recorder = SangamRecorder::connect("192.168.68.101:5555", "recording.bag")?;
|
||||
//! let info = recorder.record_duration(Duration::from_secs(60))?;
|
||||
//! println!("Recorded {} messages in {:.1}s", info.message_count, info.duration_secs());
|
||||
//! ```
|
||||
//!
|
||||
//! # Example: Playback
|
||||
//!
|
||||
//! ```ignore
|
||||
//! use dhruva_slam::bag::{BagPlayer, SimulatedClient};
|
||||
//!
|
||||
//! // Direct playback
|
||||
//! let mut player = BagPlayer::open("recording.bag")?;
|
||||
//! for msg in player {
|
||||
//! println!("Message at {} us", msg?.timestamp_us());
|
||||
//! }
|
||||
//!
|
||||
//! // Or use SimulatedClient as drop-in replacement for SangamClient
|
||||
//! let mut client = SimulatedClient::from_bag("recording.bag")?;
|
||||
//! while let Ok(msg) = client.recv() {
|
||||
//! // Same API as SangamClient
|
||||
//! }
|
||||
//! ```
|
||||
|
||||
mod player;
|
||||
mod types;
|
||||
|
||||
// Bag playback - used in main.rs
|
||||
pub use player::BagPlayer;
|
||||
pub use types::{BagMessage, SensorStatusMsg};
|
||||
@@ -1,268 +0,0 @@
|
||||
//! Bag file player for replaying recorded sensor data.
|
||||
//!
|
||||
//! Note: Some utility methods are defined for future use.
|
||||
|
||||
use std::fs::File;
|
||||
use std::io::{BufReader, Read, Seek, SeekFrom};
|
||||
use std::path::Path;
|
||||
use std::time::{Duration, Instant};
|
||||
|
||||
use super::types::{BagHeader, BagMessage, HEADER_SIZE, SensorStatusMsg};
|
||||
use crate::core::types::Timestamped;
|
||||
use crate::io::sangam_client::LidarScan;
|
||||
|
||||
/// Error type for bag playback operations.
|
||||
#[derive(Debug)]
|
||||
pub enum PlayerError {
|
||||
/// I/O error
|
||||
Io(std::io::Error),
|
||||
/// Deserialization error
|
||||
Deserialize(String),
|
||||
/// Invalid bag file format
|
||||
InvalidFormat(String),
|
||||
}
|
||||
|
||||
impl std::fmt::Display for PlayerError {
|
||||
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
|
||||
match self {
|
||||
PlayerError::Io(e) => write!(f, "I/O error: {}", e),
|
||||
PlayerError::Deserialize(e) => write!(f, "Deserialization error: {}", e),
|
||||
PlayerError::InvalidFormat(e) => write!(f, "Invalid format: {}", e),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl std::error::Error for PlayerError {}
|
||||
|
||||
impl From<std::io::Error> for PlayerError {
|
||||
fn from(e: std::io::Error) -> Self {
|
||||
PlayerError::Io(e)
|
||||
}
|
||||
}
|
||||
|
||||
impl From<postcard::Error> for PlayerError {
|
||||
fn from(e: postcard::Error) -> Self {
|
||||
PlayerError::Deserialize(e.to_string())
|
||||
}
|
||||
}
|
||||
|
||||
pub type Result<T> = std::result::Result<T, PlayerError>;
|
||||
|
||||
/// Bag file player for replaying recorded sensor data.
|
||||
///
|
||||
/// Supports both immediate playback (as fast as possible) and real-time
|
||||
/// playback with configurable speed multiplier.
|
||||
///
|
||||
/// # Example
|
||||
///
|
||||
/// ```ignore
|
||||
/// use dhruva_slam::bag::BagPlayer;
|
||||
///
|
||||
/// let mut player = BagPlayer::open("recording.bag")?;
|
||||
///
|
||||
/// // Fast playback (no timing)
|
||||
/// while let Some(msg) = player.next_immediate()? {
|
||||
/// println!("Message at {} us", msg.timestamp_us());
|
||||
/// }
|
||||
///
|
||||
/// // Real-time playback
|
||||
/// player.rewind()?;
|
||||
/// player.set_speed(1.0); // Real-time
|
||||
/// while let Some(msg) = player.next()? {
|
||||
/// // This will block to match original timing
|
||||
/// }
|
||||
/// ```
|
||||
pub struct BagPlayer {
|
||||
reader: BufReader<File>,
|
||||
header: BagHeader,
|
||||
playback_start: Option<Instant>,
|
||||
first_msg_time_us: Option<u64>,
|
||||
playback_speed: f32,
|
||||
messages_read: u64,
|
||||
}
|
||||
|
||||
impl BagPlayer {
|
||||
/// Open a bag file for playback.
|
||||
pub fn open(path: impl AsRef<Path>) -> Result<Self> {
|
||||
let file = File::open(path)?;
|
||||
let mut reader = BufReader::new(file);
|
||||
|
||||
// Read header
|
||||
let mut header_buffer = [0u8; HEADER_SIZE];
|
||||
reader.read_exact(&mut header_buffer)?;
|
||||
|
||||
// Deserialize header
|
||||
let header: BagHeader = postcard::from_bytes(&header_buffer)
|
||||
.map_err(|e| PlayerError::InvalidFormat(format!("Failed to parse header: {}", e)))?;
|
||||
|
||||
// Verify magic
|
||||
if !header.is_valid() {
|
||||
return Err(PlayerError::InvalidFormat(
|
||||
"Invalid bag file magic bytes".to_string(),
|
||||
));
|
||||
}
|
||||
|
||||
Ok(Self {
|
||||
reader,
|
||||
header,
|
||||
playback_start: None,
|
||||
first_msg_time_us: None,
|
||||
playback_speed: 0.0, // Default: no timing (as fast as possible)
|
||||
messages_read: 0,
|
||||
})
|
||||
}
|
||||
|
||||
/// Get the bag file header.
|
||||
pub fn header(&self) -> &BagHeader {
|
||||
&self.header
|
||||
}
|
||||
|
||||
/// Get total message count.
|
||||
pub fn message_count(&self) -> u64 {
|
||||
self.header.message_count
|
||||
}
|
||||
|
||||
/// Get number of messages read so far.
|
||||
pub fn messages_read(&self) -> u64 {
|
||||
self.messages_read
|
||||
}
|
||||
|
||||
/// Set playback speed.
|
||||
///
|
||||
/// - 0.0 = as fast as possible (no timing)
|
||||
/// - 1.0 = real-time
|
||||
/// - 2.0 = 2x speed
|
||||
/// - 0.5 = half speed
|
||||
pub fn set_speed(&mut self, speed: f32) {
|
||||
self.playback_speed = speed;
|
||||
}
|
||||
|
||||
/// Get current playback speed.
|
||||
pub fn speed(&self) -> f32 {
|
||||
self.playback_speed
|
||||
}
|
||||
|
||||
/// Read next message without timing delay.
|
||||
///
|
||||
/// Returns `None` when end of file is reached.
|
||||
pub fn next_immediate(&mut self) -> Result<Option<BagMessage>> {
|
||||
// Read length prefix (4 bytes, little-endian)
|
||||
let mut len_bytes = [0u8; 4];
|
||||
match self.reader.read_exact(&mut len_bytes) {
|
||||
Ok(_) => {}
|
||||
Err(e) if e.kind() == std::io::ErrorKind::UnexpectedEof => return Ok(None),
|
||||
Err(e) => return Err(e.into()),
|
||||
}
|
||||
|
||||
let len = u32::from_le_bytes(len_bytes) as usize;
|
||||
|
||||
// Sanity check length
|
||||
if len > 10_000_000 {
|
||||
// 10 MB max message size
|
||||
return Err(PlayerError::InvalidFormat(format!(
|
||||
"Message too large: {} bytes",
|
||||
len
|
||||
)));
|
||||
}
|
||||
|
||||
// Read payload
|
||||
let mut payload = vec![0u8; len];
|
||||
self.reader.read_exact(&mut payload)?;
|
||||
|
||||
// Deserialize
|
||||
let msg: BagMessage = postcard::from_bytes(&payload)?;
|
||||
|
||||
self.messages_read += 1;
|
||||
|
||||
Ok(Some(msg))
|
||||
}
|
||||
|
||||
/// Read next message with real-time timing.
|
||||
///
|
||||
/// If `playback_speed` is > 0, this will block to match the original
|
||||
/// recording timing.
|
||||
#[allow(clippy::should_implement_trait)]
|
||||
pub fn next(&mut self) -> Result<Option<BagMessage>> {
|
||||
let msg = self.next_immediate()?;
|
||||
|
||||
if let Some(ref msg) = msg
|
||||
&& self.playback_speed > 0.0
|
||||
{
|
||||
self.wait_for_timing(msg.timestamp_us());
|
||||
}
|
||||
|
||||
Ok(msg)
|
||||
}
|
||||
|
||||
/// Wait until the appropriate time to return this message.
|
||||
fn wait_for_timing(&mut self, msg_time_us: u64) {
|
||||
// Initialize playback timing on first message
|
||||
if self.playback_start.is_none() {
|
||||
self.playback_start = Some(Instant::now());
|
||||
self.first_msg_time_us = Some(msg_time_us);
|
||||
return;
|
||||
}
|
||||
|
||||
let elapsed_real = self.playback_start.unwrap().elapsed();
|
||||
let msg_offset_us = msg_time_us.saturating_sub(self.first_msg_time_us.unwrap());
|
||||
let target_elapsed =
|
||||
Duration::from_micros((msg_offset_us as f64 / self.playback_speed as f64) as u64);
|
||||
|
||||
if target_elapsed > elapsed_real {
|
||||
std::thread::sleep(target_elapsed - elapsed_real);
|
||||
}
|
||||
}
|
||||
|
||||
/// Reset to beginning of file.
|
||||
pub fn rewind(&mut self) -> Result<()> {
|
||||
self.reader.seek(SeekFrom::Start(HEADER_SIZE as u64))?;
|
||||
self.playback_start = None;
|
||||
self.first_msg_time_us = None;
|
||||
self.messages_read = 0;
|
||||
Ok(())
|
||||
}
|
||||
|
||||
/// Iterate over only sensor status messages.
|
||||
pub fn sensor_status_iter(&mut self) -> impl Iterator<Item = Result<SensorStatusMsg>> + '_ {
|
||||
std::iter::from_fn(move || {
|
||||
loop {
|
||||
match self.next_immediate() {
|
||||
Ok(Some(BagMessage::SensorStatus(msg))) => return Some(Ok(msg)),
|
||||
Ok(Some(_)) => continue,
|
||||
Ok(None) => return None,
|
||||
Err(e) => return Some(Err(e)),
|
||||
}
|
||||
}
|
||||
})
|
||||
}
|
||||
|
||||
/// Iterate over only LiDAR scans.
|
||||
pub fn lidar_iter(&mut self) -> impl Iterator<Item = Result<Timestamped<LidarScan>>> + '_ {
|
||||
std::iter::from_fn(move || {
|
||||
loop {
|
||||
match self.next_immediate() {
|
||||
Ok(Some(BagMessage::Lidar(scan))) => return Some(Ok(scan)),
|
||||
Ok(Some(_)) => continue,
|
||||
Ok(None) => return None,
|
||||
Err(e) => return Some(Err(e)),
|
||||
}
|
||||
}
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
/// Iterator implementation for convenient `for msg in player { }` syntax.
|
||||
impl Iterator for BagPlayer {
|
||||
type Item = Result<BagMessage>;
|
||||
|
||||
fn next(&mut self) -> Option<Self::Item> {
|
||||
match self.next_immediate() {
|
||||
Ok(Some(msg)) => Some(Ok(msg)),
|
||||
Ok(None) => None,
|
||||
Err(e) => Some(Err(e)),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Tests removed: They depended on BagRecorder which was removed.
|
||||
// BagPlayer functionality is tested via integration tests with real bag files.
|
||||
@@ -1,307 +0,0 @@
|
||||
//! Core data types for bag file format.
|
||||
//!
|
||||
//! Note: Some types are defined for bag recording features.
|
||||
|
||||
use serde::{Deserialize, Serialize};
|
||||
use std::path::PathBuf;
|
||||
|
||||
use crate::core::types::{Pose2D, Timestamped};
|
||||
use crate::io::sangam_client::LidarScan;
|
||||
|
||||
/// Magic bytes at start of bag file.
|
||||
pub const BAG_MAGIC: [u8; 4] = *b"DBAG";
|
||||
|
||||
/// Current bag file format version.
|
||||
pub const BAG_VERSION: u16 = 1;
|
||||
|
||||
/// Size of the bag file header in bytes.
|
||||
pub const HEADER_SIZE: usize = 64;
|
||||
|
||||
/// Bag file header (64 bytes fixed size).
|
||||
///
|
||||
/// Stored at the beginning of every bag file. Contains metadata
|
||||
/// about the recording and pointers to optional sections.
|
||||
#[derive(Debug, Clone, Serialize, Deserialize)]
|
||||
pub struct BagHeader {
|
||||
/// Magic bytes: "DBAG"
|
||||
pub magic: [u8; 4],
|
||||
/// File format version
|
||||
pub version: u16,
|
||||
/// Feature flags (compression, index)
|
||||
pub flags: u16,
|
||||
/// Timestamp of first message (microseconds since epoch)
|
||||
pub start_time_us: u64,
|
||||
/// Timestamp of last message (microseconds since epoch)
|
||||
pub end_time_us: u64,
|
||||
/// Total number of messages in the file
|
||||
pub message_count: u64,
|
||||
/// Byte offset to index section (0 if no index)
|
||||
pub index_offset: u64,
|
||||
/// Reserved for future use
|
||||
pub reserved: [u8; 24],
|
||||
}
|
||||
|
||||
impl BagHeader {
|
||||
/// Create a new header with default values.
|
||||
pub fn new() -> Self {
|
||||
Self {
|
||||
magic: BAG_MAGIC,
|
||||
version: BAG_VERSION,
|
||||
flags: 0,
|
||||
start_time_us: 0,
|
||||
end_time_us: 0,
|
||||
message_count: 0,
|
||||
index_offset: 0,
|
||||
reserved: [0; 24],
|
||||
}
|
||||
}
|
||||
|
||||
/// Check if magic bytes are valid.
|
||||
pub fn is_valid(&self) -> bool {
|
||||
self.magic == BAG_MAGIC
|
||||
}
|
||||
|
||||
/// Get recording duration in microseconds.
|
||||
pub fn duration_us(&self) -> u64 {
|
||||
self.end_time_us.saturating_sub(self.start_time_us)
|
||||
}
|
||||
|
||||
/// Get recording duration in seconds.
|
||||
pub fn duration_secs(&self) -> f64 {
|
||||
self.duration_us() as f64 / 1_000_000.0
|
||||
}
|
||||
}
|
||||
|
||||
impl Default for BagHeader {
|
||||
fn default() -> Self {
|
||||
Self::new()
|
||||
}
|
||||
}
|
||||
|
||||
/// Wheel encoder tick values.
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)]
|
||||
pub struct EncoderTicks {
|
||||
/// Left wheel encoder ticks (raw 16-bit value)
|
||||
pub left: u16,
|
||||
/// Right wheel encoder ticks (raw 16-bit value)
|
||||
pub right: u16,
|
||||
}
|
||||
|
||||
impl EncoderTicks {
|
||||
/// Create new encoder ticks.
|
||||
pub fn new(left: u16, right: u16) -> Self {
|
||||
Self { left, right }
|
||||
}
|
||||
}
|
||||
|
||||
/// Sensor status message from SangamIO.
|
||||
///
|
||||
/// Contains wheel encoder and IMU data at 110Hz.
|
||||
#[derive(Debug, Clone, Serialize, Deserialize)]
|
||||
pub struct SensorStatusMsg {
|
||||
/// Timestamp in microseconds since epoch
|
||||
pub timestamp_us: u64,
|
||||
/// Wheel encoder ticks
|
||||
pub encoder: EncoderTicks,
|
||||
/// Raw gyroscope values [x, y, z] in 0.1 deg/s units
|
||||
pub gyro_raw: [i16; 3],
|
||||
/// Raw accelerometer values [x, y, z]
|
||||
pub accel_raw: [i16; 3],
|
||||
/// LP-filtered tilt/gravity vector [x, y, z] for Mahony AHRS
|
||||
pub tilt_raw: [i16; 3],
|
||||
}
|
||||
|
||||
impl SensorStatusMsg {
|
||||
/// Create a new sensor status message.
|
||||
pub fn new(
|
||||
timestamp_us: u64,
|
||||
encoder: EncoderTicks,
|
||||
gyro_raw: [i16; 3],
|
||||
accel_raw: [i16; 3],
|
||||
tilt_raw: [i16; 3],
|
||||
) -> Self {
|
||||
Self {
|
||||
timestamp_us,
|
||||
encoder,
|
||||
gyro_raw,
|
||||
accel_raw,
|
||||
tilt_raw,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Message types stored in bag files.
|
||||
///
|
||||
/// Each variant corresponds to a different sensor stream.
|
||||
#[derive(Debug, Clone, Serialize, Deserialize)]
|
||||
pub enum BagMessage {
|
||||
/// Sensor status (110 Hz): encoders + IMU
|
||||
SensorStatus(SensorStatusMsg),
|
||||
/// LiDAR scan (5 Hz): point cloud
|
||||
Lidar(Timestamped<LidarScan>),
|
||||
/// Computed odometry pose (for ground truth or computed values)
|
||||
Odometry(Timestamped<Pose2D>),
|
||||
}
|
||||
|
||||
impl BagMessage {
|
||||
/// Get the timestamp of this message in microseconds.
|
||||
pub fn timestamp_us(&self) -> u64 {
|
||||
match self {
|
||||
BagMessage::SensorStatus(msg) => msg.timestamp_us,
|
||||
BagMessage::Lidar(msg) => msg.timestamp_us,
|
||||
BagMessage::Odometry(msg) => msg.timestamp_us,
|
||||
}
|
||||
}
|
||||
|
||||
/// Check if this is a sensor status message.
|
||||
pub fn is_sensor_status(&self) -> bool {
|
||||
matches!(self, BagMessage::SensorStatus(_))
|
||||
}
|
||||
|
||||
/// Check if this is a lidar message.
|
||||
pub fn is_lidar(&self) -> bool {
|
||||
matches!(self, BagMessage::Lidar(_))
|
||||
}
|
||||
|
||||
/// Check if this is an odometry message.
|
||||
pub fn is_odometry(&self) -> bool {
|
||||
matches!(self, BagMessage::Odometry(_))
|
||||
}
|
||||
|
||||
/// Get as sensor status if this is that type.
|
||||
pub fn as_sensor_status(&self) -> Option<&SensorStatusMsg> {
|
||||
match self {
|
||||
BagMessage::SensorStatus(msg) => Some(msg),
|
||||
_ => None,
|
||||
}
|
||||
}
|
||||
|
||||
/// Get as lidar scan if this is that type.
|
||||
pub fn as_lidar(&self) -> Option<&Timestamped<LidarScan>> {
|
||||
match self {
|
||||
BagMessage::Lidar(msg) => Some(msg),
|
||||
_ => None,
|
||||
}
|
||||
}
|
||||
|
||||
/// Get as odometry if this is that type.
|
||||
pub fn as_odometry(&self) -> Option<&Timestamped<Pose2D>> {
|
||||
match self {
|
||||
BagMessage::Odometry(msg) => Some(msg),
|
||||
_ => None,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Information about a bag file.
|
||||
///
|
||||
/// Returned after recording or when inspecting a bag file.
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct BagInfo {
|
||||
/// Path to the bag file
|
||||
pub path: PathBuf,
|
||||
/// Recording duration in microseconds
|
||||
pub duration_us: u64,
|
||||
/// Total number of messages
|
||||
pub message_count: u64,
|
||||
/// File size in bytes
|
||||
pub file_size_bytes: u64,
|
||||
/// Number of sensor status messages
|
||||
pub sensor_count: u64,
|
||||
/// Number of lidar messages
|
||||
pub lidar_count: u64,
|
||||
/// Number of odometry messages
|
||||
pub odometry_count: u64,
|
||||
}
|
||||
|
||||
impl BagInfo {
|
||||
/// Get recording duration in seconds.
|
||||
pub fn duration_secs(&self) -> f64 {
|
||||
self.duration_us as f64 / 1_000_000.0
|
||||
}
|
||||
|
||||
/// Get file size in megabytes.
|
||||
pub fn file_size_mb(&self) -> f64 {
|
||||
self.file_size_bytes as f64 / 1_048_576.0
|
||||
}
|
||||
|
||||
/// Get average message rate in Hz.
|
||||
pub fn message_rate_hz(&self) -> f64 {
|
||||
if self.duration_us == 0 {
|
||||
0.0
|
||||
} else {
|
||||
self.message_count as f64 / self.duration_secs()
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn test_bag_header_new() {
|
||||
let header = BagHeader::new();
|
||||
assert!(header.is_valid());
|
||||
assert_eq!(header.version, BAG_VERSION);
|
||||
assert_eq!(header.flags, 0);
|
||||
assert_eq!(header.message_count, 0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_bag_header_duration() {
|
||||
let mut header = BagHeader::new();
|
||||
header.start_time_us = 1_000_000;
|
||||
header.end_time_us = 6_000_000;
|
||||
|
||||
assert_eq!(header.duration_us(), 5_000_000);
|
||||
assert!((header.duration_secs() - 5.0).abs() < 0.001);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_encoder_ticks() {
|
||||
let ticks = EncoderTicks::new(1000, 1001);
|
||||
assert_eq!(ticks.left, 1000);
|
||||
assert_eq!(ticks.right, 1001);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_bag_message_timestamp() {
|
||||
let sensor_msg = BagMessage::SensorStatus(SensorStatusMsg {
|
||||
timestamp_us: 1000,
|
||||
encoder: EncoderTicks::new(0, 0),
|
||||
gyro_raw: [0, 0, 0],
|
||||
accel_raw: [0, 0, 0],
|
||||
tilt_raw: [0, 0, 1000],
|
||||
});
|
||||
assert_eq!(sensor_msg.timestamp_us(), 1000);
|
||||
assert!(sensor_msg.is_sensor_status());
|
||||
assert!(!sensor_msg.is_lidar());
|
||||
|
||||
let lidar_msg = BagMessage::Lidar(Timestamped::new(vec![], 2000));
|
||||
assert_eq!(lidar_msg.timestamp_us(), 2000);
|
||||
assert!(lidar_msg.is_lidar());
|
||||
assert!(!lidar_msg.is_sensor_status());
|
||||
|
||||
let odom_msg = BagMessage::Odometry(Timestamped::new(Pose2D::identity(), 3000));
|
||||
assert_eq!(odom_msg.timestamp_us(), 3000);
|
||||
assert!(odom_msg.is_odometry());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_bag_info_calculations() {
|
||||
let info = BagInfo {
|
||||
path: PathBuf::from("test.bag"),
|
||||
duration_us: 5_000_000,
|
||||
message_count: 2500,
|
||||
file_size_bytes: 1_048_576,
|
||||
sensor_count: 2400,
|
||||
lidar_count: 25,
|
||||
odometry_count: 75,
|
||||
};
|
||||
|
||||
assert!((info.duration_secs() - 5.0).abs() < 0.001);
|
||||
assert!((info.file_size_mb() - 1.0).abs() < 0.001);
|
||||
assert!((info.message_rate_hz() - 500.0).abs() < 0.1);
|
||||
}
|
||||
}
|
||||
@@ -6,12 +6,10 @@
|
||||
//!
|
||||
//! - [`sangam_client`]: SangamIO TCP client for commands
|
||||
//! - [`sangam_udp_receiver`]: SangamIO UDP receiver for sensor data
|
||||
//! - [`bag`]: Data recording and playback
|
||||
//! - [`streaming`]: TCP/UDP publishing for odometry and SLAM results
|
||||
//! - [`map_manager`]: Map storage and persistence
|
||||
//! - [`motion_controller`]: Velocity command interface to SangamIO
|
||||
|
||||
pub mod bag;
|
||||
pub mod map_manager;
|
||||
pub mod motion_controller;
|
||||
pub mod sangam_client;
|
||||
|
||||
@@ -267,7 +267,7 @@ impl SangamUdpReceiver {
|
||||
}
|
||||
"lidar" => {
|
||||
if let Some(data) = Self::parse_lidar(&sensor_group) {
|
||||
log::info!("Lidar scan received: {} points", data.points.len());
|
||||
log::debug!("Lidar scan received: {} points", data.points.len());
|
||||
// Non-blocking send - drop if channel full
|
||||
self.lidar_tx.try_send(data).ok();
|
||||
} else {
|
||||
|
||||
+13
-27
@@ -17,7 +17,7 @@
|
||||
//! │
|
||||
//! ┌─────────────────────────────────────────────────────┐
|
||||
//! │ io/ │ ← Infrastructure
|
||||
//! │ (sangam_client, bag, streaming) │
|
||||
//! │ (sangam_client, streaming) │
|
||||
//! └─────────────────────────────────────────────────────┘
|
||||
//! │
|
||||
//! ┌─────────────────────────────────────────────────────┐
|
||||
@@ -140,8 +140,6 @@ struct Config {
|
||||
#[serde(default)]
|
||||
map_storage: MapStorageConfig,
|
||||
#[serde(default)]
|
||||
bag: BagConfig,
|
||||
#[serde(default)]
|
||||
preprocessing: PreprocessingConfig,
|
||||
#[serde(default)]
|
||||
slam: SlamConfig,
|
||||
@@ -243,16 +241,6 @@ impl Default for MapStorageConfig {
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug, Deserialize)]
|
||||
#[serde(default)]
|
||||
#[derive(Default)]
|
||||
struct BagConfig {
|
||||
/// Bag file path for offline playback (None for live mode)
|
||||
file: Option<String>,
|
||||
/// Loop bag file playback
|
||||
loop_playback: bool,
|
||||
}
|
||||
|
||||
#[derive(Debug, Deserialize)]
|
||||
#[serde(default)]
|
||||
struct PreprocessingConfig {
|
||||
@@ -643,6 +631,9 @@ struct NavigationConfig {
|
||||
update_rate_hz: f32,
|
||||
/// Robot radius for A* inflation (m).
|
||||
robot_radius: f32,
|
||||
/// Safety margin beyond robot radius (m).
|
||||
/// Adds extra clearance to prevent getting too close to walls.
|
||||
safety_margin: f32,
|
||||
/// Maximum linear velocity (m/s).
|
||||
max_linear_vel: f32,
|
||||
/// Maximum angular velocity (rad/s).
|
||||
@@ -662,6 +653,7 @@ impl Default for NavigationConfig {
|
||||
enabled: true,
|
||||
update_rate_hz: 10.0,
|
||||
robot_radius: 0.18,
|
||||
safety_margin: 0.10, // 10cm extra clearance from walls
|
||||
max_linear_vel: 0.3,
|
||||
max_angular_vel: 0.5,
|
||||
waypoint_threshold: 0.15,
|
||||
@@ -722,7 +714,6 @@ fn print_help() {
|
||||
println!(" All settings are configured via the TOML config file:");
|
||||
println!(" - [source] sangam_address: SangamIO daemon address");
|
||||
println!(" - [output] bind_port: TCP stream port");
|
||||
println!(" - [bag] file, loop: Bag file playback settings");
|
||||
println!();
|
||||
println!("THREADS:");
|
||||
println!(" The daemon runs with 3 fixed threads:");
|
||||
@@ -930,17 +921,10 @@ fn main() {
|
||||
let config = load_config(&args);
|
||||
|
||||
log::info!("dhruva-slam starting");
|
||||
if let Some(ref bag) = config.bag.file {
|
||||
log::info!(" Input: Bag file {}", bag);
|
||||
if config.bag.loop_playback {
|
||||
log::info!(" Loop: enabled");
|
||||
}
|
||||
} else {
|
||||
log::info!(
|
||||
" SangamIO: {} (TCP commands + UDP sensors)",
|
||||
config.source.sangam_address
|
||||
);
|
||||
}
|
||||
log::info!(
|
||||
" SangamIO: {} (TCP commands + UDP sensors)",
|
||||
config.source.sangam_address
|
||||
);
|
||||
log::info!(
|
||||
" Output port: {} (TCP commands+maps, UDP status)",
|
||||
config.output.bind_port
|
||||
@@ -1059,8 +1043,6 @@ fn run_threaded_mode(
|
||||
odometry_type: config.odometry.algorithm_type(),
|
||||
odometry_config,
|
||||
preprocessor_config,
|
||||
bag_file: config.bag.file.clone(),
|
||||
loop_bag: config.bag.loop_playback,
|
||||
};
|
||||
|
||||
// 6. Build stream config
|
||||
@@ -1122,6 +1104,7 @@ fn run_threaded_mode(
|
||||
navigator: NavigatorConfig {
|
||||
planner: AStarConfig {
|
||||
robot_radius: config.navigation.robot_radius,
|
||||
safety_margin: config.navigation.safety_margin,
|
||||
unknown_is_obstacle: config.navigation.unknown_is_obstacle,
|
||||
..Default::default()
|
||||
},
|
||||
@@ -1185,6 +1168,9 @@ fn run_threaded_mode(
|
||||
frontier_detection_range: config.exploration.frontier.detection_range,
|
||||
blocked_radius: config.exploration.frontier.blocked_radius,
|
||||
clustering_threshold: config.exploration.frontier.clustering_threshold,
|
||||
// Use same robot_radius and safety_margin as navigation for consistent path planning
|
||||
robot_radius: config.navigation.robot_radius,
|
||||
safety_margin: config.navigation.safety_margin,
|
||||
..FrontierConfig::default()
|
||||
},
|
||||
loop_rate_hz: config.exploration.loop_rate_hz,
|
||||
|
||||
@@ -414,10 +414,11 @@ impl Navigator {
|
||||
// Path is complete but not at exact goal position.
|
||||
// For frontiers/exploration, accept "close enough" to avoid getting stuck
|
||||
// trying to reach goals in obstacle inflation zones.
|
||||
// Use 1.5x waypoint threshold (typically ~22cm with default 15cm threshold).
|
||||
let goal_dx = target.position.x - pose.x;
|
||||
let goal_dy = target.position.y - pose.y;
|
||||
let goal_dist = (goal_dx * goal_dx + goal_dy * goal_dy).sqrt();
|
||||
let close_enough_threshold = 0.4; // 40cm tolerance for path-complete
|
||||
let close_enough_threshold = self.config.waypoint_reached_threshold * 1.5;
|
||||
|
||||
if goal_dist < close_enough_threshold {
|
||||
log::info!(
|
||||
|
||||
@@ -1,23 +1,18 @@
|
||||
//! SLAM Thread - Real-time sensor processing.
|
||||
//!
|
||||
//! This thread:
|
||||
//! - Receives sensor data via UDP from SangamIO (live mode)
|
||||
//! - Receives sensor data via UDP from SangamIO
|
||||
//! - Processes odometry and lidar data through SLAM engine
|
||||
//! - Updates SharedState with pose, map, and sensor status
|
||||
//! - Handles commands from Command Thread via channel (non-blocking)
|
||||
//!
|
||||
//! # Communication Architecture
|
||||
//!
|
||||
//! **Live mode**: Event-driven via UDP
|
||||
//! Event-driven via UDP:
|
||||
//! - Sensors arrive via crossbeam channels from SangamUdpReceiver
|
||||
//! - Uses `crossbeam::select!` to wait on sensor_status + lidar channels
|
||||
//! - Processes immediately on arrival (no fixed rate loop)
|
||||
//!
|
||||
//! Note: Some methods are defined for planned features.
|
||||
//!
|
||||
//! **Bag mode**: Same as before
|
||||
//! - Reads from bag file with timing control
|
||||
//!
|
||||
//! CRITICAL: This thread NEVER blocks on commands or publishing.
|
||||
//! Sensor data flows uninterrupted.
|
||||
|
||||
@@ -29,7 +24,6 @@ use std::time::Instant;
|
||||
|
||||
use crate::core::types::{LaserScan, Pose2D, PoseTracker};
|
||||
use crate::engine::slam::{OnlineSlam, OnlineSlamConfig, SlamEngine, SlamMode};
|
||||
use crate::io::bag::{BagMessage, BagPlayer, SensorStatusMsg};
|
||||
use crate::io::sangam_client::Message as SangamMessage;
|
||||
use crate::io::sangam_udp_receiver::{
|
||||
LidarData, ReceiverConfig, SangamUdpReceiver, SensorStatusData,
|
||||
@@ -59,10 +53,6 @@ pub struct SlamThreadConfig {
|
||||
pub odometry_config: DynOdometryConfig,
|
||||
/// Scan preprocessor configuration.
|
||||
pub preprocessor_config: PreprocessorConfig,
|
||||
/// Bag file path for offline playback (None for live mode).
|
||||
pub bag_file: Option<String>,
|
||||
/// Loop bag file playback.
|
||||
pub loop_bag: bool,
|
||||
}
|
||||
|
||||
/// SLAM Thread handle.
|
||||
@@ -81,11 +71,7 @@ impl SlamThread {
|
||||
let handle = thread::Builder::new()
|
||||
.name("slam".into())
|
||||
.spawn(move || {
|
||||
if let Some(bag_path) = config.bag_file.clone() {
|
||||
run_bag_loop(config, bag_path, shared_state, command_rx, running);
|
||||
} else {
|
||||
run_live_loop(config, shared_state, command_rx, running);
|
||||
}
|
||||
run_live_loop(config, shared_state, command_rx, running);
|
||||
})
|
||||
.expect("Failed to spawn SLAM thread");
|
||||
|
||||
@@ -191,76 +177,6 @@ fn run_live_loop(
|
||||
log::info!("SLAM thread shutdown complete");
|
||||
}
|
||||
|
||||
/// Run SLAM loop with bag file playback.
|
||||
fn run_bag_loop(
|
||||
config: SlamThreadConfig,
|
||||
bag_path: String,
|
||||
shared_state: SharedStateHandle,
|
||||
command_rx: CommandReceiver,
|
||||
running: Arc<AtomicBool>,
|
||||
) {
|
||||
log::info!("SLAM thread starting (bag playback)");
|
||||
log::info!(" Bag file: {}", bag_path);
|
||||
|
||||
loop {
|
||||
// Open bag file
|
||||
let mut player = match BagPlayer::open(&bag_path) {
|
||||
Ok(p) => p,
|
||||
Err(e) => {
|
||||
log::error!("Failed to open bag file: {}", e);
|
||||
return;
|
||||
}
|
||||
};
|
||||
player.set_speed(1.0); // Real-time playback
|
||||
|
||||
// Initialize components
|
||||
let mut slam_context = SlamContext::new(&config);
|
||||
|
||||
// Main loop
|
||||
while running.load(Ordering::Relaxed) {
|
||||
// 1. Check for commands (non-blocking)
|
||||
slam_context.process_commands(&command_rx, &shared_state);
|
||||
|
||||
// 2. Get next message from bag
|
||||
let msg = match player.next() {
|
||||
Ok(Some(m)) => m,
|
||||
Ok(None) => {
|
||||
log::info!("End of bag file reached");
|
||||
break;
|
||||
}
|
||||
Err(e) => {
|
||||
log::error!("Bag read error: {}", e);
|
||||
break;
|
||||
}
|
||||
};
|
||||
|
||||
match msg {
|
||||
BagMessage::SensorStatus(status) => {
|
||||
slam_context.process_bag_sensor_status(&status, &shared_state);
|
||||
}
|
||||
BagMessage::Lidar(timestamped) => {
|
||||
slam_context.process_lidar(
|
||||
×tamped.data,
|
||||
timestamped.timestamp_us,
|
||||
&shared_state,
|
||||
);
|
||||
}
|
||||
BagMessage::Odometry(_) => {
|
||||
// Skip raw odometry messages, we compute our own
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Check if we should loop
|
||||
if !config.loop_bag || !running.load(Ordering::Relaxed) {
|
||||
break;
|
||||
}
|
||||
log::info!("Looping bag file...");
|
||||
}
|
||||
|
||||
log::info!("SLAM thread shutting down (bag playback)");
|
||||
}
|
||||
|
||||
/// Robot radius used for bumper obstacle marking.
|
||||
const ROBOT_RADIUS: f32 = 0.18;
|
||||
|
||||
@@ -503,22 +419,6 @@ impl SlamContext {
|
||||
}
|
||||
}
|
||||
|
||||
/// Process sensor status from bag file.
|
||||
fn process_bag_sensor_status(
|
||||
&mut self,
|
||||
status: &SensorStatusMsg,
|
||||
shared_state: &SharedStateHandle,
|
||||
) {
|
||||
let timestamp_us = status.timestamp_us;
|
||||
self.process_odometry(
|
||||
status.encoder.left,
|
||||
status.encoder.right,
|
||||
status.gyro_raw[2],
|
||||
timestamp_us,
|
||||
shared_state,
|
||||
);
|
||||
}
|
||||
|
||||
/// Common odometry processing.
|
||||
fn process_odometry(
|
||||
&mut self,
|
||||
|
||||
@@ -125,13 +125,13 @@
|
||||
//! - [`noise`]: Configurable noise generator
|
||||
|
||||
pub mod config;
|
||||
mod encoder_sim;
|
||||
mod imu_sim;
|
||||
mod lidar_sim;
|
||||
mod map_loader;
|
||||
mod noise;
|
||||
mod physics;
|
||||
mod sensor_sim;
|
||||
pub mod encoder_sim;
|
||||
pub mod imu_sim;
|
||||
pub mod lidar_sim;
|
||||
pub mod map_loader;
|
||||
pub mod noise;
|
||||
pub mod physics;
|
||||
pub mod sensor_sim;
|
||||
|
||||
use crate::config::DeviceConfig;
|
||||
use crate::core::driver::{DeviceDriver, DriverInitResult};
|
||||
|
||||
@@ -0,0 +1,18 @@
|
||||
//! SangamIO - Hardware abstraction library for robot vacuum
|
||||
//!
|
||||
//! This library provides the core components for interacting with robot vacuum
|
||||
//! hardware and simulation.
|
||||
//!
|
||||
//! ## Features
|
||||
//!
|
||||
//! - `mock`: Enable mock device simulation for hardware-free testing
|
||||
|
||||
pub mod config;
|
||||
pub mod core;
|
||||
pub mod devices;
|
||||
pub mod error;
|
||||
pub mod streaming;
|
||||
|
||||
// Re-export commonly used types
|
||||
pub use config::Config;
|
||||
pub use error::{Error, Result};
|
||||
+11
-16
@@ -8,16 +8,11 @@
|
||||
//! When a TCP client connects for commands, their IP is automatically registered
|
||||
//! for UDP sensor streaming. This eliminates network flooding from broadcasts.
|
||||
|
||||
mod config;
|
||||
mod core;
|
||||
mod devices;
|
||||
mod error;
|
||||
mod streaming;
|
||||
|
||||
use crate::config::Config;
|
||||
use crate::devices::create_device;
|
||||
use crate::error::Result;
|
||||
use crate::streaming::{TcpReceiver, UdpClientRegistry, UdpPublisher, create_serializer};
|
||||
use sangam_io::config::Config;
|
||||
use sangam_io::core::types::Command;
|
||||
use sangam_io::devices::create_device;
|
||||
use sangam_io::error::{Error, Result};
|
||||
use sangam_io::streaming::{TcpReceiver, UdpClientRegistry, UdpPublisher, create_serializer};
|
||||
use std::env;
|
||||
use std::net::{Shutdown, SocketAddr, TcpListener, UdpSocket};
|
||||
use std::sync::atomic::{AtomicBool, Ordering};
|
||||
@@ -94,7 +89,7 @@ fn main() -> Result<()> {
|
||||
log::info!("Received shutdown signal");
|
||||
r.store(false, Ordering::Relaxed);
|
||||
})
|
||||
.map_err(|e| error::Error::Other(format!("Error setting Ctrl-C handler: {}", e)))?;
|
||||
.map_err(|e| Error::Other(format!("Error setting Ctrl-C handler: {}", e)))?;
|
||||
|
||||
log::debug!("Wire format: Protobuf");
|
||||
|
||||
@@ -106,7 +101,7 @@ fn main() -> Result<()> {
|
||||
|
||||
// Bind UDP socket to any available port (we only send, not receive)
|
||||
let udp_socket = UdpSocket::bind("0.0.0.0:0")
|
||||
.map_err(|e| error::Error::Other(format!("Failed to create UDP socket: {}", e)))?;
|
||||
.map_err(|e| Error::Other(format!("Failed to create UDP socket: {}", e)))?;
|
||||
|
||||
if udp_streaming_port == config.network.port() {
|
||||
log::debug!(
|
||||
@@ -145,14 +140,14 @@ fn main() -> Result<()> {
|
||||
log::error!("UDP publisher error: {}", e);
|
||||
}
|
||||
})
|
||||
.map_err(|e| error::Error::Other(format!("Failed to spawn UDP publisher: {}", e)))?;
|
||||
.map_err(|e| Error::Other(format!("Failed to spawn UDP publisher: {}", e)))?;
|
||||
|
||||
// =========================================================================
|
||||
// TCP Server Setup (commands only)
|
||||
// =========================================================================
|
||||
let bind_addr = &config.network.bind_address;
|
||||
let listener = TcpListener::bind(bind_addr)
|
||||
.map_err(|e| error::Error::Other(format!("Failed to bind to {}: {}", bind_addr, e)))?;
|
||||
.map_err(|e| Error::Other(format!("Failed to bind to {}: {}", bind_addr, e)))?;
|
||||
if let Err(e) = listener.set_nonblocking(true) {
|
||||
log::warn!("Failed to set nonblocking mode: {}", e);
|
||||
}
|
||||
@@ -251,8 +246,8 @@ fn main() -> Result<()> {
|
||||
{
|
||||
let mut driver = driver
|
||||
.lock()
|
||||
.map_err(|e| error::Error::MutexPoisoned(format!("driver mutex: {}", e)))?;
|
||||
driver.send_command(crate::core::types::Command::Shutdown)?;
|
||||
.map_err(|e| Error::MutexPoisoned(format!("driver mutex: {}", e)))?;
|
||||
driver.send_command(Command::Shutdown)?;
|
||||
}
|
||||
|
||||
log::info!("SangamIO stopped");
|
||||
|
||||
@@ -139,10 +139,12 @@ clustering_threshold = 0.5
|
||||
enabled = true
|
||||
update_rate_hz = 10.0
|
||||
robot_radius = 0.18
|
||||
safety_margin = 0.10 # 10cm extra clearance from walls
|
||||
max_linear_vel = 0.4
|
||||
max_angular_vel = 0.8
|
||||
waypoint_threshold = 0.15
|
||||
heading_threshold = 0.35 # ~20 degrees - allow more angular error before rotating
|
||||
unknown_is_obstacle = false # Required for exploration - allow paths through unknown areas
|
||||
|
||||
[session]
|
||||
state_path = "/tmp/dhruva_test_session.json"
|
||||
|
||||
Reference in New Issue
Block a user