From 91d669b82fde83862d952ba30bfb32e60724a4d3 Mon Sep 17 00:00:00 2001
From: codetiger <nharishankar@gmail.com>
Date: Fri, 19 Dec 2025 10:06:46 +0530
Subject: [PATCH] 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
---
 .gitignore                                   |   4 +-
 dhruva-slam/Cargo.toml                       |   1 -
 dhruva-slam/README.md                        |  18 +-
 dhruva-slam/dhruva-slam.toml                 |  78 +++--
 dhruva-slam/src/algorithms/planning/astar.rs |   2 +-
 dhruva-slam/src/exploration/frontier.rs      | 115 ++++++-
 dhruva-slam/src/io/README.md                 | 170 +---------
 dhruva-slam/src/io/bag/mod.rs                |  61 ----
 dhruva-slam/src/io/bag/player.rs             | 268 ----------------
 dhruva-slam/src/io/bag/types.rs              | 307 -------------------
 dhruva-slam/src/io/mod.rs                    |   2 -
 dhruva-slam/src/io/sangam_udp_receiver.rs    |   2 +-
 dhruva-slam/src/main.rs                      |  40 +--
 dhruva-slam/src/navigation/navigator.rs      |   3 +-
 dhruva-slam/src/threads/slam_thread.rs       | 106 +------
 sangam-io/src/devices/mock/mod.rs            |  14 +-
 sangam-io/src/lib.rs                         |  18 ++
 sangam-io/src/main.rs                        |  27 +-
 slam-test/dhruva-test.toml                   |   2 +
 19 files changed, 232 insertions(+), 1006 deletions(-)
 delete mode 100644 dhruva-slam/src/io/bag/mod.rs
 delete mode 100644 dhruva-slam/src/io/bag/player.rs
 delete mode 100644 dhruva-slam/src/io/bag/types.rs
 create mode 100644 sangam-io/src/lib.rs

diff --git a/.gitignore b/.gitignore
index f654245..5b7a131 100644
--- a/.gitignore
+++ b/.gitignore
@@ -55,4 +55,6 @@ Thumbs.db
 analysis/
 .claude/
 
-*.log
\ No newline at end of file
+*.log
+
+results/
diff --git a/dhruva-slam/Cargo.toml b/dhruva-slam/Cargo.toml
index 3cb1540..c239fab 100644
--- a/dhruva-slam/Cargo.toml
+++ b/dhruva-slam/Cargo.toml
@@ -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"] }
diff --git a/dhruva-slam/README.md b/dhruva-slam/README.md
index cb44641..bf0de3f 100644
--- a/dhruva-slam/README.md
+++ b/dhruva-slam/README.md
@@ -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)
 
diff --git a/dhruva-slam/dhruva-slam.toml b/dhruva-slam/dhruva-slam.toml
index c06534d..a77d280 100644
--- a/dhruva-slam/dhruva-slam.toml
+++ b/dhruva-slam/dhruva-slam.toml
@@ -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
 # ============================================================================
diff --git a/dhruva-slam/src/algorithms/planning/astar.rs b/dhruva-slam/src/algorithms/planning/astar.rs
index 50a63c8..c6a1781 100644
--- a/dhruva-slam/src/algorithms/planning/astar.rs
+++ b/dhruva-slam/src/algorithms/planning/astar.rs
@@ -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
diff --git a/dhruva-slam/src/exploration/frontier.rs b/dhruva-slam/src/exploration/frontier.rs
index 04161be..29a7917 100644
--- a/dhruva-slam/src/exploration/frontier.rs
+++ b/dhruva-slam/src/exploration/frontier.rs
@@ -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
diff --git a/dhruva-slam/src/io/README.md b/dhruva-slam/src/io/README.md
index 6ad0138..5b38e12 100644
--- a/dhruva-slam/src/io/README.md
+++ b/dhruva-slam/src/io/README.md
@@ -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 |
diff --git a/dhruva-slam/src/io/bag/mod.rs b/dhruva-slam/src/io/bag/mod.rs
deleted file mode 100644
index 6e14cad..0000000
--- a/dhruva-slam/src/io/bag/mod.rs
+++ /dev/null
@@ -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};
diff --git a/dhruva-slam/src/io/bag/player.rs b/dhruva-slam/src/io/bag/player.rs
deleted file mode 100644
index cf27705..0000000
--- a/dhruva-slam/src/io/bag/player.rs
+++ /dev/null
@@ -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.
diff --git a/dhruva-slam/src/io/bag/types.rs b/dhruva-slam/src/io/bag/types.rs
deleted file mode 100644
index 8b0b93d..0000000
--- a/dhruva-slam/src/io/bag/types.rs
+++ /dev/null
@@ -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);
-    }
-}
diff --git a/dhruva-slam/src/io/mod.rs b/dhruva-slam/src/io/mod.rs
index 9b3c933..72c19b7 100644
--- a/dhruva-slam/src/io/mod.rs
+++ b/dhruva-slam/src/io/mod.rs
@@ -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;
diff --git a/dhruva-slam/src/io/sangam_udp_receiver.rs b/dhruva-slam/src/io/sangam_udp_receiver.rs
index f6ce7bd..eb8cbf8 100644
--- a/dhruva-slam/src/io/sangam_udp_receiver.rs
+++ b/dhruva-slam/src/io/sangam_udp_receiver.rs
@@ -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 {
diff --git a/dhruva-slam/src/main.rs b/dhruva-slam/src/main.rs
index ab79a0f..589493f 100644
--- a/dhruva-slam/src/main.rs
+++ b/dhruva-slam/src/main.rs
@@ -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,
diff --git a/dhruva-slam/src/navigation/navigator.rs b/dhruva-slam/src/navigation/navigator.rs
index be1e69a..78756a7 100644
--- a/dhruva-slam/src/navigation/navigator.rs
+++ b/dhruva-slam/src/navigation/navigator.rs
@@ -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!(
diff --git a/dhruva-slam/src/threads/slam_thread.rs b/dhruva-slam/src/threads/slam_thread.rs
index 527b5b3..5ec93dd 100644
--- a/dhruva-slam/src/threads/slam_thread.rs
+++ b/dhruva-slam/src/threads/slam_thread.rs
@@ -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(
-                        &timestamped.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,
diff --git a/sangam-io/src/devices/mock/mod.rs b/sangam-io/src/devices/mock/mod.rs
index c3bc3d4..e613e0e 100644
--- a/sangam-io/src/devices/mock/mod.rs
+++ b/sangam-io/src/devices/mock/mod.rs
@@ -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};
diff --git a/sangam-io/src/lib.rs b/sangam-io/src/lib.rs
new file mode 100644
index 0000000..0e70732
--- /dev/null
+++ b/sangam-io/src/lib.rs
@@ -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};
diff --git a/sangam-io/src/main.rs b/sangam-io/src/main.rs
index 5b83c17..29ae695 100644
--- a/sangam-io/src/main.rs
+++ b/sangam-io/src/main.rs
@@ -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");
diff --git a/slam-test/dhruva-test.toml b/slam-test/dhruva-test.toml
index 448e1f7..fd85d01 100644
--- a/slam-test/dhruva-test.toml
+++ b/slam-test/dhruva-test.toml
@@ -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"