Add A* path-planned exploration and hazard recovery system

Major enhancements to frontier exploration and navigation: Exploration: - Integrate A* planner for frontier reachability checking - Add recovery phases (backup/rotation) after bumper events - Track path history for reverse direction calculation - Detect "surrounded" state to avoid infinite stuck loops - Support edge-point fallback when centroid unreachable Navigation: - Add Escaping state for obstacle avoidance maneuvers - Back away from obstacles before attempting replan - Accept "close enough" tolerance for frontier targets - Enhanced waypoint progress logging Thread coordination: - NavigateTo action delegates to path-planned navigation - MoveTo action for direct control (recovery moves) - Proper success/failure callbacks between threads Drishti client: - Upgrade to dual TCP+UDP architecture - TCP for maps/registration, UDP for high-freq streams - Non-blocking I/O with select() for both sockets Also adds test map with obstacles and movement test scripts.
2025-12-17 21:32:30 +05:30
parent 70e7678fa1
commit 906c8e4ef1
28 changed files with 2795 additions and 283 deletions
@@ -21,6 +21,7 @@ serde_json = "1.0"  # JSON output for benchmarks
 sysinfo = "0.32"  # Memory monitoring for benchmarks
 image = { version = "0.25", default-features = false, features = ["png", "pnm"] }  # PNG and PGM export
 serde_yaml = "0.9"  # YAML for ROS-standard map metadata
+socket2 = { version = "0.5", features = ["all"] }     # For SO_REUSEADDR on UDP sockets
 thiserror = "2.0"
 basic-toml = "0.1"  # Minimal TOML parser

@@ -265,10 +265,11 @@ enum NavState {
  NAV_STATE_IDLE = 0;                 // No active navigation
  NAV_STATE_PLANNING = 1;             // Computing path
  NAV_STATE_NAVIGATING = 2;           // Following path
-  NAV_STATE_ROTATING_TO_HEADING = 3;  // Rotating to final heading at target
-  NAV_STATE_REACHED = 4;              // Goal reached
-  NAV_STATE_FAILED = 5;               // Navigation failed
-  NAV_STATE_CANCELLED = 6;            // Navigation cancelled by user
+  NAV_STATE_ESCAPING = 3;             // Backing away from obstacle before replan
+  NAV_STATE_ROTATING_TO_HEADING = 4;  // Rotating to final heading at target
+  NAV_STATE_REACHED = 5;              // Goal reached
+  NAV_STATE_FAILED = 6;               // Navigation failed
+  NAV_STATE_CANCELLED = 7;            // Navigation cancelled by user
 }

 // ----------------------------------------------------------------------------
@@ -24,6 +24,12 @@ pub struct AStarConfig {
    /// can pass through without collision.
    pub robot_radius: f32,

+    /// Additional safety margin beyond robot radius (meters).
+    ///
+    /// Adds extra clearance to prevent getting too close to walls.
+    /// Total inflation = robot_radius + safety_margin.
+    pub safety_margin: f32,
+
    /// Allow diagonal movement (8-connected vs 4-connected grid).
    pub allow_diagonal: bool,

@@ -48,7 +54,8 @@ pub struct AStarConfig {
 impl Default for AStarConfig {
    fn default() -> Self {
        Self {
-            robot_radius: 0.18, // CRL-200S robot radius
+            robot_radius: 0.18,  // CRL-200S robot radius
+            safety_margin: 0.05, // 5cm extra clearance from walls
            allow_diagonal: true,
            max_iterations: 100_000,
            simplification_tolerance: 0.05, // 5cm
@@ -260,8 +267,9 @@ impl AStarPlanner {
        self.origin_x = map.origin_x;
        self.origin_y = map.origin_y;

-        // Calculate inflation in cells
-        self.inflation_cells = (self.config.robot_radius / map.resolution).ceil() as i32;
+        // Calculate inflation in cells (robot_radius + safety_margin)
+        let total_inflation = self.config.robot_radius + self.config.safety_margin;
+        self.inflation_cells = (total_inflation / map.resolution).ceil() as i32;

        // Start with all cells free
        let total_cells = self.grid_width * self.grid_height;
@@ -275,8 +283,11 @@ impl AStarPlanner {
                let cell_value = map.cells[idx];

                // Check if obstacle or unknown (depending on config)
-                let is_obstacle =
-                    cell_value >= 100 || (self.config.unknown_is_obstacle && cell_value == 255);
+                // Note: occupied = 100, unknown = 255, free = 0
+                // Cell is occupied if value >= 100 BUT NOT if it's unknown (255)
+                let is_occupied = cell_value >= 100 && cell_value != 255;
+                let is_unknown_obstacle = cell_value == 255 && self.config.unknown_is_obstacle;
+                let is_obstacle = is_occupied || is_unknown_obstacle;

                if is_obstacle {
                    // Inflate: mark surrounding cells as blocked
@@ -153,6 +153,24 @@ pub struct OnlineSlamConfig {

    /// Loop closure configuration.
    pub loop_closure: LoopClosureConfig,
+
+    /// Maximum allowed rotation deviation between scan match and odometry (radians).
+    ///
+    /// If the scan matcher returns a rotation that differs from odometry by more
+    /// than this threshold, the match is rejected and odometry is used instead.
+    /// This prevents false positive matches in symmetric environments (e.g.,
+    /// rectangular rooms where walls can match at 90° intervals).
+    ///
+    /// Default: 0.35 radians (~20 degrees)
+    pub max_theta_deviation: f32,
+
+    /// Maximum allowed translation deviation between scan match and odometry (meters).
+    ///
+    /// If the scan matcher returns a translation that differs from odometry by more
+    /// than this threshold, the match is rejected and odometry is used instead.
+    ///
+    /// Default: 0.5 meters
+    pub max_translation_deviation: f32,
 }

 impl Default for OnlineSlamConfig {
@@ -172,6 +190,9 @@ impl Default for OnlineSlamConfig {
            use_submap_matching: false,
            min_scan_points: 50,
            loop_closure: LoopClosureConfig::default(),
+            // Sanity check thresholds to reject false positive matches
+            max_theta_deviation: 0.35,      // ~20 degrees
+            max_translation_deviation: 0.5, // 50cm
        }
    }
 }
@@ -577,6 +598,37 @@ impl OnlineSlam {
    fn apply_scan_match(&mut self, match_result: &ScanMatchResult, odom_delta: &Pose2D) -> Pose2D {
        let odom_pose = self.current_pose.compose(odom_delta);

+        // Sanity check: reject matches that deviate too much from odometry.
+        // This prevents false positive matches in symmetric environments where
+        // the correlative matcher might find high-scoring matches at wrong orientations
+        // (e.g., 90° off in a rectangular room where walls match walls).
+        let theta_diff =
+            crate::core::math::normalize_angle(match_result.transform.theta - odom_delta.theta)
+                .abs();
+        let translation_diff = ((match_result.transform.x - odom_delta.x).powi(2)
+            + (match_result.transform.y - odom_delta.y).powi(2))
+        .sqrt();
+
+        if theta_diff > self.config.max_theta_deviation {
+            log::warn!(
+                "Rejecting scan match: rotation deviation {:.1}° exceeds threshold {:.1}° (score={:.3})",
+                theta_diff.to_degrees(),
+                self.config.max_theta_deviation.to_degrees(),
+                match_result.score
+            );
+            return odom_pose;
+        }
+
+        if translation_diff > self.config.max_translation_deviation {
+            log::warn!(
+                "Rejecting scan match: translation deviation {:.3}m exceeds threshold {:.3}m (score={:.3})",
+                translation_diff,
+                self.config.max_translation_deviation,
+                match_result.score
+            );
+            return odom_pose;
+        }
+
        // Compute scan-match-corrected pose:
        // match_result.transform is the robot motion from previous to current.
        // Apply it directly to get the new pose.
@@ -84,13 +84,20 @@ impl HazardEvent {
 /// Result of exploration planning.
 #[derive(Debug, Clone)]
 pub enum ExplorationAction {
-    /// Move toward target pose.
+    /// Move toward target pose (direct line motion, for hazard recovery).
    MoveTo {
        /// Target pose to navigate to.
        target: Pose2D,
        /// Human-readable reason for this target.
        reason: &'static str,
    },
+    /// Navigate to target using A* path planning (for frontier exploration).
+    NavigateTo {
+        /// Target pose to navigate to via planned path.
+        target: Pose2D,
+        /// Human-readable reason for this target.
+        reason: &'static str,
+    },
    /// Rotate in place to scan surroundings.
    RotateInPlace {
        /// Angle to rotate (radians, positive = CCW).
@@ -142,4 +149,16 @@ pub trait ExplorationStrategy: Send + Sync {
    fn completion_percent(&self) -> f32 {
        0.0
    }
+
+    /// Mark a target location as blocked (navigation failed to reach it).
+    /// This allows the strategy to avoid selecting this location again.
+    fn mark_target_blocked(&mut self, _x: f32, _y: f32) {
+        // Default implementation does nothing
+    }
+
+    /// Mark current target as successfully reached.
+    /// Called when navigation completes successfully to clear internal target state.
+    fn mark_target_reached(&mut self) {
+        // Default implementation does nothing
+    }
 }
@@ -44,8 +44,9 @@

 use crossbeam_channel::{Receiver, Sender, bounded};
 use prost::Message as ProstMessage;
+use socket2::{Domain, Protocol, Socket, Type};
 use std::collections::HashMap;
-use std::net::UdpSocket;
+use std::net::{SocketAddr, UdpSocket};
 use std::sync::Arc;
 use std::sync::atomic::{AtomicBool, Ordering};
 use std::time::Duration;
@@ -149,7 +150,23 @@ impl SangamUdpReceiver {
        config: ReceiverConfig,
        running: Arc<AtomicBool>,
    ) -> Result<(Self, Receiver<SensorStatusData>, Receiver<LidarData>)> {
-        let socket = UdpSocket::bind(&config.bind_addr)?;
+        // Use socket2 to set SO_REUSEADDR before binding
+        // This is needed for localhost testing where TCP and UDP share the same port
+        let addr: SocketAddr = config.bind_addr.parse().map_err(|e| {
+            ReceiverError::Io(std::io::Error::new(
+                std::io::ErrorKind::InvalidInput,
+                format!("Invalid bind address: {}", e),
+            ))
+        })?;
+
+        let socket2 = Socket::new(Domain::IPV4, Type::DGRAM, Some(Protocol::UDP))?;
+        socket2.set_reuse_address(true)?;
+        #[cfg(target_os = "macos")]
+        socket2.set_reuse_port(true)?;
+        socket2.bind(&addr.into())?;
+
+        // Convert to std UdpSocket
+        let socket: UdpSocket = socket2.into();

        // Set non-blocking with short timeout for shutdown checks
        socket.set_read_timeout(Some(Duration::from_millis(100)))?;
@@ -158,7 +175,10 @@ impl SangamUdpReceiver {
        let (sensor_tx, sensor_rx) = bounded(SENSOR_CHANNEL_CAPACITY);
        let (lidar_tx, lidar_rx) = bounded(LIDAR_CHANNEL_CAPACITY);

-        log::info!("UDP receiver bound to {}", config.bind_addr);
+        log::info!(
+            "UDP receiver bound to {} (SO_REUSEADDR enabled)",
+            config.bind_addr
+        );

        Ok((
            Self {
@@ -179,6 +199,8 @@ impl SangamUdpReceiver {
        log::info!("UDP receiver started");

        let mut buffer = vec![0u8; MAX_DATAGRAM_SIZE];
+        let mut packet_count: u64 = 0;
+        let mut last_log_time = std::time::Instant::now();

        while self.running.load(Ordering::Relaxed) {
            // Receive UDP datagram
@@ -192,6 +214,18 @@ impl SangamUdpReceiver {
                }
            };

+            // Track reception rate
+            packet_count += 1;
+            if last_log_time.elapsed().as_secs() >= 5 {
+                log::info!(
+                    "UDP receiver: {} packets in last 5s ({})",
+                    packet_count,
+                    _src
+                );
+                packet_count = 0;
+                last_log_time = std::time::Instant::now();
+            }
+
            // Parse length-prefixed message
            if len < 4 {
                log::warn!("UDP datagram too short: {} bytes", len);
@@ -233,12 +267,15 @@ impl SangamUdpReceiver {
                }
                "lidar" => {
                    if let Some(data) = Self::parse_lidar(&sensor_group) {
+                        log::info!("Lidar scan received: {} points", data.points.len());
                        // Non-blocking send - drop if channel full
                        self.lidar_tx.try_send(data).ok();
+                    } else {
+                        log::warn!("Failed to parse lidar data from sensor group");
                    }
                }
                other => {
-                    log::trace!("Ignoring unknown group_id: {}", other);
+                    log::debug!("Ignoring unknown group_id: {}", other);
                }
            }
        }
@@ -112,6 +112,18 @@ impl DhruvaUdpPublisher {
        log::info!("UDP client registered: {}", client_addr);
    }

+    /// Check if a client is already registered for UDP streaming.
+    pub fn has_client(registry: &UdpClientRegistry) -> bool {
+        let guard = registry.lock().unwrap_or_else(|e| e.into_inner());
+        guard.is_some()
+    }
+
+    /// Get the currently registered client address.
+    pub fn get_registered_client(registry: &UdpClientRegistry) -> Option<SocketAddr> {
+        let guard = registry.lock().unwrap_or_else(|e| e.into_inner());
+        *guard
+    }
+
    /// Unregister the current client.
    ///
    /// Called when TCP client disconnects.
@@ -302,6 +314,7 @@ fn build_navigation_status_message(
        NavState::Idle => ProtoNavState::Idle,
        NavState::Planning => ProtoNavState::Planning,
        NavState::Navigating => ProtoNavState::Navigating,
+        NavState::Escaping => ProtoNavState::Escaping,
        NavState::RotatingToHeading => ProtoNavState::RotatingToHeading,
        NavState::TargetReached => ProtoNavState::Reached,
        NavState::Failed => ProtoNavState::Failed,
@@ -174,12 +174,17 @@ struct SourceConfig {
    /// Format: "host:port" (e.g., "192.168.68.101:5555")
    /// Both TCP and UDP use the same port for simpler configuration.
    sangam_address: String,
+    /// UDP port override for sensor streaming (optional).
+    /// If specified, UDP receiver will bind to this port instead of the TCP port.
+    /// Useful for localhost testing where TCP and UDP need different ports.
+    udp_port: Option<u16>,
 }

 impl Default for SourceConfig {
    fn default() -> Self {
        Self {
            sangam_address: "192.168.68.101:5555".to_string(),
+            udp_port: None,
        }
    }
 }
@@ -276,6 +281,10 @@ struct SlamConfig {
    min_scan_points: usize,
    encoder_weight: f32,
    use_submap_matching: bool,
+    /// Maximum rotation deviation from odometry before rejecting scan match (radians)
+    max_theta_deviation: f32,
+    /// Maximum translation deviation from odometry before rejecting scan match (meters)
+    max_translation_deviation: f32,
 }

 impl Default for SlamConfig {
@@ -286,6 +295,8 @@ impl Default for SlamConfig {
            min_scan_points: 50,
            encoder_weight: 0.8,
            use_submap_matching: false,
+            max_theta_deviation: 0.35,      // ~20 degrees
+            max_translation_deviation: 0.5, // 50cm
        }
    }
 }
@@ -642,6 +653,9 @@ struct NavigationConfig {
    goal_threshold: f32,
    /// Heading threshold (rad).
    heading_threshold: f32,
+    /// Treat unknown cells as obstacles for path planning.
+    /// Set to false to allow navigation through unexplored areas.
+    unknown_is_obstacle: bool,
 }

 impl Default for NavigationConfig {
@@ -655,6 +669,7 @@ impl Default for NavigationConfig {
            waypoint_threshold: 0.15,
            goal_threshold: 0.08,
            heading_threshold: 0.15,
+            unknown_is_obstacle: true,
        }
    }
 }
@@ -867,6 +882,8 @@ fn build_slam_config(config: &Config) -> OnlineSlamConfig {
        lost_threshold: config.slam.lost_threshold,
        use_submap_matching: config.slam.use_submap_matching,
        min_scan_points: config.slam.min_scan_points,
+        max_theta_deviation: config.slam.max_theta_deviation,
+        max_translation_deviation: config.slam.max_translation_deviation,
        loop_closure: SlamLoopClosureConfig {
            enabled: config.loop_closure.enabled,
            detection_interval: config.loop_closure.detection_interval,
@@ -1040,6 +1057,7 @@ fn run_threaded_mode(

    let slam_thread_config = SlamThreadConfig {
        sangam_address: config.source.sangam_address.clone(),
+        udp_port: config.source.udp_port,
        slam_config,
        odometry_type: config.odometry.algorithm_type(),
        odometry_config,
@@ -1108,6 +1126,7 @@ fn run_threaded_mode(
            navigator: NavigatorConfig {
                planner: AStarConfig {
                    robot_radius: config.navigation.robot_radius,
+                    unknown_is_obstacle: config.navigation.unknown_is_obstacle,
                    ..Default::default()
                },
                waypoint_reached_threshold: config.navigation.waypoint_threshold,
@@ -1172,7 +1191,7 @@ 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,
-                max_stuck_count: 10, // Increased to allow more exploration attempts
+                ..FrontierConfig::default()
            },
            loop_rate_hz: config.exploration.loop_rate_hz,
        };
@@ -63,6 +63,19 @@ pub struct NavigatorConfig {

    /// Minimum distance from path before replanning (meters).
    pub replan_deviation_threshold: f32,
+
+    /// Escape duration (seconds).
+    ///
+    /// How long to back away from obstacle before replanning.
+    pub escape_duration_s: f32,
+
+    /// Escape velocity (m/s).
+    ///
+    /// Speed at which to back away from obstacles (negative = backward).
+    pub escape_velocity: f32,
+
+    /// Minimum clearance to achieve during escape (meters).
+    pub escape_clearance_m: f32,
 }

 impl Default for NavigatorConfig {
@@ -76,9 +89,12 @@ impl Default for NavigatorConfig {
            max_angular_vel: 0.5,
            linear_vel: 0.2,
            angular_vel: 0.3,
-            stuck_timeout_s: 10.0,
+            stuck_timeout_s: 5.0,
            max_replan_attempts: 3,
            replan_deviation_threshold: 0.3,
+            escape_duration_s: 1.5,
+            escape_velocity: -0.1, // Negative = backward
+            escape_clearance_m: 0.15,
        }
    }
 }
@@ -175,11 +191,17 @@ impl Navigator {
        map: Option<&CurrentMapData>,
        hazard: bool,
    ) -> NavUpdate {
-        // Handle hazard - stop and request replan
+        // Handle hazard - escape before replanning
        if hazard && nav_state.is_navigating() {
-            log::warn!("Hazard detected during navigation, replanning...");
-            nav_state.request_replan();
-            return NavUpdate::stop().with_state_change();
+            log::warn!("Hazard detected during navigation, escaping before replan...");
+            // Calculate escape direction (opposite of current heading = backward)
+            let escape_dir = pose.theta + std::f32::consts::PI;
+            nav_state.start_escape(escape_dir);
+            return NavUpdate {
+                linear_vel: self.config.escape_velocity,
+                angular_vel: 0.0,
+                state_changed: true,
+            };
        }

        match nav_state.nav_state {
@@ -198,6 +220,11 @@ impl Navigator {
                self.handle_navigating(nav_state, pose)
            }

+            NavState::Escaping => {
+                // Back away from obstacle before replanning
+                self.handle_escaping(nav_state, pose)
+            }
+
            NavState::RotatingToHeading => {
                // Rotate to final heading
                self.handle_rotating_to_heading(nav_state, pose)
@@ -212,6 +239,22 @@ impl Navigator {
        pose: &Pose2D,
        map: Option<&CurrentMapData>,
    ) -> NavUpdate {
+        // Check if we've exceeded max replan attempts
+        if nav_state.replan_attempts >= self.config.max_replan_attempts {
+            log::error!(
+                "Navigation failed: exceeded {} replan attempts",
+                self.config.max_replan_attempts
+            );
+            nav_state.set_failed(PathFailureReason::PathBlocked {
+                replan_attempts: nav_state.replan_attempts,
+            });
+            nav_state.set_status(format!(
+                "Path blocked after {} attempts",
+                nav_state.replan_attempts
+            ));
+            return NavUpdate::stop().with_state_change();
+        }
+
        // Get target info (clone what we need to avoid borrow issues)
        let (target_id, goal_x, goal_y, description) = match nav_state.current_target() {
            Some(t) => (t.id, t.position.x, t.position.y, t.description.clone()),
@@ -331,10 +374,15 @@ impl Navigator {
        }

        // Get current waypoint
-        let waypoint = match nav_state.current_waypoint() {
-            Some(w) => *w,
+        let (waypoint, waypoint_idx, total_waypoints) = match nav_state.current_waypoint() {
+            Some(w) => {
+                let idx = nav_state.current_waypoint_index;
+                let total = nav_state.current_path().map(|p| p.len()).unwrap_or(0);
+                (*w, idx, total)
+            }
            None => {
                // No more waypoints but not at goal - replan
+                log::debug!("NAV: No waypoints available, requesting replan");
                nav_state.request_replan();
                return NavUpdate::stop().with_state_change();
            }
@@ -345,6 +393,19 @@ impl Navigator {
        let dy = waypoint.y - pose.y;
        let dist_to_waypoint = (dx * dx + dy * dy).sqrt();

+        log::debug!(
+            "NAV: Waypoint {}/{} at ({:.2},{:.2}), robot at ({:.2},{:.2},{:.1}°), dist={:.2}m, threshold={:.2}m",
+            waypoint_idx + 1,
+            total_waypoints,
+            waypoint.x,
+            waypoint.y,
+            pose.x,
+            pose.y,
+            pose.theta.to_degrees(),
+            dist_to_waypoint,
+            self.config.waypoint_reached_threshold
+        );
+
        if dist_to_waypoint < self.config.waypoint_reached_threshold {
            // Advance to next waypoint
            if !nav_state.advance_waypoint() {
@@ -356,6 +417,26 @@ impl Navigator {
                    }
                    return self.complete_target(nav_state);
                }
+
+                // 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.
+                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
+
+                if goal_dist < close_enough_threshold {
+                    log::info!(
+                        "Path complete, accepting position as 'close enough' ({:.2}m from goal)",
+                        goal_dist
+                    );
+                    if target.heading.is_some() && !target.is_heading_reached(pose.theta) {
+                        nav_state.nav_state = NavState::RotatingToHeading;
+                        return NavUpdate::stop().with_state_change();
+                    }
+                    return self.complete_target(nav_state);
+                }
            }

            // Get new current waypoint
@@ -397,6 +478,41 @@ impl Navigator {
        self.compute_velocity_to_waypoint(pose, &waypoint, &target)
    }

+    /// Handle the Escaping state.
+    ///
+    /// Robot backs away from obstacle for a duration before replanning.
+    fn handle_escaping(&mut self, nav_state: &mut NavigationState, pose: &Pose2D) -> NavUpdate {
+        let elapsed = nav_state.escape_elapsed_secs();
+
+        // Check if escape complete (duration elapsed)
+        if elapsed >= self.config.escape_duration_s {
+            log::info!(
+                "Escape complete (elapsed={:.1}s), transitioning to replan",
+                elapsed
+            );
+            nav_state.finish_escape_and_replan();
+
+            // Reset stuck detection for the replan
+            self.last_position = Some((pose.x, pose.y));
+            self.last_progress_time = Instant::now();
+
+            return NavUpdate::stop().with_state_change();
+        }
+
+        // Continue backing up
+        log::debug!(
+            "Escaping: {:.1}s / {:.1}s",
+            elapsed,
+            self.config.escape_duration_s
+        );
+
+        NavUpdate {
+            linear_vel: self.config.escape_velocity,
+            angular_vel: 0.0,
+            state_changed: false,
+        }
+    }
+
    /// Handle the RotatingToHeading state.
    fn handle_rotating_to_heading(
        &mut self,
@@ -487,6 +603,18 @@ impl Navigator {
        if adjusted_angle_error.abs() > self.config.heading_threshold {
            // Need to rotate first
            let angular_vel = adjusted_angle_error.signum() * self.config.angular_vel;
+            log::debug!(
+                "NAV: Rotating to align - pose=({:.2},{:.2},{:.1}°) waypoint=({:.2},{:.2}) dist={:.2}m angle_err={:.1}° threshold={:.1}° -> angular={:.2}",
+                pose.x,
+                pose.y,
+                pose.theta.to_degrees(),
+                waypoint.x,
+                waypoint.y,
+                distance,
+                adjusted_angle_error.to_degrees(),
+                self.config.heading_threshold.to_degrees(),
+                angular_vel
+            );
            return NavUpdate {
                linear_vel: 0.0,
                angular_vel: angular_vel
@@ -500,6 +628,18 @@ impl Navigator {
        let angular_vel =
            (adjusted_angle_error * 0.5).clamp(-self.config.angular_vel, self.config.angular_vel);

+        log::debug!(
+            "NAV: Moving forward - pose=({:.2},{:.2},{:.1}°) waypoint=({:.2},{:.2}) dist={:.2}m -> linear={:.2} angular={:.2}",
+            pose.x,
+            pose.y,
+            pose.theta.to_degrees(),
+            waypoint.x,
+            waypoint.y,
+            distance,
+            linear_vel,
+            angular_vel
+        );
+
        NavUpdate {
            linear_vel: linear_vel.clamp(-self.config.max_linear_vel, self.config.max_linear_vel),
            angular_vel: angular_vel
@@ -587,7 +727,7 @@ mod tests {
    }

    #[test]
-    fn test_navigator_hazard_stops() {
+    fn test_navigator_hazard_triggers_escape() {
        let mut navigator = Navigator::new(NavigatorConfig::default());
        let mut nav_state = NavigationState::new();
        let pose = Pose2D::new(0.0, 0.0, 0.0);
@@ -596,13 +736,14 @@ mod tests {
        nav_state.push_target(make_test_target(1.0, 1.0, None));
        nav_state.nav_state = NavState::Navigating;

-        // Hazard detected
+        // Hazard detected - should trigger escape (backing up)
        let update = navigator.update(&mut nav_state, &pose, None, true);

-        assert_eq!(update.linear_vel, 0.0);
+        // Robot should back up (negative velocity) and enter Escaping state
+        assert_eq!(update.linear_vel, navigator.config.escape_velocity);
        assert_eq!(update.angular_vel, 0.0);
        assert!(update.state_changed);
-        assert!(nav_state.needs_replan());
+        assert_eq!(nav_state.nav_state, NavState::Escaping);
    }

    #[test]
@@ -8,6 +8,7 @@
 use super::{NavTarget, NavTargetSource, Path};
 use serde::{Deserialize, Serialize};
 use std::collections::VecDeque;
+use std::time::Instant;

 /// Navigation execution state.
 #[derive(Debug, Clone, Copy, PartialEq, Eq, Default, Serialize, Deserialize)]
@@ -22,6 +23,10 @@ pub enum NavState {
    /// Following path to target.
    Navigating,

+    /// Escaping from obstacle before replanning.
+    /// Robot backs away from wall/obstacle to get clearance.
+    Escaping,
+
    /// Rotating to final heading at target.
    RotatingToHeading,

@@ -40,7 +45,10 @@ impl NavState {
    pub fn is_active(&self) -> bool {
        matches!(
            self,
-            NavState::Planning | NavState::Navigating | NavState::RotatingToHeading
+            NavState::Planning
+                | NavState::Navigating
+                | NavState::Escaping
+                | NavState::RotatingToHeading
        )
    }

@@ -50,6 +58,7 @@ impl NavState {
            NavState::Idle => "IDLE",
            NavState::Planning => "PLANNING",
            NavState::Navigating => "NAVIGATING",
+            NavState::Escaping => "ESCAPING",
            NavState::RotatingToHeading => "ROTATING_TO_HEADING",
            NavState::TargetReached => "TARGET_REACHED",
            NavState::Failed => "FAILED",
@@ -150,6 +159,12 @@ pub struct NavigationState {

    /// Flag indicating a replan is needed.
    needs_replan: bool,
+
+    /// Time when escape maneuver started (for timeout).
+    pub escape_start_time: Option<Instant>,
+
+    /// Direction to escape (radians, in world frame).
+    pub escape_direction: f32,
 }

 impl NavigationState {
@@ -364,6 +379,48 @@ impl NavigationState {
        self.needs_replan = false;
    }

+    // ========================================================================
+    // Escape Operations
+    // ========================================================================
+
+    /// Start an escape maneuver (backing away from obstacle).
+    ///
+    /// Called when hazard is detected during navigation.
+    /// Robot will back away before attempting to replan.
+    pub fn start_escape(&mut self, direction: f32) {
+        self.nav_state = NavState::Escaping;
+        self.escape_start_time = Some(Instant::now());
+        self.escape_direction = direction;
+        self.status_message = "Escaping from obstacle".to_string();
+        log::info!("Starting escape maneuver, direction={:.2} rad", direction);
+    }
+
+    /// Complete escape and transition to replanning.
+    ///
+    /// Called when escape timeout or clearance achieved.
+    pub fn finish_escape_and_replan(&mut self) {
+        self.escape_start_time = None;
+        self.replan_attempts += 1;
+        self.needs_replan = true;
+        self.nav_state = NavState::Planning;
+        log::info!(
+            "Escape complete, transitioning to replan (attempt {})",
+            self.replan_attempts
+        );
+    }
+
+    /// Check if currently escaping.
+    pub fn is_escaping(&self) -> bool {
+        self.nav_state == NavState::Escaping
+    }
+
+    /// Get elapsed time since escape started.
+    pub fn escape_elapsed_secs(&self) -> f32 {
+        self.escape_start_time
+            .map(|t| t.elapsed().as_secs_f32())
+            .unwrap_or(0.0)
+    }
+
    // ========================================================================
    // Dirty Flag Operations
    // ========================================================================
@@ -456,6 +513,8 @@ impl NavigationState {
        self.targets_completed = 0;
        self.replan_attempts = 0;
        self.needs_replan = false;
+        self.escape_start_time = None;
+        self.escape_direction = 0.0;
        self.targets_dirty = true;
        self.path_dirty = true;
    }
@@ -694,6 +753,7 @@ mod tests {
        assert!(!NavState::Idle.is_active());
        assert!(NavState::Planning.is_active());
        assert!(NavState::Navigating.is_active());
+        assert!(NavState::Escaping.is_active());
        assert!(NavState::RotatingToHeading.is_active());
        assert!(!NavState::TargetReached.is_active());
        assert!(!NavState::Failed.is_active());
@@ -18,6 +18,8 @@ use crate::exploration::{
 use crate::io::motion_controller::{
    MotionConfig, SharedMotionController, compute_velocity_to_target,
 };
+use crate::navigation::NavState;
+use crate::navigation::NavTarget;
 use crate::state::{CliffState, ExplorationMode, SharedStateHandle};

 /// State for initial scanning behavior when no map exists yet.
@@ -207,17 +209,67 @@ fn run_exploration_loop(
        }

        // Check if navigation has active targets - yield to navigation
-        let nav_has_targets = {
+        let (nav_has_targets, nav_state, current_target_pos) = {
            match shared_state.read() {
-                Ok(s) => s.navigation_state.has_targets(),
-                Err(_) => false,
+                Ok(s) => {
+                    let target_pos = s
+                        .navigation_state
+                        .current_target()
+                        .map(|t| (t.position.x, t.position.y));
+                    (
+                        s.navigation_state.has_targets(),
+                        s.navigation_state.nav_state,
+                        target_pos,
+                    )
+                }
+                Err(_) => (false, NavState::Idle, None),
            }
        };

+        // Handle navigation failure - clear target and notify strategy
+        if nav_state == NavState::Failed && nav_has_targets {
+            log::warn!("Navigation failed for frontier target, clearing and marking blocked");
+
+            // Get and notify strategy about blocked target before clearing
+            if let Some((tx, ty)) = current_target_pos {
+                strategy.mark_target_blocked(tx, ty);
+            }
+
+            // Clear the failed target from navigation state
+            if let Ok(mut state) = shared_state.write() {
+                state.navigation_state.clear_targets();
+                state.exploration_state.status = "Frontier blocked, finding new target".to_string();
+            }
+
+            // Continue to next iteration - exploration will pick a new frontier
+            thread::sleep(loop_duration);
+            continue;
+        }
+
+        // Handle navigation success - notify strategy that target was reached
+        // This happens when navigation completed but we still yielded last iteration
+        if nav_state == NavState::TargetReached && !nav_has_targets {
+            log::info!("Navigation completed successfully, notifying exploration strategy");
+
+            // Notify strategy that target was reached (clears internal state)
+            strategy.mark_target_reached();
+
+            // Clear the TargetReached state
+            if let Ok(mut state) = shared_state.write() {
+                state.navigation_state.nav_state = NavState::Idle;
+                state.exploration_state.status = "Frontier reached, scanning...".to_string();
+            }
+
+            // Continue to next iteration - exploration will pick a new frontier
+            thread::sleep(loop_duration);
+            continue;
+        }
+
        // Only explore if in exploration mode AND navigation is not active
        if exploration_mode != ExplorationMode::Exploring || nav_has_targets {
-            // Disable motors and lidar if we were exploring
-            if motion_enabled || lidar_enabled {
+            // Only disable motors/lidar if exploration mode changed to Disabled
+            // When yielding to navigation (nav_has_targets), keep motors enabled!
+            if exploration_mode == ExplorationMode::Disabled && (motion_enabled || lidar_enabled) {
                let mut mc = motion_controller.lock().unwrap();
                if motion_enabled {
                    if let Err(e) = mc.disable() {
@@ -231,16 +283,12 @@ fn run_exploration_loop(
                    }
                    lidar_enabled = false;
                }
-            }
-
-            // Check if we need to reset strategy (mode changed)
-            if exploration_mode == ExplorationMode::Disabled {
                strategy.reset();
                // Reset initial scan state for next exploration session
                initial_scan_state = InitialScanState::NotStarted;
            }

-            // If yielding to navigation, update status
+            // If yielding to navigation, update status (but keep motors enabled!)
            if nav_has_targets
                && exploration_mode == ExplorationMode::Exploring
                && let Ok(mut state) = shared_state.write()
@@ -377,7 +425,13 @@ fn run_exploration_loop(
        // Execute action
        match action {
            ExplorationAction::MoveTo { target, reason } => {
-                log::debug!("Moving to ({:.2}, {:.2}) - {}", target.x, target.y, reason);
+                // Direct motion - used for hazard recovery (backing up, etc.)
+                log::debug!(
+                    "Direct move to ({:.2}, {:.2}) - {}",
+                    target.x,
+                    target.y,
+                    reason
+                );

                // Compute velocity to reach target
                let (linear, angular) = compute_velocity_to_target(
@@ -400,7 +454,36 @@ fn run_exploration_loop(
                // Update shared state
                if let Ok(mut state) = shared_state.write() {
                    state.exploration_state.current_target = Some(target);
-                    state.exploration_state.status = format!("Exploring: {}", reason);
+                    state.exploration_state.status = format!("Recovery: {}", reason);
+                }
+            }
+
+            ExplorationAction::NavigateTo { target, reason } => {
+                // Path-planned navigation - delegate to navigation system
+                log::info!(
+                    "Navigating to frontier at ({:.2}, {:.2}) - {}",
+                    target.x,
+                    target.y,
+                    reason
+                );
+
+                // Create a frontier target for the navigation system
+                let nav_target = NavTarget::frontier(target.x, target.y);
+
+                // Push target to navigation state (navigation thread will handle path planning)
+                if let Ok(mut state) = shared_state.write() {
+                    // Clear any existing targets first to prevent stale targets
+                    state.navigation_state.clear_targets();
+                    state.navigation_state.push_target(nav_target);
+
+                    state.exploration_state.current_target = Some(target);
+                    state.exploration_state.status = format!("Navigating: {}", reason);
+                }
+
+                // Set zero velocity - navigation thread will take over control
+                // DON'T use emergency_stop() as it disables motors
+                if let Err(e) = motion_controller.lock().unwrap().set_velocity(0.0, 0.0) {
+                    log::warn!("Failed to stop before navigation handoff: {}", e);
                }
            }

@@ -177,7 +177,7 @@ fn run_navigation_loop(
        }

        // Read current state
-        let (pose, map, hazard, nav_active, is_charging, lidar_scan, current_target) = {
+        let (pose, map, hazard, nav_active, is_charging, lidar_scan, current_target, slam_mapping) = {
            match shared_state.read() {
                Ok(state) => {
                    let pose = state.robot_status.pose;
@@ -187,6 +187,9 @@ fn run_navigation_loop(
                        || state.sensor_status.cliff.any_triggered();
                    let nav_active = state.navigation_state.is_active();
                    let is_charging = state.robot_status.is_charging;
+                    // Check if SLAM is in mapping mode (set by StartMapping command)
+                    let slam_mapping =
+                        state.robot_status.state == crate::state::RobotState::Mapping;

                    // Create LaserScan from sensor status if available
                    let lidar_scan = if !state.sensor_status.lidar_ranges.is_empty() {
@@ -213,6 +216,7 @@ fn run_navigation_loop(
                        is_charging,
                        lidar_scan,
                        current_target,
+                        slam_mapping,
                    )
                }
                Err(e) => {
@@ -226,8 +230,9 @@ fn run_navigation_loop(
        // Check if mapping feature is active
        let mapping_active = mapping_feature.is_active();

-        // Handle motor/lidar enabling when navigation or mapping becomes active
-        if (nav_active || mapping_active) && motion_available {
+        // Handle motor/lidar enabling when navigation, mapping, or SLAM mapping becomes active
+        // slam_mapping is true when SLAM thread received StartMapping command (even without MappingFeature)
+        if (nav_active || mapping_active || slam_mapping) && motion_available {
            // Clear inactive timer
            inactive_since = None;

@@ -272,17 +277,28 @@ fn run_navigation_loop(
        }

        // Handle disabling when navigation becomes inactive
-        if !nav_active && !mapping_active && was_active {
+        // IMPORTANT: slam_mapping keeps motors enabled during SLAM mapping even without active navigation
+        if !nav_active && !mapping_active && !slam_mapping && was_active {
            // Start the inactive timer
            if inactive_since.is_none() {
                inactive_since = Some(Instant::now());
-                log::debug!("Navigation became inactive, starting disable timer");
+                log::debug!(
+                    "Navigation became inactive, starting disable timer (nav={}, mapping={}, slam={})",
+                    nav_active,
+                    mapping_active,
+                    slam_mapping
+                );
            }
+        } else if slam_mapping && inactive_since.is_some() {
+            // Clear inactive timer if slam_mapping is active
+            log::debug!("Clearing inactive timer - SLAM mapping is active");
+            inactive_since = None;
        }

        // Disable motors/lidar after being inactive for a while
        if !nav_active
            && !mapping_active
+            && !slam_mapping
            && motion_available
            && let Some(since) = inactive_since
            && since.elapsed() >= IDLE_DISABLE_DELAY
@@ -292,14 +308,20 @@ fn run_navigation_loop(
                    log::warn!("Navigation: Failed to disable motors: {}", e);
                } else {
                    motors_enabled = false;
-                    log::info!("Navigation: Motors disabled after idle timeout");
+                    log::info!(
+                        "Navigation: Motors disabled after idle timeout (nav={}, mapping={}, slam={})",
+                        nav_active,
+                        mapping_active,
+                        slam_mapping
+                    );
                }
            }
            // Note: Keep lidar enabled - SLAM needs it for localization
            inactive_since = None;
        }

-        was_active = nav_active || mapping_active;
+        // Include slam_mapping in was_active to properly track activity during SLAM mapping
+        was_active = nav_active || mapping_active || slam_mapping;

        // Process mapping feature state machine
        if mapping_active {
@@ -121,7 +121,8 @@ impl Client {
                // Would block - client is slow, skip this message
                true
            }
-            Err(_) => {
+            Err(e) => {
+                log::info!("TCP client {} write error: {}", self.addr, e);
                self.failed = true;
                false
            }
@@ -147,18 +148,24 @@ impl Client {
                }
                Ok(0) => {
                    // Connection closed
+                    log::info!(
+                        "TCP client {} connection closed (read header returned 0)",
+                        self.addr
+                    );
                    self.failed = true;
                    return None;
                }
-                Ok(_) => {
+                Ok(n) => {
                    // Partial read of length - shouldn't happen with TCP
+                    log::debug!("TCP client {} partial header read: {} bytes", self.addr, n);
                    return None;
                }
                Err(ref e) if e.kind() == std::io::ErrorKind::WouldBlock => {
                    // No data available
                    return None;
                }
-                Err(_) => {
+                Err(e) => {
+                    log::info!("TCP client {} read error: {}", self.addr, e);
                    self.failed = true;
                    return None;
                }
@@ -172,6 +179,10 @@ impl Client {
            match self.stream.read(&mut buf) {
                Ok(0) => {
                    // Connection closed
+                    log::info!(
+                        "TCP client {} connection closed (read body returned 0)",
+                        self.addr
+                    );
                    self.failed = true;
                    return None;
                }
@@ -182,7 +193,8 @@ impl Client {
                    // No data available
                    return None;
                }
-                Err(_) => {
+                Err(e) => {
+                    log::info!("TCP client {} body read error: {}", self.addr, e);
                    self.failed = true;
                    return None;
                }
@@ -284,14 +296,21 @@ fn run_publisher_loop(
    while running.load(Ordering::Relaxed) {
        // Accept new TCP clients (non-blocking)
        while let Ok((stream, addr)) = listener.accept() {
-            log::info!(
-                "TCP client connected: {} - registering for UDP streaming",
-                addr
-            );
-
-            // Register client for UDP streaming (single client model)
-            // Note: New client replaces any existing UDP registration
-            DhruvaUdpPublisher::register_client(&udp_registry, addr);
+            // Only register for UDP streaming if no client is currently registered
+            // This prevents command-only TCP connections from stealing the UDP stream
+            // from an existing pose monitor client
+            if DhruvaUdpPublisher::has_client(&udp_registry) {
+                log::info!(
+                    "TCP client connected: {} (UDP already registered, not replacing)",
+                    addr
+                );
+            } else {
+                log::info!(
+                    "TCP client connected: {} - registering for UDP streaming",
+                    addr
+                );
+                DhruvaUdpPublisher::register_client(&udp_registry, addr);
+            }

            clients.push(Client::new(stream, addr));
        }
@@ -406,7 +425,7 @@ fn run_publisher_loop(
            publish_to_clients(&mut clients, &msg);
        }

-        // Remove disconnected/failed clients and unregister from UDP
+        // Remove disconnected/failed clients
        let client_count_before = clients.len();
        let failed_clients: Vec<_> = clients
            .iter()
@@ -417,14 +436,28 @@ fn run_publisher_loop(
        clients.retain(|c| !c.failed);

        if clients.len() < client_count_before {
-            // Unregister UDP for disconnected clients
-            for _addr in &failed_clients {
-                DhruvaUdpPublisher::unregister_client(&udp_registry);
+            // Check if the registered UDP client was one of the failed clients
+            let registered_udp_client = DhruvaUdpPublisher::get_registered_client(&udp_registry);
+            let mut udp_unregistered = false;
+
+            if let Some(registered_addr) = registered_udp_client {
+                // Only unregister if the registered client failed
+                if failed_clients.contains(&registered_addr) {
+                    DhruvaUdpPublisher::unregister_client(&udp_registry);
+                    udp_unregistered = true;
+                }
            }
+
            log::info!(
-                "Removed {} disconnected TCP clients, {} remaining (UDP unregistered)",
+                "Removed {} disconnected TCP clients ({:?}), {} remaining{}",
                client_count_before - clients.len(),
-                clients.len()
+                failed_clients,
+                clients.len(),
+                if udp_unregistered {
+                    " (UDP unregistered)"
+                } else {
+                    ""
+                }
            );
        }

@@ -47,6 +47,10 @@ pub struct SlamThreadConfig {
    /// Format: "host:port" (e.g., "192.168.68.101:5555").
    /// Both TCP and UDP use the same port.
    pub sangam_address: String,
+    /// UDP port override for sensor streaming (optional).
+    /// If specified, UDP receiver will bind to this port instead of the TCP port.
+    /// Useful for localhost testing where TCP and UDP need different ports.
+    pub udp_port: Option<u16>,
    /// SLAM engine configuration.
    pub slam_config: OnlineSlamConfig,
    /// Odometry algorithm type.
@@ -108,22 +112,31 @@ fn run_live_loop(
 ) {
    log::info!("SLAM thread starting (live mode - UDP)");

-    // Extract port from sangam_address (same port for TCP commands and UDP sensors)
-    let udp_port = config
+    // Get UDP port: use explicit udp_port if set, otherwise extract from sangam_address
+    let tcp_port = config
        .sangam_address
        .rsplit(':')
        .next()
        .and_then(|p| p.parse::<u16>().ok())
        .unwrap_or(5555);
+    let udp_port = config.udp_port.unwrap_or(tcp_port);

    let receiver_config = ReceiverConfig {
        bind_addr: format!("0.0.0.0:{}", udp_port),
    };

-    log::info!(
-        "  Starting UDP receiver on port {} (same as TCP)...",
-        udp_port
-    );
+    if udp_port == tcp_port {
+        log::info!(
+            "  Starting UDP receiver on port {} (same as TCP)...",
+            udp_port
+        );
+    } else {
+        log::info!(
+            "  Starting UDP receiver on port {} (TCP on {})...",
+            udp_port,
+            tcp_port
+        );
+    }

    // Create UDP receiver
    let (receiver, sensor_rx, lidar_rx) =
@@ -562,9 +575,24 @@ impl SlamContext {

        // Skip sparse scans
        if scan_cloud.len() < 50 {
+            log::debug!("Skipping sparse scan: {} points", scan_cloud.len());
            return;
        }

+        // Log scan processing decision
+        if !self.is_mapping && !self.is_localizing {
+            static SKIP_COUNT: std::sync::atomic::AtomicU32 = std::sync::atomic::AtomicU32::new(0);
+            let count = SKIP_COUNT.fetch_add(1, std::sync::atomic::Ordering::Relaxed);
+            if count < 5 || count.is_multiple_of(50) {
+                log::info!(
+                    "Skipping scan (not mapping/localizing): is_mapping={}, is_localizing={}, skip#{}",
+                    self.is_mapping,
+                    self.is_localizing,
+                    count
+                );
+            }
+        }
+
        // Compute odometry delta since last scan
        let current_odom = self.odom_tracker.pose();
        let odom_delta = self.last_odom_pose.inverse().compose(&current_odom);
@@ -576,6 +604,23 @@ impl SlamContext {
                .slam
                .process_scan(&scan_cloud, &odom_delta, timestamp_us);

+            // Log first few pose updates
+            static POSE_LOG_COUNT: std::sync::atomic::AtomicU32 =
+                std::sync::atomic::AtomicU32::new(0);
+            let log_count = POSE_LOG_COUNT.fetch_add(1, std::sync::atomic::Ordering::Relaxed);
+            if log_count < 5 || log_count.is_multiple_of(50) {
+                log::info!(
+                    "SLAM result: pose=({:.3}, {:.3}, {:.1}°), score={:.3}, odom_delta=({:.3}, {:.3}, {:.1}°)",
+                    result.pose.x,
+                    result.pose.y,
+                    result.pose.theta.to_degrees(),
+                    result.match_score,
+                    odom_delta.x,
+                    odom_delta.y,
+                    odom_delta.theta.to_degrees()
+                );
+            }
+
            // Update shared state
            if let Ok(mut state) = shared_state.write() {
                let status = self.slam.status();
@@ -700,6 +745,19 @@ impl SlamContext {
    fn process_udp_lidar(&mut self, data: &LidarData, shared_state: &SharedStateHandle) {
        let timestamp_us = data.timestamp_us;

+        // Log first few scans to trace processing
+        static SCAN_COUNT: std::sync::atomic::AtomicU32 = std::sync::atomic::AtomicU32::new(0);
+        let count = SCAN_COUNT.fetch_add(1, std::sync::atomic::Ordering::Relaxed);
+        if count < 5 || count.is_multiple_of(50) {
+            log::info!(
+                "Processing UDP lidar: {} points, is_mapping={}, is_localizing={}, scan#{}",
+                data.points.len(),
+                self.is_mapping,
+                self.is_localizing,
+                count
+            );
+        }
+
        // Convert UDP LidarData to LidarScan format expected by process_lidar
        let lidar_scan: crate::io::LidarScan = data
            .points
@@ -1,20 +1,23 @@
 """
 SLAM thread for receiving data from DhruvaSLAM daemon.

-Uses DhruvaStream protocol for continuous data (via TCP):
- RobotStatus: pose, state, battery, mapping progress
- SensorStatus: lidar, encoders, IMU
- CurrentMap: occupancy grid
- MapList: saved maps
- NavigationStatus: path planning
+Uses DhruvaStream protocol with dual TCP/UDP architecture:

-And DhruvaCommand/Response protocol for commands (via same TCP port):
+UDP (high-frequency streams):
+- RobotStatus: pose, state, battery (10Hz)
+- SensorStatus: lidar, encoders, IMU (10Hz)
+- NavigationStatus: path planning (5Hz)
+- MappingProgress: exploration stats (2Hz)
+
+TCP (large/critical data):
+- CurrentMap: occupancy grid (1Hz)
+- MapList: saved maps (on change)
+
+Commands use DhruvaCommand/Response protocol via separate TCP connection:
 - StartMapping, StopMapping, ClearMap
 - RenameMap, DeleteMap, EnableMap, SetGoal, CancelGoal

-Note: Both streams and commands use the same port (5557) following the
-unified DhruvaSLAM architecture. Two separate TCP connections are used:
-one for streaming data, one for command/response.
+Note: TCP connection also registers client for UDP streaming (unicast).
 """

 import socket
@@ -22,6 +25,7 @@ import struct
 import logging
 import uuid
 import os
+import select
 from PyQt5.QtCore import QThread, pyqtSignal

 # Import protobuf definitions
@@ -81,30 +85,74 @@ class SlamThread(QThread):
        # Command port defaults to same as stream port (unified architecture)
        self.command_port = command_port if command_port is not None else port
        self._running = True
-        self.stream_socket = None
-        self.command_socket = None
+        self.stream_socket = None  # TCP for large data (maps) and registration
+        self.udp_socket = None     # UDP for high-frequency streams (nav, robot, sensor)

    def run(self):
-        """Main thread loop - connect and receive data."""
+        """Main thread loop - connect and receive data via TCP+UDP."""
        logger.info(f"SLAM thread starting, connecting to {self.host}:{self.port}")

        while self._running:
            try:
-                # Connect to stream port
+                # Step 1: Connect TCP (registers us for UDP streaming from DhruvaSLAM)
                self.stream_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
                self.stream_socket.settimeout(5.0)
                self.stream_socket.connect((self.host, self.port))
-                self.connection_status.emit(True, f"Connected to DhruvaSLAM at {self.host}:{self.port}")
-                logger.info(f"Connected to DhruvaSLAM stream at {self.host}:{self.port}")

-                # Receive loop
+                # Get our local (source) port - DhruvaSLAM sends UDP to this port
+                local_addr = self.stream_socket.getsockname()
+                local_port = local_addr[1]
+                logger.info(f"Connected to DhruvaSLAM TCP at {self.host}:{self.port} (local port {local_port})")
+
+                self.stream_socket.setblocking(False)  # Non-blocking for select()
+
+                # Step 2: Create UDP socket bound to same local port as TCP
+                # DhruvaSLAM sends UDP to client's TCP source address (IP:port)
+                self.udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+                self.udp_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+                self.udp_socket.bind(('0.0.0.0', local_port))
+                self.udp_socket.setblocking(False)  # Non-blocking for select()
+                logger.info(f"UDP socket bound to port {local_port} (matching TCP source port)")
+
+                self.connection_status.emit(True, f"Connected to DhruvaSLAM at {self.host}:{self.port}")
+
+                # Step 3: Receive loop - handle both TCP and UDP with select()
+                tcp_buffer = b''  # Buffer for TCP stream reassembly
+
                while self._running:
                    try:
-                        message = self._read_stream_message()
-                        if message:
-                            self._process_stream_message(message)
-                    except socket.timeout:
-                        continue
+                        # Wait for data on either socket (100ms timeout)
+                        readable, _, _ = select.select(
+                            [self.stream_socket, self.udp_socket], [], [], 0.1
+                        )
+
+                        for sock in readable:
+                            if sock == self.udp_socket:
+                                # UDP: Complete datagrams, no reassembly needed
+                                try:
+                                    data, addr = self.udp_socket.recvfrom(65536)
+                                    if data:
+                                        message = self._parse_udp_message(data)
+                                        if message:
+                                            self._process_stream_message(message)
+                                except BlockingIOError:
+                                    pass
+
+                            elif sock == self.stream_socket:
+                                # TCP: May need stream reassembly
+                                try:
+                                    chunk = self.stream_socket.recv(65536)
+                                    if not chunk:
+                                        # Connection closed
+                                        logger.warning("TCP connection closed by server")
+                                        raise ConnectionError("TCP closed")
+                                    tcp_buffer += chunk
+
+                                    # Process complete messages from buffer
+                                    tcp_buffer = self._process_tcp_buffer(tcp_buffer)
+                                except BlockingIOError:
+                                    pass
+
                    except Exception as e:
                        if self._running:
                            logger.error(f"Stream read error: {e}")
@@ -119,6 +167,12 @@ class SlamThread(QThread):
                    self.msleep(3000)

            finally:
+                if self.udp_socket:
+                    try:
+                        self.udp_socket.close()
+                    except:
+                        pass
+                    self.udp_socket = None
                if self.stream_socket:
                    try:
                        self.stream_socket.close()
@@ -132,18 +186,76 @@ class SlamThread(QThread):
        """Stop the thread."""
        logger.info("Stopping SLAM thread...")
        self._running = False
+        if self.udp_socket:
+            try:
+                self.udp_socket.close()
+            except:
+                pass
        if self.stream_socket:
            try:
                self.stream_socket.close()
            except:
                pass
-        if self.command_socket:
-            try:
-                self.command_socket.close()
-            except:
-                pass
        self.wait()

+    def _parse_udp_message(self, data: bytes) -> dhruva_pb2.DhruvaStream:
+        """Parse a UDP datagram containing a length-prefixed Protobuf message.
+
+        UDP datagrams are received atomically, so we get the complete message
+        in one receive. Format: [4-byte big-endian length][protobuf payload]
+        """
+        if len(data) < 4:
+            logger.warning(f"UDP packet too short: {len(data)} bytes")
+            return None
+
+        length = struct.unpack('>I', data[:4])[0]
+        expected_size = 4 + length
+
+        if len(data) != expected_size:
+            logger.warning(f"UDP packet size mismatch: got {len(data)}, expected {expected_size}")
+            return None
+
+        try:
+            msg = dhruva_pb2.DhruvaStream()
+            msg.ParseFromString(data[4:])
+            return msg
+        except Exception as e:
+            logger.error(f"UDP protobuf parse error: {e}")
+            return None
+
+    def _process_tcp_buffer(self, buffer: bytes) -> bytes:
+        """Process complete messages from TCP buffer, return remaining bytes.
+
+        TCP is a stream, so we may receive partial messages or multiple
+        messages in one recv(). This reassembles them properly.
+        """
+        while len(buffer) >= 4:
+            # Read length prefix
+            length = struct.unpack('>I', buffer[:4])[0]
+
+            # Sanity check
+            if length > 10 * 1024 * 1024:  # 10MB max
+                logger.error(f"TCP message too large: {length} bytes, clearing buffer")
+                return b''
+
+            total_size = 4 + length
+            if len(buffer) < total_size:
+                # Incomplete message, wait for more data
+                break
+
+            # Extract complete message
+            message_data = buffer[4:total_size]
+            buffer = buffer[total_size:]
+
+            try:
+                msg = dhruva_pb2.DhruvaStream()
+                msg.ParseFromString(message_data)
+                self._process_stream_message(msg)
+            except Exception as e:
+                logger.error(f"TCP protobuf parse error: {e}")
+
+        return buffer
+
    def _read_stream_message(self) -> dhruva_pb2.DhruvaStream:
        """Read a length-prefixed DhruvaStream message."""
        # Read 4-byte length prefix
@@ -341,28 +453,32 @@ class SlamThread(QThread):

        Returns True if command was sent successfully.
        Response will be emitted via command_response_received signal.
+
+        Uses a fresh connection for each command to avoid stale socket issues.
        """
+        cmd_socket = None
        try:
-            # Connect to command port if needed
-            if not self.command_socket:
-                self.command_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
-                self.command_socket.settimeout(5.0)
-                self.command_socket.connect((self.host, self.command_port))
-                logger.info(f"Connected to DhruvaSLAM command port at {self.host}:{self.command_port}")
+            # Create fresh connection for each command
+            logger.info(f"Connecting to command port {self.host}:{self.command_port}")
+            cmd_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+            cmd_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+            cmd_socket.settimeout(5.0)
+            cmd_socket.connect((self.host, self.command_port))

            # Serialize and send
            payload = command.SerializeToString()
            length = struct.pack('>I', len(payload))
-            self.command_socket.sendall(length + payload)
+            logger.info(f"Sending command ({len(payload)} bytes)")
+            cmd_socket.sendall(length + payload)

            # Read response
-            len_bytes = self._recv_exact(self.command_socket, 4)
+            len_bytes = self._recv_exact(cmd_socket, 4)
            if not len_bytes:
                logger.error("No response from command port")
                return False

            resp_len = struct.unpack('>I', len_bytes)[0]
-            resp_data = self._recv_exact(self.command_socket, resp_len)
+            resp_data = self._recv_exact(cmd_socket, resp_len)
            if not resp_data:
                logger.error("Incomplete response from command port")
                return False
@@ -392,18 +508,22 @@ class SlamThread(QThread):
                resp_payload['data'] = {}

            self.command_response_received.emit(resp_payload)
+            logger.info(f"Command successful: {response.success}")
            return True

        except Exception as e:
            logger.error(f"Command send failed: {e}")
-            # Reset command socket
-            if self.command_socket:
+            import traceback
+            logger.error(traceback.format_exc())
+            return False
+
+        finally:
+            # Always close the command socket
+            if cmd_socket:
                try:
-                    self.command_socket.close()
+                    cmd_socket.close()
                except:
                    pass
-                self.command_socket = None
-            return False

    def start_mapping(self, map_name: str = "") -> str:
        """Start mapping with optional map name. Returns request_id."""
@@ -0,0 +1,104 @@
+P2
+# Large room with obstacles for SLAM testing (5m x 5m)
+100 100
+255
+0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 255 0 0
+0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
@@ -0,0 +1,7 @@
+# Large room with obstacles for SLAM testing (5m x 5m)
+image: large_room_obstacles.pgm
+resolution: 0.05
+origin: [0.0, 0.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
@@ -7,11 +7,12 @@ name = "Mock CRL-200S Simulator"

 [device.simulation]
 # Map file (YAML file that references PGM image)
-map_file = "maps/large_room.yaml"
+# Using large_room_obstacles for testing exploration with obstacles
+map_file = "maps/large_room_obstacles.yaml"

-# Initial robot pose (corner of 5m x 5m room - requires movement to explore)
-start_x = 1.0    # meters (near left wall)
-start_y = 1.0    # meters (near bottom wall)
+# Initial robot pose (5m x 5m room - map is 100x100 at 0.05m/pixel)
+start_x = 2.5    # meters (center of room)
+start_y = 2.5    # meters (center of room)
 start_theta = 0.0  # radians (0 = facing +X)

 # Simulation parameters
@@ -26,7 +27,7 @@ ticks_per_meter = 4464.0   # Encoder ticks per meter
 max_linear_speed = 0.3     # Maximum linear velocity (m/s)
 max_angular_speed = 1.0    # Maximum angular velocity (rad/s)
 robot_radius = 0.17        # Collision radius (meters)
-collision_mode = "passthrough"    # "stop", "slide", "passthrough" - passthrough for testing
+collision_mode = "stop"           # "stop", "slide", "passthrough" - stop for realistic testing
 slide_friction = 0.3       # Used when collision_mode = "slide"

 # Lidar configuration (Delta-2D specifications)
@@ -121,6 +122,7 @@ is_charging = false
 log_state_changes = true

 # Network configuration
-# Both TCP (commands) and UDP (sensor streaming) use the same port
+# TCP and UDP on same port (5555) - unified architecture
 [network]
 bind_address = "0.0.0.0:5555"
+udp_client_port = 5555
@@ -303,11 +303,20 @@ pub struct NetworkConfig {
    ///
    /// **Required**: Yes
    pub bind_address: String,
+
+    /// UDP client port override (optional).
+    ///
+    /// If specified, UDP streaming will use this port instead of the TCP port.
+    /// Useful for localhost testing where TCP and UDP need different ports.
+    ///
+    /// **Default**: Same as TCP port from `bind_address`
+    #[serde(default)]
+    pub udp_client_port: Option<u16>,
 }

 impl NetworkConfig {
    /// Extract the port number from bind_address.
-    /// Used for both TCP listening and UDP streaming to clients.
+    /// Used for TCP listening.
    pub fn port(&self) -> u16 {
        self.bind_address
            .rsplit(':')
@@ -315,6 +324,12 @@ impl NetworkConfig {
            .and_then(|p| p.parse().ok())
            .unwrap_or(5555)
    }
+
+    /// Get the UDP streaming port.
+    /// Uses `udp_client_port` if set, otherwise same as TCP port.
+    pub fn udp_port(&self) -> u16 {
+        self.udp_client_port.unwrap_or_else(|| self.port())
+    }
 }

 /// Root configuration
@@ -597,6 +597,13 @@ fn simulation_loop(
                let scan = lidar_sim.generate_scan(&map, physics.x(), physics.y(), physics.theta());

                if let Ok(mut lidar) = lidar_data.lock() {
+                    log::info!(
+                        "Generated lidar scan: {} points at ({:.2}, {:.2}, {:.2}°)",
+                        scan.len(),
+                        physics.x(),
+                        physics.y(),
+                        physics.theta().to_degrees()
+                    );
                    lidar.set("scan", SensorValue::PointCloud2D(scan));
                    lidar.touch();
                }
@@ -101,17 +101,25 @@ fn main() -> Result<()> {
    // =========================================================================
    // UDP Unicast Setup (sensor streaming to registered clients)
    // =========================================================================
-    // UDP streaming uses the same port as TCP (extracted from bind_address)
-    let udp_streaming_port = config.network.port();
+    // UDP streaming port (may be different from TCP for localhost testing)
+    let udp_streaming_port = config.network.udp_port();

    // 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)))?;

-    log::info!(
-        "UDP unicast streaming enabled (port {} - same as TCP)",
-        udp_streaming_port
-    );
+    if udp_streaming_port == config.network.port() {
+        log::info!(
+            "UDP unicast streaming enabled (port {} - same as TCP)",
+            udp_streaming_port
+        );
+    } else {
+        log::info!(
+            "UDP unicast streaming enabled (port {} - different from TCP:{})",
+            udp_streaming_port,
+            config.network.port()
+        );
+    }

    // Client registry: tracks which IP to send UDP sensor data to
    // Updated when TCP clients connect/disconnect
@@ -229,13 +229,22 @@ impl UdpPublisher {
                        // UDP send errors are not fatal - just log and continue
                        log::warn!("Failed to send {} poll data: {}", group_id, e);
                    } else {
-                        log::trace!(
-                            "Sent {} (seq: {}, ts: {}) to {} [polled]",
-                            cloned.group_id,
-                            cloned.sequence_number,
-                            cloned.timestamp_us,
-                            target_addr
-                        );
+                        // Log lidar at INFO level to trace data flow
+                        if group_id == "lidar" {
+                            log::info!(
+                                "Sent {} (seq: {}) to {} [polled]",
+                                cloned.group_id,
+                                cloned.sequence_number,
+                                target_addr
+                            );
+                        } else {
+                            log::trace!(
+                                "Sent {} (seq: {}) to {} [polled]",
+                                cloned.group_id,
+                                cloned.sequence_number,
+                                target_addr
+                            );
+                        }
                    }
                    sent_any = true;
                }
@@ -47,6 +47,9 @@ lost_threshold = 0.1
 min_scan_points = 50
 encoder_weight = 0.8
 use_submap_matching = false
+# Sanity check thresholds to reject false positive matches in symmetric environments
+max_theta_deviation = 0.35       # ~20 degrees - reject if rotation differs too much from odometry
+max_translation_deviation = 0.5  # 50cm - reject if translation differs too much from odometry

 [matcher]
 algorithm = "hybrid_p2l"
@@ -120,11 +123,11 @@ strategy = "frontier"
 loop_rate_hz = 10.0

 [exploration.motion]
-max_linear_vel = 0.3
-max_angular_vel = 0.5
+max_linear_vel = 0.4
+max_angular_vel = 0.8
 obstacle_clearance = 0.15
-linear_vel = 0.2
-angular_vel = 0.3
+linear_vel = 0.3
+angular_vel = 0.5

 [exploration.frontier]
 min_frontier_size = 3       # Smaller frontiers still worth exploring
@@ -136,11 +139,11 @@ clustering_threshold = 0.5
 enabled = true
 update_rate_hz = 10.0
 robot_radius = 0.18
-max_linear_vel = 0.3
-max_angular_vel = 0.5
+max_linear_vel = 0.4
+max_angular_vel = 0.8
 waypoint_threshold = 0.15
 goal_threshold = 0.08
-heading_threshold = 0.15
+heading_threshold = 0.35  # ~20 degrees - allow more angular error before rotating

 [session]
 state_path = "/tmp/dhruva_test_session.json"
@@ -0,0 +1,742 @@
+#!/usr/bin/env python3
+"""
+Movement and Scan Matching Test Script
+
+Tests robot movement accuracy independently from exploration/mapping logic.
+Uses SetGoal commands to move robot to specific locations and monitors
+pose estimates to check for drift and rotation errors.
+
+Test modes:
+1. rotation_only  - Pure rotation tests (no translation)
+2. linear_only    - Straight line movement tests
+3. square         - Square path to test cumulative error
+4. waypoints      - Custom waypoint sequence
+
+Usage:
+    python test_movement.py --mode rotation_only --port 5557
+    python test_movement.py --mode linear_only --distance 1.0
+    python test_movement.py --mode square --size 1.0
+    python test_movement.py --mode waypoints --waypoints "1,1,0;2,1,0;2,2,0"
+"""
+
+import argparse
+import json
+import math
+import socket
+import struct
+import sys
+import threading
+import time
+import uuid
+from dataclasses import dataclass, field
+from datetime import datetime
+from pathlib import Path
+from typing import List, Optional, Tuple
+
+# Add proto directory to path
+SCRIPT_DIR = Path(__file__).parent
+PROTO_DIR = SCRIPT_DIR / "proto"
+sys.path.insert(0, str(PROTO_DIR))
+
+try:
+    import dhruva_pb2
+except ImportError:
+    print("ERROR: Proto files not compiled. Run 'make proto' first.")
+    sys.exit(1)
+
+
+@dataclass
+class Pose:
+    """2D pose with timestamp."""
+    x: float
+    y: float
+    theta: float
+    timestamp: float = 0.0
+
+    def distance_to(self, other: 'Pose') -> float:
+        return math.sqrt((self.x - other.x)**2 + (self.y - other.y)**2)
+
+    def angle_diff(self, other: 'Pose') -> float:
+        """Compute smallest angle difference."""
+        diff = self.theta - other.theta
+        while diff > math.pi:
+            diff -= 2 * math.pi
+        while diff < -math.pi:
+            diff += 2 * math.pi
+        return diff
+
+    def __str__(self):
+        return f"({self.x:.3f}, {self.y:.3f}, {math.degrees(self.theta):.1f}°)"
+
+
+@dataclass
+class GoalResult:
+    """Result of a single goal navigation."""
+    goal: Pose
+    start_pose: Pose
+    end_pose: Pose
+    expected_pose: Pose
+    duration_s: float
+    position_error: float
+    rotation_error: float
+    success: bool
+    status_message: str = ""
+
+
+@dataclass
+class TestResult:
+    """Complete test results."""
+    test_name: str
+    test_mode: str
+    timestamp: str
+    goals: List[GoalResult] = field(default_factory=list)
+    total_duration_s: float = 0.0
+
+    def summary(self) -> dict:
+        if not self.goals:
+            return {"error": "No goals executed"}
+
+        pos_errors = [g.position_error for g in self.goals]
+        rot_errors = [abs(g.rotation_error) for g in self.goals]
+
+        return {
+            "test_name": self.test_name,
+            "test_mode": self.test_mode,
+            "timestamp": self.timestamp,
+            "num_goals": len(self.goals),
+            "success_rate": sum(1 for g in self.goals if g.success) / len(self.goals),
+            "position_error": {
+                "mean_m": sum(pos_errors) / len(pos_errors),
+                "max_m": max(pos_errors),
+                "min_m": min(pos_errors),
+            },
+            "rotation_error": {
+                "mean_deg": math.degrees(sum(rot_errors) / len(rot_errors)),
+                "max_deg": math.degrees(max(rot_errors)),
+                "min_deg": math.degrees(min(rot_errors)),
+            },
+            "total_duration_s": self.total_duration_s,
+        }
+
+
+class MovementTester:
+    """Test robot movement using SetGoal commands."""
+
+    def __init__(self, host: str = "localhost", port: int = 5557):
+        self.host = host
+        self.port = port
+        self.current_pose: Optional[Pose] = None
+        self.nav_state: int = 0  # NAV_STATE_IDLE
+        self.pose_history: List[Pose] = []
+        self._lock = threading.Lock()
+        self._running = False
+        self._udp_thread: Optional[threading.Thread] = None
+
+    def _send_command(self, cmd: dhruva_pb2.DhruvaCommand,
+                      timeout: float = 5.0) -> Optional[dhruva_pb2.DhruvaResponse]:
+        """Send command and receive response."""
+        with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as sock:
+            sock.settimeout(timeout)
+            sock.connect((self.host, self.port))
+
+            # Send length-prefixed message
+            data = cmd.SerializeToString()
+            sock.sendall(struct.pack(">I", len(data)) + data)
+
+            # Receive response
+            try:
+                length_data = b""
+                while len(length_data) < 4:
+                    chunk = sock.recv(4 - len(length_data))
+                    if not chunk:
+                        return None
+                    length_data += chunk
+
+                length = struct.unpack(">I", length_data)[0]
+
+                resp_data = b""
+                while len(resp_data) < length:
+                    chunk = sock.recv(length - len(resp_data))
+                    if not chunk:
+                        return None
+                    resp_data += chunk
+
+                response = dhruva_pb2.DhruvaResponse()
+                response.ParseFromString(resp_data)
+                return response
+            except socket.timeout:
+                return None
+
+    def _start_udp_listener(self):
+        """Start UDP listener for pose updates.
+
+        Uses SO_REUSEADDR and SO_REUSEPORT to share port with TCP.
+        The TCP connection registers us for UDP streaming.
+        """
+        self._running = True
+
+        # Use a dedicated port for UDP reception
+        # We'll bind both UDP and TCP to this port
+        self._udp_port = 5560  # Different from server port
+
+        # Create UDP socket first
+        self._udp_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+        self._udp_sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+        try:
+            self._udp_sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1)
+        except AttributeError:
+            pass  # SO_REUSEPORT not available on all platforms
+        self._udp_sock.bind(("0.0.0.0", self._udp_port))
+        self._udp_sock.settimeout(0.5)
+
+        # Create TCP socket bound to same port for registration
+        self._tcp_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+        self._tcp_sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+        try:
+            self._tcp_sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1)
+        except AttributeError:
+            pass
+        self._tcp_sock.bind(("0.0.0.0", self._udp_port))
+        self._tcp_sock.connect((self.host, self.port))
+
+        # Start listener thread
+        self._udp_thread = threading.Thread(target=self._udp_listener_loop, daemon=True)
+        self._udp_thread.start()
+
+    def _stop_udp_listener(self):
+        """Stop UDP listener."""
+        self._running = False
+        if self._udp_thread:
+            self._udp_thread.join(timeout=2.0)
+        if hasattr(self, '_tcp_sock') and self._tcp_sock:
+            try:
+                self._tcp_sock.close()
+            except:
+                pass
+        if hasattr(self, '_udp_sock') and self._udp_sock:
+            try:
+                self._udp_sock.close()
+            except:
+                pass
+
+    def _udp_listener_loop(self):
+        """Listen for UDP status updates."""
+        while self._running:
+            try:
+                data, addr = self._udp_sock.recvfrom(65535)
+                if len(data) < 4:
+                    continue
+
+                length = struct.unpack(">I", data[:4])[0]
+                if len(data) < 4 + length:
+                    continue
+
+                msg = dhruva_pb2.DhruvaStream()
+                msg.ParseFromString(data[4:4+length])
+
+                timestamp = msg.timestamp_us / 1e6
+
+                if msg.HasField("robot_status"):
+                    rs = msg.robot_status
+                    with self._lock:
+                        self.current_pose = Pose(
+                            x=rs.pose.x,
+                            y=rs.pose.y,
+                            theta=rs.pose.theta,
+                            timestamp=timestamp
+                        )
+                        self.pose_history.append(self.current_pose)
+
+                if msg.HasField("navigation_status"):
+                    with self._lock:
+                        self.nav_state = msg.navigation_status.state
+
+            except socket.timeout:
+                continue
+            except Exception as e:
+                if self._running:
+                    print(f"UDP error: {e}")
+
+    def get_current_pose(self) -> Optional[Pose]:
+        """Get current pose (thread-safe)."""
+        with self._lock:
+            return self.current_pose
+
+    def get_nav_state(self) -> int:
+        """Get current navigation state (thread-safe)."""
+        with self._lock:
+            return self.nav_state
+
+    def start_mapping(self, name: str = "movement_test") -> bool:
+        """Start a mapping session."""
+        cmd = dhruva_pb2.DhruvaCommand()
+        cmd.request_id = str(uuid.uuid4())[:8]
+        cmd.start_mapping.map_name = name
+
+        response = self._send_command(cmd)
+        if response and response.success:
+            print(f"  Mapping started (map_id={response.mapping_started.map_id})")
+            return True
+        else:
+            print(f"  Failed to start mapping: {response.error_message if response else 'No response'}")
+            return False
+
+    def stop_mapping(self, save: bool = False) -> bool:
+        """Stop mapping session."""
+        cmd = dhruva_pb2.DhruvaCommand()
+        cmd.request_id = str(uuid.uuid4())[:8]
+        cmd.stop_mapping.save = save
+
+        response = self._send_command(cmd)
+        return response is not None and response.success
+
+    def set_goal(self, x: float, y: float, theta: Optional[float] = None) -> bool:
+        """Send SetGoal command."""
+        cmd = dhruva_pb2.DhruvaCommand()
+        cmd.request_id = str(uuid.uuid4())[:8]
+        cmd.set_goal.x = x
+        cmd.set_goal.y = y
+        if theta is not None:
+            cmd.set_goal.theta = theta
+        cmd.set_goal.description = f"Test goal ({x:.2f}, {y:.2f})"
+
+        response = self._send_command(cmd)
+        if response and response.success:
+            return True
+        else:
+            print(f"  Failed to set goal: {response.error_message if response else 'No response'}")
+            return False
+
+    def wait_for_navigation_complete(self, timeout: float = 30.0) -> Tuple[bool, str]:
+        """Wait for navigation to complete or fail."""
+        start = time.time()
+
+        # Wait for navigation to start
+        while time.time() - start < 5.0:
+            state = self.get_nav_state()
+            if state in [2, 3]:  # NAVIGATING or ROTATING_TO_HEADING
+                break
+            time.sleep(0.1)
+
+        # Wait for completion
+        while time.time() - start < timeout:
+            state = self.get_nav_state()
+
+            if state == 4:  # NAV_STATE_REACHED
+                return True, "Goal reached"
+            elif state == 5:  # NAV_STATE_FAILED
+                return False, "Navigation failed"
+            elif state == 6:  # NAV_STATE_CANCELLED
+                return False, "Navigation cancelled"
+            elif state == 0:  # NAV_STATE_IDLE
+                # Might have completed very quickly
+                return True, "Navigation idle (possibly completed)"
+
+            time.sleep(0.1)
+
+        return False, "Navigation timeout"
+
+    def execute_goal(self, goal: Pose, timeout: float = 30.0) -> GoalResult:
+        """Execute a single goal and measure result."""
+        start_pose = self.get_current_pose()
+        if start_pose is None:
+            start_pose = Pose(0, 0, 0)
+
+        start_time = time.time()
+
+        # Send goal
+        success = self.set_goal(goal.x, goal.y, goal.theta if goal.theta != 0 else None)
+        if not success:
+            return GoalResult(
+                goal=goal,
+                start_pose=start_pose,
+                end_pose=start_pose,
+                expected_pose=goal,
+                duration_s=0,
+                position_error=goal.distance_to(start_pose),
+                rotation_error=goal.angle_diff(start_pose),
+                success=False,
+                status_message="Failed to send goal"
+            )
+
+        # Wait for completion
+        nav_success, status_msg = self.wait_for_navigation_complete(timeout)
+
+        duration = time.time() - start_time
+        end_pose = self.get_current_pose()
+        if end_pose is None:
+            end_pose = start_pose
+
+        # Compute errors
+        # Expected pose is the goal
+        position_error = end_pose.distance_to(goal)
+        rotation_error = end_pose.angle_diff(goal)
+
+        return GoalResult(
+            goal=goal,
+            start_pose=start_pose,
+            end_pose=end_pose,
+            expected_pose=goal,
+            duration_s=duration,
+            position_error=position_error,
+            rotation_error=rotation_error,
+            success=nav_success and position_error < 0.5,  # 50cm tolerance
+            status_message=status_msg
+        )
+
+    def test_rotation_only(self, angles_deg: List[float] = None) -> TestResult:
+        """Test rotation accuracy using small movements that end with target heading.
+
+        Note: Pure rotation (same position, different heading) is not supported
+        by the current path planner. We use small movements instead.
+        """
+        if angles_deg is None:
+            angles_deg = [90, 180, -90, 45, -45]
+
+        result = TestResult(
+            test_name="rotation_test",
+            test_mode="rotation_only",
+            timestamp=datetime.now().isoformat()
+        )
+
+        current_pose = self.get_current_pose()
+        if current_pose is None:
+            print("ERROR: Cannot get current pose")
+            return result
+
+        print(f"\n  Starting rotation test from {current_pose}")
+        print("  Note: Using small movements (0.3m) to test rotation accuracy")
+        start_time = time.time()
+
+        # Use small movements with target heading
+        move_distance = 0.3  # Small movement to trigger path planning
+
+        for angle_deg in angles_deg:
+            target_theta = math.radians(angle_deg)
+
+            # Move slightly in the direction of target heading
+            goal = Pose(
+                x=current_pose.x + move_distance * math.cos(target_theta),
+                y=current_pose.y + move_distance * math.sin(target_theta),
+                theta=target_theta
+            )
+
+            print(f"  Moving 0.3m toward {angle_deg}° to {goal}...")
+            goal_result = self.execute_goal(goal, timeout=20.0)
+            result.goals.append(goal_result)
+
+            status = "OK" if goal_result.success else "FAIL"
+            print(f"    {status}: Position error = {goal_result.position_error*100:.1f}cm, "
+                  f"Rotation error = {math.degrees(goal_result.rotation_error):.1f}°")
+
+            # Update current pose for next goal
+            new_pose = self.get_current_pose()
+            if new_pose:
+                current_pose = new_pose
+
+            time.sleep(0.5)  # Pause between tests
+
+        result.total_duration_s = time.time() - start_time
+        return result
+
+    def test_linear_only(self, distance: float = 1.0, directions: List[float] = None) -> TestResult:
+        """Test straight line movement accuracy."""
+        if directions is None:
+            directions = [0, 90, 180, 270]  # Forward, left, back, right
+
+        result = TestResult(
+            test_name="linear_test",
+            test_mode="linear_only",
+            timestamp=datetime.now().isoformat()
+        )
+
+        start_pose = self.get_current_pose()
+        if start_pose is None:
+            print("ERROR: Cannot get current pose")
+            return result
+
+        print(f"\n  Starting linear test from {start_pose}")
+        start_time = time.time()
+
+        for direction_deg in directions:
+            direction_rad = math.radians(direction_deg)
+            goal = Pose(
+                x=start_pose.x + distance * math.cos(direction_rad),
+                y=start_pose.y + distance * math.sin(direction_rad),
+                theta=direction_rad
+            )
+
+            print(f"  Moving {distance}m in direction {direction_deg}° to {goal}...")
+            goal_result = self.execute_goal(goal)
+            result.goals.append(goal_result)
+
+            status = "OK" if goal_result.success else "FAIL"
+            print(f"    {status}: Position error = {goal_result.position_error*100:.1f}cm, "
+                  f"Rotation error = {math.degrees(goal_result.rotation_error):.1f}°")
+
+            # Update start pose for next movement
+            current = self.get_current_pose()
+            if current:
+                start_pose = current
+
+            time.sleep(1.0)
+
+        result.total_duration_s = time.time() - start_time
+        return result
+
+    def test_square(self, size: float = 1.0) -> TestResult:
+        """Test square path to measure cumulative error."""
+        result = TestResult(
+            test_name="square_test",
+            test_mode="square",
+            timestamp=datetime.now().isoformat()
+        )
+
+        start_pose = self.get_current_pose()
+        if start_pose is None:
+            print("ERROR: Cannot get current pose")
+            return result
+
+        print(f"\n  Starting square test ({size}m x {size}m) from {start_pose}")
+        start_time = time.time()
+
+        # Define square corners relative to start
+        corners = [
+            (start_pose.x + size, start_pose.y, 0),          # Right
+            (start_pose.x + size, start_pose.y + size, math.pi/2),  # Up
+            (start_pose.x, start_pose.y + size, math.pi),    # Left
+            (start_pose.x, start_pose.y, -math.pi/2),        # Back to start
+        ]
+
+        for i, (x, y, theta) in enumerate(corners):
+            goal = Pose(x=x, y=y, theta=theta)
+            print(f"  Corner {i+1}/4: Moving to {goal}...")
+
+            goal_result = self.execute_goal(goal)
+            result.goals.append(goal_result)
+
+            status = "OK" if goal_result.success else "FAIL"
+            print(f"    {status}: Position error = {goal_result.position_error*100:.1f}cm, "
+                  f"Rotation error = {math.degrees(goal_result.rotation_error):.1f}°")
+
+            time.sleep(0.5)
+
+        # Measure return-to-start error
+        final_pose = self.get_current_pose()
+        if final_pose:
+            return_error = final_pose.distance_to(start_pose)
+            print(f"\n  Return-to-start error: {return_error*100:.1f}cm")
+
+        result.total_duration_s = time.time() - start_time
+        return result
+
+    def test_waypoints(self, waypoints: List[Tuple[float, float, float]]) -> TestResult:
+        """Test custom waypoint sequence."""
+        result = TestResult(
+            test_name="waypoints_test",
+            test_mode="waypoints",
+            timestamp=datetime.now().isoformat()
+        )
+
+        print(f"\n  Starting waypoint test with {len(waypoints)} waypoints")
+        start_time = time.time()
+
+        for i, (x, y, theta) in enumerate(waypoints):
+            goal = Pose(x=x, y=y, theta=theta)
+            print(f"  Waypoint {i+1}/{len(waypoints)}: Moving to {goal}...")
+
+            goal_result = self.execute_goal(goal)
+            result.goals.append(goal_result)
+
+            status = "OK" if goal_result.success else "FAIL"
+            print(f"    {status}: Position error = {goal_result.position_error*100:.1f}cm, "
+                  f"Rotation error = {math.degrees(goal_result.rotation_error):.1f}°")
+
+            time.sleep(0.5)
+
+        result.total_duration_s = time.time() - start_time
+        return result
+
+
+def parse_waypoints(waypoints_str: str) -> List[Tuple[float, float, float]]:
+    """Parse waypoints from string format 'x,y,theta;x,y,theta;...'"""
+    waypoints = []
+    for wp in waypoints_str.split(";"):
+        parts = [float(p.strip()) for p in wp.split(",")]
+        if len(parts) >= 2:
+            x, y = parts[0], parts[1]
+            theta = math.radians(parts[2]) if len(parts) > 2 else 0.0
+            waypoints.append((x, y, theta))
+    return waypoints
+
+
+def main():
+    parser = argparse.ArgumentParser(
+        description="Test robot movement and scan matching accuracy",
+        formatter_class=argparse.RawDescriptionHelpFormatter,
+        epilog="""
+Examples:
+  # Test rotation accuracy
+  python test_movement.py --mode rotation_only
+
+  # Test linear movement (1m in each cardinal direction)
+  python test_movement.py --mode linear_only --distance 1.0
+
+  # Test square path (0.5m x 0.5m square)
+  python test_movement.py --mode square --size 0.5
+
+  # Test custom waypoints (x,y,theta_deg separated by semicolons)
+  python test_movement.py --mode waypoints --waypoints "0.5,0,0;0.5,0.5,90;0,0.5,180;0,0,-90"
+        """
+    )
+
+    parser.add_argument("--host", default="localhost", help="DhruvaSLAM host")
+    parser.add_argument("--port", type=int, default=5557, help="DhruvaSLAM port")
+    parser.add_argument("--mode", required=True,
+                        choices=["rotation_only", "linear_only", "square", "waypoints"],
+                        help="Test mode")
+    parser.add_argument("--distance", type=float, default=1.0,
+                        help="Distance for linear tests (meters)")
+    parser.add_argument("--size", type=float, default=1.0,
+                        help="Size for square test (meters)")
+    parser.add_argument("--waypoints", type=str,
+                        help="Waypoints for custom test: 'x,y,theta_deg;...'")
+    parser.add_argument("--output", type=str,
+                        help="Output JSON file for results")
+    parser.add_argument("--skip-mapping", action="store_true",
+                        help="Skip starting mapping (assume already started)")
+    parser.add_argument("--timeout", type=float, default=30.0,
+                        help="Goal timeout in seconds")
+
+    args = parser.parse_args()
+
+    print("=" * 60)
+    print("  Movement & Scan Matching Test")
+    print("=" * 60)
+    print(f"  Host: {args.host}:{args.port}")
+    print(f"  Mode: {args.mode}")
+
+    tester = MovementTester(args.host, args.port)
+
+    try:
+        # Start UDP listener for pose updates
+        print("\n[1] Starting pose monitor...")
+        tester._start_udp_listener()
+        time.sleep(1.0)  # Let UDP start
+
+        # Start mapping if needed
+        if not args.skip_mapping:
+            print("\n[2] Starting mapping session...")
+            if not tester.start_mapping("movement_test"):
+                print("ERROR: Failed to start mapping")
+                sys.exit(1)
+
+            # Wait longer for mapping to build initial map data
+            # The path planner needs map data to plan paths
+            print("  Waiting for initial scan and map data (15s)...")
+            time.sleep(15.0)
+
+        # Wait for first pose
+        print("\n[3] Waiting for initial pose...")
+        for _ in range(50):  # 5 seconds
+            if tester.get_current_pose() is not None:
+                break
+            time.sleep(0.1)
+
+        initial_pose = tester.get_current_pose()
+        if initial_pose is None:
+            print("ERROR: Could not get initial pose")
+            sys.exit(1)
+        print(f"  Initial pose: {initial_pose}")
+
+        # Run test
+        print(f"\n[4] Running {args.mode} test...")
+
+        if args.mode == "rotation_only":
+            result = tester.test_rotation_only()
+        elif args.mode == "linear_only":
+            result = tester.test_linear_only(args.distance)
+        elif args.mode == "square":
+            result = tester.test_square(args.size)
+        elif args.mode == "waypoints":
+            if not args.waypoints:
+                print("ERROR: --waypoints required for waypoints mode")
+                sys.exit(1)
+            waypoints = parse_waypoints(args.waypoints)
+            result = tester.test_waypoints(waypoints)
+
+        # Print summary
+        print("\n" + "=" * 60)
+        print("  Test Summary")
+        print("=" * 60)
+        summary = result.summary()
+        print(f"  Mode: {summary['test_mode']}")
+        print(f"  Goals executed: {summary['num_goals']}")
+        print(f"  Success rate: {summary['success_rate']*100:.1f}%")
+        print(f"  Position error (mean): {summary['position_error']['mean_m']*100:.1f}cm")
+        print(f"  Position error (max):  {summary['position_error']['max_m']*100:.1f}cm")
+        print(f"  Rotation error (mean): {summary['rotation_error']['mean_deg']:.1f}°")
+        print(f"  Rotation error (max):  {summary['rotation_error']['max_deg']:.1f}°")
+        print(f"  Total duration: {summary['total_duration_s']:.1f}s")
+
+        # Save results if requested
+        if args.output:
+            output_path = Path(args.output)
+            output_path.parent.mkdir(parents=True, exist_ok=True)
+
+            # Convert to JSON-serializable format
+            output_data = {
+                "summary": summary,
+                "goals": [
+                    {
+                        "goal": {"x": g.goal.x, "y": g.goal.y, "theta": g.goal.theta},
+                        "start": {"x": g.start_pose.x, "y": g.start_pose.y, "theta": g.start_pose.theta},
+                        "end": {"x": g.end_pose.x, "y": g.end_pose.y, "theta": g.end_pose.theta},
+                        "position_error_m": g.position_error,
+                        "rotation_error_rad": g.rotation_error,
+                        "duration_s": g.duration_s,
+                        "success": g.success,
+                        "status": g.status_message
+                    }
+                    for g in result.goals
+                ],
+                "pose_history": [
+                    {"x": p.x, "y": p.y, "theta": p.theta, "t": p.timestamp}
+                    for p in tester.pose_history[-1000:]  # Last 1000 poses
+                ]
+            }
+
+            with open(output_path, 'w') as f:
+                json.dump(output_data, f, indent=2)
+            print(f"\n  Results saved to: {output_path}")
+
+        # Stop mapping
+        if not args.skip_mapping:
+            print("\n[5] Stopping mapping...")
+            tester.stop_mapping(save=False)
+
+        print("\nTest complete!")
+
+        # Exit with error if tests failed
+        if summary['success_rate'] < 0.5:
+            sys.exit(1)
+
+    except KeyboardInterrupt:
+        print("\n\nInterrupted by user")
+        tester.stop_mapping(save=False)
+        sys.exit(1)
+    except ConnectionRefusedError:
+        print(f"\nERROR: Could not connect to {args.host}:{args.port}")
+        print("Is DhruvaSLAM running?")
+        sys.exit(1)
+    except Exception as e:
+        print(f"\nERROR: {e}")
+        import traceback
+        traceback.print_exc()
+        sys.exit(1)
+    finally:
+        tester._stop_udp_listener()
+
+
+if __name__ == "__main__":
+    main()
@@ -11,15 +11,15 @@ coverage_target = 0.8
 # All paths relative to VacuumTiger root
 sangamio_config = "sangam-io/mock.toml"
 dhruva_config = "slam-test/dhruva-test.toml"
-ground_truth_map = "sangam-io/maps/example.pgm"
-ground_truth_yaml = "sangam-io/maps/example.yaml"
+ground_truth_map = "sangam-io/maps/large_room_obstacles.pgm"
+ground_truth_yaml = "sangam-io/maps/large_room_obstacles.yaml"

 [ground_truth]
-# Map resolution in meters per pixel (from example.yaml)
+# Map resolution in meters per pixel (from large_room.yaml)
 resolution = 0.05
 # Robot start position in world coordinates (from mock.toml)
-robot_start_x = 1.5
-robot_start_y = 3.5
+robot_start_x = 2.5
+robot_start_y = 2.5
 robot_start_theta = 0.0

 [network]
@@ -0,0 +1,353 @@
+#!/bin/bash
+#
+# Movement & Scan Matching Test Script
+#
+# Tests robot movement accuracy independently from exploration/mapping logic.
+# Uses SetGoal commands to move robot to specific locations.
+#
+# Usage:
+#   ./test_movement.sh [OPTIONS]
+#
+# Options:
+#   --mode MODE      Test mode: rotation_only, linear_only, square, waypoints
+#   --output DIR     Output directory (default: results/movement_TIMESTAMP)
+#   --distance D     Distance for linear tests in meters (default: 0.5)
+#   --size S         Size for square test in meters (default: 0.5)
+#   --waypoints WP   Custom waypoints: "x,y,theta;..."
+#   --help           Show this help message
+
+set -e
+
+# Default values
+MODE="rotation_only"
+DISTANCE=0.5
+SIZE=0.5
+WAYPOINTS=""
+OUTPUT_DIR=""
+
+# Parse arguments
+while [[ $# -gt 0 ]]; do
+    case $1 in
+        --mode|-m)
+            MODE="$2"
+            shift 2
+            ;;
+        --output|-o)
+            OUTPUT_DIR="$2"
+            shift 2
+            ;;
+        --distance|-d)
+            DISTANCE="$2"
+            shift 2
+            ;;
+        --size|-s)
+            SIZE="$2"
+            shift 2
+            ;;
+        --waypoints|-w)
+            WAYPOINTS="$2"
+            shift 2
+            ;;
+        --help|-h)
+            head -20 "$0" | tail -15
+            exit 0
+            ;;
+        *)
+            echo "Unknown option: $1"
+            exit 1
+            ;;
+    esac
+done
+
+# Get script directory and project root
+SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+PROJECT_ROOT="$(dirname "$SCRIPT_DIR")"
+
+# Set output directory with timestamp if not specified
+if [[ -z "$OUTPUT_DIR" ]]; then
+    OUTPUT_DIR="$SCRIPT_DIR/results/movement_$(date +%Y%m%d_%H%M%S)"
+else
+    OUTPUT_DIR="$(cd "$(dirname "$OUTPUT_DIR")" 2>/dev/null && pwd)/$(basename "$OUTPUT_DIR")" || OUTPUT_DIR="$SCRIPT_DIR/$OUTPUT_DIR"
+fi
+
+# Create output directory
+mkdir -p "$OUTPUT_DIR"
+
+# Use drishti venv Python
+PYTHON="$PROJECT_ROOT/drishti/venv/bin/python3"
+if [[ ! -x "$PYTHON" ]]; then
+    echo "ERROR: Python venv not found at $PYTHON"
+    echo "Create it with: cd ../drishti && python3 -m venv venv && source venv/bin/activate && pip install protobuf"
+    exit 1
+fi
+
+echo "=============================================="
+echo "   Movement & Scan Matching Test"
+echo "=============================================="
+echo "Mode:     $MODE"
+echo "Output:   $OUTPUT_DIR"
+case $MODE in
+    linear_only)
+        echo "Distance: ${DISTANCE}m"
+        ;;
+    square)
+        echo "Size:     ${SIZE}m"
+        ;;
+    waypoints)
+        echo "Waypoints: $WAYPOINTS"
+        ;;
+esac
+echo "=============================================="
+
+# Cleanup function
+cleanup() {
+    echo ""
+    echo "Cleaning up..."
+    if [[ -n "$SANGAM_PID" ]]; then
+        kill $SANGAM_PID 2>/dev/null || true
+    fi
+    if [[ -n "$DHRUVA_PID" ]]; then
+        kill $DHRUVA_PID 2>/dev/null || true
+    fi
+    wait 2>/dev/null || true
+}
+trap cleanup EXIT
+
+# 1. Start SangamIO (mock mode)
+echo ""
+echo "[1/5] Starting SangamIO (mock mode)..."
+(cd "$PROJECT_ROOT/sangam-io" && cargo run --release --features mock -- --config mock.toml) \
+    > "$OUTPUT_DIR/sangamio.log" 2>&1 &
+SANGAM_PID=$!
+echo "  PID: $SANGAM_PID"
+sleep 2
+
+# Check if SangamIO started
+if ! kill -0 $SANGAM_PID 2>/dev/null; then
+    echo "ERROR: SangamIO failed to start. Check $OUTPUT_DIR/sangamio.log"
+    cat "$OUTPUT_DIR/sangamio.log" | tail -20
+    exit 1
+fi
+echo "  SangamIO started successfully"
+
+# 2. Start DhruvaSLAM with exploration disabled for manual control
+echo ""
+echo "[2/5] Starting DhruvaSLAM..."
+
+# Create a modified config for manual movement tests (disable exploration)
+DHRUVA_CONFIG="$OUTPUT_DIR/dhruva-movement.toml"
+cat > "$DHRUVA_CONFIG" << 'EOF'
+# DhruvaSLAM config for movement testing
+# Exploration DISABLED - we control movement via SetGoal
+
+[source]
+sangam_address = "localhost:5555"
+# Use separate UDP port for localhost testing (SangamIO sends on 5556)
+udp_port = 5556
+
+[output]
+bind_port = 5557
+odometry_rate_hz = 50
+map_rate_hz = 5.0
+feature_rate_hz = 1.0
+
+[stream]
+robot_status_hz = 10.0
+sensor_status_hz = 10.0
+map_hz = 2.0
+navigation_hz = 5.0
+
+[map_storage]
+path = "/tmp/dhruva_movement_test"
+
+[odometry]
+algorithm = "wheel"
+ticks_per_meter = 4464.0
+wheel_base = 0.233
+
+[lidar]
+mounting_x = -0.110
+mounting_y = 0.0
+optical_offset = 0.025
+angle_offset = 0.2182
+
+[preprocessing]
+min_range = 0.15
+max_range = 5.0
+target_points = 180
+min_angle_step = 0.0175
+
+[slam]
+min_match_score = 0.3
+lost_threshold = 0.1
+min_scan_points = 50
+encoder_weight = 0.8
+use_submap_matching = false
+
+[matcher]
+algorithm = "hybrid_p2l"
+always_correlative = true
+encoder_weight = 1.0
+search_window_x = 0.3
+search_window_y = 0.3
+search_window_theta = 0.5
+linear_resolution = 0.02
+angular_resolution = 0.01
+grid_resolution = 0.05
+correlative_min_score = 0.4
+icp_max_iterations = 50
+icp_translation_epsilon = 0.001
+icp_rotation_epsilon = 0.001
+icp_max_correspondence_distance = 0.5
+icp_min_correspondences = 10
+icp_outlier_ratio = 0.1
+icp_robust_kernel = "welsch"
+icp_kernel_scale = 0.1
+
+[keyframe]
+min_translation = 0.5
+min_rotation = 0.5
+max_keyframes = 1000
+min_interval_ms = 500
+
+[submap]
+scans_per_submap = 100
+overlap_scans = 10
+max_active_submaps = 2
+
+[map]
+resolution = 0.05
+initial_width = 20.0
+initial_height = 20.0
+log_odds_occupied = 0.9
+log_odds_free = -0.7
+log_odds_max = 50.0
+log_odds_min = -50.0
+occupied_threshold = 0.5
+free_threshold = -0.5
+
+[loop_closure]
+enabled = false
+
+[exploration]
+# DISABLED for movement testing - we control via SetGoal only
+# With unknown_is_obstacle=false, navigation works through unexplored areas
+enabled = false
+strategy = "frontier"
+frontier_min_size = 3
+scan_at_frontiers = true
+max_frontier_distance = 5.0
+replan_interval_ms = 5000
+stuck_timeout_ms = 10000
+recovery_distance = 0.3
+
+[navigation]
+enabled = true
+update_rate_hz = 10.0
+robot_radius = 0.18
+max_linear_vel = 0.3
+max_angular_vel = 0.5
+waypoint_threshold = 0.15
+goal_threshold = 0.08
+heading_threshold = 0.15
+unknown_is_obstacle = false  # Allow navigation through unexplored areas
+
+[session]
+state_path = "/tmp/dhruva_movement_session.json"
+auto_load_last_map = false
+EOF
+
+(cd "$PROJECT_ROOT/dhruva-slam" && RUST_LOG=info cargo run --release -- --config "$DHRUVA_CONFIG") \
+    > "$OUTPUT_DIR/dhruva.log" 2>&1 &
+DHRUVA_PID=$!
+echo "  PID: $DHRUVA_PID"
+sleep 3
+
+# Check if DhruvaSLAM started
+if ! kill -0 $DHRUVA_PID 2>/dev/null; then
+    echo "ERROR: DhruvaSLAM failed to start. Check $OUTPUT_DIR/dhruva.log"
+    cat "$OUTPUT_DIR/dhruva.log" | tail -30
+    exit 1
+fi
+echo "  DhruvaSLAM started successfully"
+
+# 3. Wait for services to be ready
+echo ""
+echo "[3/5] Waiting for services to be ready..."
+sleep 2
+
+# 4. Run movement test
+echo ""
+echo "[4/5] Running movement test..."
+
+# Build test command
+TEST_CMD="$PYTHON $SCRIPT_DIR/scripts/test_movement.py --port 5557 --mode $MODE"
+TEST_CMD="$TEST_CMD --output $OUTPUT_DIR/results.json"
+
+case $MODE in
+    linear_only)
+        TEST_CMD="$TEST_CMD --distance $DISTANCE"
+        ;;
+    square)
+        TEST_CMD="$TEST_CMD --size $SIZE"
+        ;;
+    waypoints)
+        if [[ -z "$WAYPOINTS" ]]; then
+            echo "ERROR: --waypoints required for waypoints mode"
+            exit 1
+        fi
+        TEST_CMD="$TEST_CMD --waypoints '$WAYPOINTS'"
+        ;;
+esac
+
+echo "  Command: $TEST_CMD"
+echo ""
+
+# Run test
+eval $TEST_CMD
+TEST_EXIT=$?
+
+# 5. Collect logs
+echo ""
+echo "[5/5] Collecting results..."
+
+# Kill processes before final analysis
+echo "  Stopping processes..."
+kill $SANGAM_PID $DHRUVA_PID 2>/dev/null || true
+wait 2>/dev/null || true
+unset SANGAM_PID DHRUVA_PID
+
+# Check for pose errors in logs
+echo ""
+echo "  Checking logs for issues..."
+
+SLAM_ERRORS=$(grep -c "ERROR\|WARN" "$OUTPUT_DIR/dhruva.log" 2>/dev/null || echo "0")
+echo "  SLAM log warnings/errors: $SLAM_ERRORS"
+
+# Extract key metrics from result
+if [[ -f "$OUTPUT_DIR/results.json" ]]; then
+    echo ""
+    echo "  Test results saved to: $OUTPUT_DIR/results.json"
+fi
+
+# Print summary
+echo ""
+echo "=============================================="
+echo "   Test Complete"
+echo "=============================================="
+echo "Results: $OUTPUT_DIR"
+echo ""
+echo "Files:"
+echo "  - results.json     Test metrics and pose history"
+echo "  - dhruva.log       SLAM process logs"
+echo "  - sangamio.log     Simulator logs"
+
+if [[ $TEST_EXIT -eq 0 ]]; then
+    echo ""
+    echo "Status: PASSED"
+else
+    echo ""
+    echo "Status: FAILED (exit code $TEST_EXIT)"
+fi
+
+exit $TEST_EXIT
@@ -164,9 +164,9 @@ sleep 2
 echo ""
 echo "[6/7] Collecting outputs..."

-# Copy ground truth (use the map from mock.toml - currently large_room)
-cp "$PROJECT_ROOT/sangam-io/maps/large_room.pgm" "$OUTPUT_DIR/ground_truth.pgm"
-cp "$PROJECT_ROOT/sangam-io/maps/large_room.yaml" "$OUTPUT_DIR/ground_truth.yaml"
+# Copy ground truth (use the map from mock.toml - large_room_obstacles)
+cp "$PROJECT_ROOT/sangam-io/maps/large_room_obstacles.pgm" "$OUTPUT_DIR/ground_truth.pgm"
+cp "$PROJECT_ROOT/sangam-io/maps/large_room_obstacles.yaml" "$OUTPUT_DIR/ground_truth.yaml"
 echo "  Copied ground truth map"

 # Find and copy SLAM output
@@ -202,7 +202,7 @@ if [[ -f "$OUTPUT_DIR/slam_output.pgm" ]]; then
    $PYTHON "$SCRIPT_DIR/scripts/analyze_map.py" \
        --ground-truth "$OUTPUT_DIR/ground_truth.pgm" \
        --slam-output "$OUTPUT_DIR/slam_output.pgm" \
-        --start-pose 1.0,1.0,0 \
+        --start-pose 2.5,2.5,0 \
        --resolution 0.05 \
        --output-dir "$OUTPUT_DIR"
 else