From 397eee1455d220f8f58741e293f76de72b1e9ad9 Mon Sep 17 00:00:00 2001 From: codetiger Date: Tue, 6 Jan 2026 21:22:14 +0530 Subject: [PATCH] Add DhruvaNav autonomous exploration controller New navigation controller implementing frontier-based exploration for the VacuumTiger robot vacuum. Key features: Multi-threaded architecture: - Sensor thread (~100Hz): UDP sensor reading, odometry, safety checks - Mapping thread (~5-10Hz): VastuSLAM scan matching, grid updates - Exploration thread (~20Hz): Frontier detection, path planning Path planning: - Theta* any-angle pathfinding (smoother than A*) - SIMD-optimized cost map with obstacle inflation - Path smoother for differential-drive kinematics Frontier exploration: - Flood-fill clustering of frontier cells - Normalized scoring (distance, size, info gain) - Blacklisting and recovery for stuck situations Integration: - SangamIO TCP/UDP client for robot communication - VastuSLAM for SLAM and occupancy grid mapping - SVG visualization with trajectory and frontier markers --- dhruva-nav/Cargo.toml | 28 + dhruva-nav/build.rs | 11 + dhruva-nav/dhruva.toml | 21 + dhruva-nav/proposal.md | 1204 ++++++++++++++++++++++ dhruva-nav/proto/sangamio.proto | 1 + dhruva-nav/rust-toolchain.toml | 2 + dhruva-nav/src/client.rs | 349 +++++++ dhruva-nav/src/config.rs | 275 +++++ dhruva-nav/src/error.rs | 33 + dhruva-nav/src/exploration/explorer.rs | 1277 ++++++++++++++++++++++++ dhruva-nav/src/exploration/follower.rs | 384 +++++++ dhruva-nav/src/exploration/frontier.rs | 517 ++++++++++ dhruva-nav/src/exploration/mod.rs | 14 + dhruva-nav/src/main.rs | 266 +++++ dhruva-nav/src/odometry.rs | 137 +++ dhruva-nav/src/planning/cost_map.rs | 594 +++++++++++ dhruva-nav/src/planning/mod.rs | 14 + dhruva-nav/src/planning/smoother.rs | 287 ++++++ dhruva-nav/src/planning/theta_star.rs | 422 ++++++++ dhruva-nav/src/shared.rs | 275 +++++ dhruva-nav/src/threads/exploration.rs | 254 +++++ dhruva-nav/src/threads/mapping.rs | 174 ++++ dhruva-nav/src/threads/mod.rs | 110 ++ dhruva-nav/src/threads/sensor.rs | 240 +++++ dhruva-nav/src/utils.rs | 16 + 25 files changed, 6905 insertions(+) create mode 100644 dhruva-nav/Cargo.toml create mode 100644 dhruva-nav/build.rs create mode 100644 dhruva-nav/dhruva.toml create mode 100644 dhruva-nav/proposal.md create mode 120000 dhruva-nav/proto/sangamio.proto create mode 100644 dhruva-nav/rust-toolchain.toml create mode 100644 dhruva-nav/src/client.rs create mode 100644 dhruva-nav/src/config.rs create mode 100644 dhruva-nav/src/error.rs create mode 100644 dhruva-nav/src/exploration/explorer.rs create mode 100644 dhruva-nav/src/exploration/follower.rs create mode 100644 dhruva-nav/src/exploration/frontier.rs create mode 100644 dhruva-nav/src/exploration/mod.rs create mode 100644 dhruva-nav/src/main.rs create mode 100644 dhruva-nav/src/odometry.rs create mode 100644 dhruva-nav/src/planning/cost_map.rs create mode 100644 dhruva-nav/src/planning/mod.rs create mode 100644 dhruva-nav/src/planning/smoother.rs create mode 100644 dhruva-nav/src/planning/theta_star.rs create mode 100644 dhruva-nav/src/shared.rs create mode 100644 dhruva-nav/src/threads/exploration.rs create mode 100644 dhruva-nav/src/threads/mapping.rs create mode 100644 dhruva-nav/src/threads/mod.rs create mode 100644 dhruva-nav/src/threads/sensor.rs create mode 100644 dhruva-nav/src/utils.rs diff --git a/dhruva-nav/Cargo.toml b/dhruva-nav/Cargo.toml new file mode 100644 index 0000000..acc982b --- /dev/null +++ b/dhruva-nav/Cargo.toml @@ -0,0 +1,28 @@ +[package] +name = "dhruva-nav" +version = "0.1.0" +edition = "2024" +description = "Navigation controller for VacuumTiger robot using VastuSLAM" +license = "MIT OR Apache-2.0" + +[[bin]] +name = "dhruva-nav" +path = "src/main.rs" + +[dependencies] +vastu-slam = { path = "../vastu-slam" } +prost = "0.13" +toml = "0.9" +serde = { version = "1", features = ["derive"] } +tracing = "0.1" +tracing-subscriber = { version = "0.3", features = ["env-filter"] } +thiserror = "2" + +[build-dependencies] +prost-build = "0.13" + +[profile.release] +opt-level = 3 +lto = true +codegen-units = 1 +strip = true diff --git a/dhruva-nav/build.rs b/dhruva-nav/build.rs new file mode 100644 index 0000000..6e32ec7 --- /dev/null +++ b/dhruva-nav/build.rs @@ -0,0 +1,11 @@ +//! Build script for compiling protobuf definitions + +fn main() -> Result<(), Box> { + // Compile proto files + prost_build::compile_protos(&["proto/sangamio.proto"], &["proto/"])?; + + // Rerun if proto files change + println!("cargo:rerun-if-changed=proto/sangamio.proto"); + + Ok(()) +} diff --git a/dhruva-nav/dhruva.toml b/dhruva-nav/dhruva.toml new file mode 100644 index 0000000..353e496 --- /dev/null +++ b/dhruva-nav/dhruva.toml @@ -0,0 +1,21 @@ +# DhruvaNav Configuration +# Navigation controller for VacuumTiger robot + +[connection] +# SangamIO daemon address +# Use 127.0.0.1 for local mock device, or robot IP for real hardware +robot_ip = "127.0.0.1" +port = 5555 +timeout_ms = 5000 + +[robot] +# CRL-200S robot parameters +wheel_base = 0.233 # Distance between wheels (meters) +ticks_per_meter = 4464.0 # Encoder ticks per meter of travel +max_linear_vel = 0.2 # Maximum linear velocity (m/s) +max_angular_vel = 0.5 # Maximum angular velocity (rad/s) + +[output] +# Output file paths +map_path = "output/map.vastu" +svg_path = "output/map.svg" diff --git a/dhruva-nav/proposal.md b/dhruva-nav/proposal.md new file mode 100644 index 0000000..9600876 --- /dev/null +++ b/dhruva-nav/proposal.md @@ -0,0 +1,1204 @@ +# DhruvaNav - Autonomous Exploration Proposal + +## Overview + +DhruvaNav is a navigation controller for the VacuumTiger robot vacuum. Phase 1 (predefined path testing with map recording) is complete. This proposal focuses on implementing autonomous exploration using frontier-based exploration. + +## Current Status (Phase 1 Complete) + +- SangamIO TCP/UDP client with sensor streaming +- Wheel encoder odometry with VastuSLAM pose correction +- Rectangular path executor with proportional control +- Occupancy grid mapping using VastuSLAM +- Safety stops on bumper/cliff triggers +- Map saving to `.vastu` and `.svg` formats + +## Architecture + +``` +┌────────────────────────────────────────────────────────────────────┐ +│ DhruvaNav Application │ +├────────────────────────────────────────────────────────────────────┤ +│ ┌─────────────────┐ ┌─────────────────┐ ┌────────────────────┐ │ +│ │ Explorer │ │ Path Planner │ │ Motion Controller │ │ +│ │ (Frontier-based)│ │ (A*/RRT) │ │ (Velocity Control) │ │ +│ └────────┬────────┘ └────────┬────────┘ └─────────┬──────────┘ │ +│ │ │ │ │ +│ ┌────────▼────────────────────▼─────────────────────▼──────────┐ │ +│ │ State Machine │ │ +│ │ INIT → EXPLORING → NAVIGATING → EXPLORING → ... → DONE │ │ +│ └──────────────────────────────┬────────────────────────────────┘ │ +│ │ │ +│ ┌──────────────────────────────▼────────────────────────────────┐ │ +│ │ Mapper (VastuSLAM) │ │ +│ │ • Scan matching for pose correction │ │ +│ │ • Occupancy grid updates │ │ +│ │ • Frontier detection from grid │ │ +│ └──────────────────────────────┬────────────────────────────────┘ │ +│ │ │ +│ ┌──────────────────────────────▼────────────────────────────────┐ │ +│ │ SangamIO Client │ │ +│ │ • TCP: Commands (velocity, stop, actuators) │ │ +│ │ • UDP: Sensors (lidar @ 5Hz, encoders @ 110Hz) │ │ +│ └───────────────────────────────────────────────────────────────┘ │ +└────────────────────────────────────────────────────────────────────┘ + │ + │ Network (Port 5555) + ▼ +┌────────────────────────────────────────────────────────────────────┐ +│ SangamIO Daemon │ +│ (Hardware Abstraction Layer) │ +└────────────────────────────────────────────────────────────────────┘ +``` + +--- + +## Frontier-Based Exploration Algorithm + +### Concept + +Frontier-based exploration autonomously maps unknown environments by: +1. Identifying **frontiers** - boundaries between explored (free) and unexplored (unknown) space +2. Selecting the best frontier based on distance, size, and information gain +3. Planning a collision-free path to the selected frontier +4. Navigating to the frontier while updating the map +5. Repeating until no frontiers remain (full coverage) + +### Algorithm Flow + +``` +┌──────────────────────────────────────────────────────────────────┐ +│ EXPLORATION LOOP │ +├──────────────────────────────────────────────────────────────────┤ +│ │ +│ ┌─────────────┐ │ +│ │ Update Map │◄────────────────────────────────────────────┐ │ +│ │ (lidar scan)│ │ │ +│ └──────┬──────┘ │ │ +│ ▼ │ │ +│ ┌──────────────┐ No frontiers ┌────────────────────┐ │ │ +│ │ Detect │───────────────────►│ DONE: Full │ │ │ +│ │ Frontiers │ │ Coverage! │ │ │ +│ └──────┬───────┘ └────────────────────┘ │ │ +│ │ Frontiers exist │ │ +│ ▼ │ │ +│ ┌──────────────┐ │ │ +│ │ Score & │ │ │ +│ │ Select │ │ │ +│ │ Frontier │ │ │ +│ └──────┬───────┘ │ │ +│ ▼ │ │ +│ ┌──────────────┐ No path found ┌────────────────────┐ │ │ +│ │ Plan Path │───────────────────►│ Mark frontier as │───┘ │ +│ │ to Frontier │ │ unreachable │ │ +│ └──────┬───────┘ └────────────────────┘ │ +│ │ Path found │ +│ ▼ │ +│ ┌──────────────┐ │ +│ │ Navigate │ │ +│ │ Along Path │────────────────────────────────────────────────┘ +│ └──────────────┘ (continuous map updates during navigation) +│ │ +└──────────────────────────────────────────────────────────────────┘ +``` + +--- + +## Component Design + +### 1. Frontier Detector + +Identifies frontiers from the occupancy grid by finding free cells adjacent to unknown cells. + +```rust +/// A frontier is a connected region of free cells adjacent to unknown space +pub struct Frontier { + /// Cells that make up this frontier + pub cells: Vec, + /// Centroid in world coordinates (navigation target) + pub centroid: WorldPoint, + /// Size in number of cells + pub size: usize, + /// Average distance from robot (for scoring) + pub distance: f32, +} + +pub struct FrontierDetector { + /// Minimum cells to consider a valid frontier + min_frontier_size: usize, // default: 5 + /// Grid resolution for distance calculation + resolution: f32, +} + +impl FrontierDetector { + /// Detect all frontiers in the current map + pub fn detect(&self, grid: &GridStorage, robot_pose: Pose2D) -> Vec; + + /// Check if a cell is a frontier cell (free with unknown neighbor) + fn is_frontier_cell(&self, grid: &GridStorage, coord: GridCoord) -> bool; + + /// Group adjacent frontier cells into connected regions + fn cluster_frontiers(&self, frontier_cells: Vec) -> Vec; +} +``` + +**Frontier Detection Algorithm:** +``` +for each cell in grid: + if cell is FREE and has UNKNOWN neighbor: + mark as frontier cell + +cluster frontier cells using flood-fill: + for each unvisited frontier cell: + BFS to find all connected frontier cells + create Frontier with cells, compute centroid + +filter frontiers by minimum size +compute distance from robot to each frontier centroid +``` + +### 2. Frontier Scorer + +Ranks frontiers to determine exploration priority. + +```rust +pub struct FrontierScorer { + /// Weight for distance (closer = better) + distance_weight: f32, // default: 1.0 + /// Weight for size (larger = more information gain) + size_weight: f32, // default: 0.5 + /// Weight for heading alignment (facing frontier = better) + alignment_weight: f32, // default: 0.3 +} + +impl FrontierScorer { + /// Score a frontier (higher = better target) + pub fn score(&self, frontier: &Frontier, robot_pose: Pose2D) -> f32 { + let distance_score = 1.0 / (1.0 + frontier.distance); + let size_score = (frontier.size as f32).ln(); + let alignment_score = self.compute_alignment(frontier, robot_pose); + + self.distance_weight * distance_score + + self.size_weight * size_score + + self.alignment_weight * alignment_score + } + + /// Select the best frontier + pub fn select_best(&self, frontiers: &[Frontier], robot_pose: Pose2D) -> Option<&Frontier>; +} +``` + +**Scoring Factors:** +| Factor | Rationale | +|--------|-----------| +| Distance | Closer frontiers reduce travel time | +| Size | Larger frontiers reveal more unknown area | +| Alignment | Frontiers in current heading require less rotation | +| Reachability | Penalize previously failed frontiers | + +### 3. Cost Map + +Pre-computes traversability and wall-distance costs for efficient path planning. + +```rust +/// Cost map for path planning with obstacle inflation and wall distance penalty +pub struct CostMap { + /// Grid dimensions + width: usize, + height: usize, + /// Resolution in meters per cell + resolution: f32, + /// Cost values: 0 = free, 255 = lethal obstacle + costs: Vec, + /// Distance to nearest obstacle (in cells) for each cell + distance_field: Vec, +} + +/// Cost thresholds +pub const COST_FREE: u8 = 0; // Safe to traverse +pub const COST_NEAR_OBSTACLE: u8 = 50; // Close to wall, penalized +pub const COST_INSCRIBED: u8 = 254; // Within robot radius, avoid +pub const COST_LETHAL: u8 = 255; // Obstacle or unknown, blocked + +impl CostMap { + /// Build cost map from occupancy grid + pub fn from_grid( + grid: &GridStorage, + robot_radius: f32, // 0.17m + safety_margin: f32, // 0.05m extra clearance + wall_penalty_dist: f32, // 0.30m - penalize within this distance + ) -> Self; + + /// Update cost map incrementally when grid changes + pub fn update_region(&mut self, grid: &GridStorage, min: GridCoord, max: GridCoord); + + /// Get traversal cost at a grid coordinate + pub fn cost(&self, coord: GridCoord) -> u8; + + /// Check if line-of-sight is clear (for Theta*) + pub fn line_of_sight(&self, from: GridCoord, to: GridCoord) -> bool; + + /// Get distance to nearest obstacle (for wall avoidance) + pub fn obstacle_distance(&self, coord: GridCoord) -> f32; +} +``` + +**Cost Map Layers:** + +``` +┌─────────────────────────────────────────────────────────────────┐ +│ COST MAP COMPUTATION │ +├─────────────────────────────────────────────────────────────────┤ +│ │ +│ 1. LETHAL LAYER (cost = 255) │ +│ • All cells with CellType: Wall, Cliff, Bump, Unknown │ +│ │ +│ 2. INSCRIBED LAYER (cost = 254) │ +│ • Cells within robot_radius of any lethal cell │ +│ • Robot center cannot enter these cells │ +│ │ +│ 3. INFLATION LAYER (cost = 50-253) │ +│ • Cells within safety_margin beyond inscribed radius │ +│ • Exponential decay: cost = 50 * exp(-distance/decay) │ +│ │ +│ 4. WALL PENALTY LAYER (cost = 1-49) │ +│ • Cells within wall_penalty_dist of obstacles │ +│ • Linear penalty: closer to wall = higher cost │ +│ • Encourages paths through center of corridors │ +│ │ +│ 5. FREE LAYER (cost = 0) │ +│ • All remaining traversable cells │ +│ │ +└─────────────────────────────────────────────────────────────────┘ +``` + +**Distance Field Computation (Brushfire Algorithm):** + +``` +1. Initialize distance_field with 0 for obstacles, ∞ for others +2. Add all obstacle cells to queue +3. BFS expansion: + for each cell in queue: + for each neighbor: + new_dist = distance_field[cell] + 1 + if new_dist < distance_field[neighbor]: + distance_field[neighbor] = new_dist + add neighbor to queue +4. Convert cell distances to meters: dist_m = dist_cells * resolution +``` + +### 4. Path Planner (Theta* Algorithm) + +Plans any-angle paths that avoid walls with safe clearance. + +**Why Theta\* over A\*:** +- A* produces grid-constrained paths (45°/90° turns only) +- Theta* allows any-angle paths → smoother, shorter +- Better suited for differential drive robots +- Wall distance penalty ensures safe clearance + +```rust +pub struct PathPlanner { + /// Pre-computed cost map + cost_map: CostMap, + /// Robot radius (for collision checking) + robot_radius: f32, + /// Maximum iterations before giving up + max_iterations: usize, + /// Wall penalty weight in path cost + wall_penalty_weight: f32, +} + +impl PathPlanner { + /// Create planner with cost map + pub fn new(cost_map: CostMap, robot_radius: f32) -> Self; + + /// Plan path using Theta* algorithm + pub fn plan( + &self, + start: Pose2D, + goal: WorldPoint, + ) -> Option; + + /// Check if direct line-of-sight exists (no obstacles) + fn line_of_sight(&self, from: GridCoord, to: GridCoord) -> bool; + + /// Compute path cost including wall penalty + fn path_cost(&self, from: GridCoord, to: GridCoord) -> f32; + + /// Update cost map when occupancy grid changes + pub fn update_cost_map(&mut self, grid: &GridStorage); +} + +pub struct PlannedPath { + /// Waypoints in world coordinates + pub waypoints: Vec, + /// Total path length in meters + pub length: f32, + /// Total path cost (includes wall penalties) + pub cost: f32, +} +``` + +**Theta\* Algorithm:** + +``` +Theta* extends A* with line-of-sight checks for any-angle paths: + +function theta_star(start, goal): + open_set = priority_queue() + open_set.push(start, heuristic(start, goal)) + + g_score[start] = 0 + parent[start] = start + + while not open_set.empty(): + current = open_set.pop() + + if current == goal: + return reconstruct_path(goal) + + for neighbor in current.neighbors_8(): + if cost_map.cost(neighbor) >= COST_INSCRIBED: + continue // Skip blocked cells + + // KEY DIFFERENCE FROM A*: Check line-of-sight to grandparent + if line_of_sight(parent[current], neighbor): + // Path 2: Direct from grandparent (any-angle) + new_g = g_score[parent[current]] + + path_cost(parent[current], neighbor) + new_parent = parent[current] + else: + // Path 1: Through current (grid-constrained) + new_g = g_score[current] + path_cost(current, neighbor) + new_parent = current + + if new_g < g_score[neighbor]: + g_score[neighbor] = new_g + parent[neighbor] = new_parent + f_score = new_g + heuristic(neighbor, goal) + open_set.push(neighbor, f_score) + + return None // No path found + +function path_cost(from, to): + distance = euclidean_distance(from, to) + // Sample points along line to accumulate wall penalty + wall_penalty = integrate_wall_penalty(from, to) + return distance + wall_penalty_weight * wall_penalty + +function line_of_sight(from, to): + // Bresenham's line algorithm + for each cell on line from→to: + if cost_map.cost(cell) >= COST_INSCRIBED: + return false + return true +``` + +**Wall Avoidance via Path Cost:** + +``` +Path cost = distance + wall_penalty_weight × wall_penalty + +wall_penalty for segment (A → B): + penalty = 0 + for each cell along line A→B: + dist_to_wall = cost_map.obstacle_distance(cell) + if dist_to_wall < wall_penalty_dist: + penalty += (wall_penalty_dist - dist_to_wall) / wall_penalty_dist + return penalty + +Effect: Paths through corridor centers are cheaper than hugging walls +``` + +### 5. Path Smoother (Differential Drive) + +Post-processes Theta* paths for differential drive kinematics. + +```rust +pub struct PathSmoother { + /// Maximum linear velocity (m/s) + max_linear_vel: f32, + /// Maximum angular velocity (rad/s) + max_angular_vel: f32, + /// Minimum turning radius (m) = max_linear_vel / max_angular_vel + min_turn_radius: f32, + /// Lookahead distance for smoothing + lookahead: f32, +} + +impl PathSmoother { + /// Smooth path for differential drive constraints + pub fn smooth(&self, path: &PlannedPath) -> SmoothedPath; + + /// Insert arc segments at sharp turns + fn insert_turn_arcs(&self, waypoints: &[WorldPoint]) -> Vec; + + /// Check if turn angle exceeds threshold requiring in-place rotation + fn needs_point_turn(&self, angle: f32) -> bool; +} + +pub enum PathSegment { + /// Straight line segment + Line { start: WorldPoint, end: WorldPoint }, + /// Arc segment for smooth turns + Arc { center: WorldPoint, radius: f32, start_angle: f32, end_angle: f32 }, + /// In-place rotation (for sharp turns > 90°) + PointTurn { position: WorldPoint, from_angle: f32, to_angle: f32 }, +} + +pub struct SmoothedPath { + /// Path segments (lines, arcs, point turns) + pub segments: Vec, + /// Estimated traversal time + pub estimated_time: f32, +} +``` + +**Smoothing Algorithm:** + +``` +1. Analyze turn angles at each waypoint +2. For each turn: + - If angle < 30°: Keep as-is (Theta* already smooth) + - If 30° < angle < 90°: Insert arc with min_turn_radius + - If angle > 90°: Insert point turn (rotate in place) +3. Verify smoothed path still avoids obstacles +4. Return segments with velocity profiles +``` + +**Differential Drive Constraints:** + +``` +┌─────────────────────────────────────────────────────────────────┐ +│ DIFFERENTIAL DRIVE KINEMATICS │ +├─────────────────────────────────────────────────────────────────┤ +│ │ +│ Minimum turn radius: R_min = v_max / ω_max │ +│ │ +│ For CRL-200S robot: │ +│ v_max = 0.2 m/s │ +│ ω_max = 0.5 rad/s │ +│ R_min = 0.2 / 0.5 = 0.4 m │ +│ │ +│ Sharp turns (> 90°): │ +│ - Cannot achieve with continuous motion at R_min │ +│ - Must stop, rotate in place, then continue │ +│ │ +└─────────────────────────────────────────────────────────────────┘ +``` + +### 6. Path Follower + +Follows planned paths with obstacle avoidance. + +```rust +pub struct PathFollower { + /// Planned path waypoints + path: Vec, + /// Current waypoint index + current_index: usize, + /// Lookahead distance for pure pursuit + lookahead_distance: f32, // default: 0.3m + /// Waypoint reached tolerance + waypoint_tolerance: f32, // default: 0.1m +} + +impl PathFollower { + /// Set a new path to follow + pub fn set_path(&mut self, path: Vec); + + /// Compute velocity command using pure pursuit + pub fn compute_velocity(&self, robot_pose: Pose2D) -> (f32, f32); + + /// Check if current path segment is blocked by new obstacle + pub fn is_path_blocked(&self, grid: &GridStorage) -> bool; + + /// Check if path is complete + pub fn is_complete(&self) -> bool; +} +``` + +**Pure Pursuit Controller:** +``` +1. Find lookahead point on path at distance L from robot +2. Compute curvature to reach lookahead point +3. Convert curvature to (linear, angular) velocity +4. Apply velocity limits +``` + +### 7. Exploration State Machine + +Orchestrates the exploration process. + +```rust +pub enum ExplorationState { + /// Initial state, waiting to start + Init, + /// Scanning environment, detecting frontiers + Scanning, + /// Planning path to selected frontier + Planning, + /// Navigating to frontier + Navigating, + /// Recovering from navigation failure + Recovering, + /// Exploration complete, no frontiers remain + Complete, + /// Safety stop (bumper/cliff triggered) + SafetyStop, +} + +pub struct Explorer { + state: ExplorationState, + frontier_detector: FrontierDetector, + frontier_scorer: FrontierScorer, + path_planner: PathPlanner, + path_follower: PathFollower, + + /// Currently targeted frontier + current_target: Option, + /// Frontiers that couldn't be reached + blacklisted_frontiers: HashSet, + /// Statistics + stats: ExplorationStats, +} + +pub struct ExplorationStats { + pub frontiers_visited: usize, + pub total_distance: f32, + pub coverage_percent: f32, + pub time_elapsed: Duration, +} +``` + +**State Transitions:** + +``` +┌────────┐ +│ Init │ +└───┬────┘ + │ Start exploration + ▼ +┌─────────┐◄─────────────────────────────────────┐ +│Scanning │ │ +└────┬────┘ │ + │ │ + ├─── No frontiers ──► [Complete] │ + │ │ + │ Frontiers found │ + ▼ │ +┌──────────┐ │ +│ Planning │ │ +└────┬─────┘ │ + │ │ + ├─── No path ──► Blacklist frontier ─────────┤ + │ │ + │ Path found │ + ▼ │ +┌────────────┐ │ +│ Navigating │ │ +└─────┬──────┘ │ + │ │ + ├─── Reached frontier ──────────────────────┘ + │ + ├─── Path blocked ──► [Recovering] ──► [Scanning] + │ + └─── Bumper/Cliff ──► [SafetyStop] +``` + +--- + +## Configuration + +```toml +# dhruva.toml + +[connection] +robot_ip = "127.0.0.1" +port = 5555 +timeout_ms = 5000 + +[robot] +wheel_base = 0.233 +ticks_per_meter = 4464.0 +max_linear_vel = 0.2 +max_angular_vel = 0.5 +robot_radius = 0.17 + +[exploration] +# Frontier detection +min_frontier_size = 5 # Minimum cells for valid frontier +frontier_update_interval = 2.0 # Seconds between frontier updates + +# Frontier scoring weights +distance_weight = 1.0 +size_weight = 0.5 +alignment_weight = 0.3 + +# Termination +max_exploration_time = 600 # Maximum time in seconds (0 = unlimited) +return_to_start = true # Navigate back to start position when done + +[path_planning] +# Cost map parameters +safety_margin = 0.05 # Extra clearance beyond robot radius (meters) +wall_penalty_distance = 0.30 # Penalize paths within this distance of walls (meters) +wall_penalty_weight = 2.0 # Weight for wall proximity penalty in path cost + +# Theta* algorithm +max_iterations = 50000 # Maximum search iterations before giving up +max_path_length = 20.0 # Maximum path length (meters) + +# Path smoothing for differential drive +enable_smoothing = true # Post-process paths for smooth turns +sharp_turn_threshold = 1.57 # Angle (rad) above which to insert point turn (~90°) +arc_turn_threshold = 0.52 # Angle (rad) above which to insert arc (~30°) + +[navigation] +# Pure pursuit path following +lookahead_distance = 0.3 # Lookahead for path following (meters) +waypoint_tolerance = 0.10 # Distance to consider waypoint reached (meters) +goal_tolerance = 0.15 # Distance to consider goal reached (meters) + +# Replanning +replan_on_obstacle = true # Replan if path becomes blocked +replan_interval = 5.0 # Seconds between replan checks during navigation + +[output] +map_path = "output/map.vastu" +svg_path = "output/map.svg" +save_interval = 30 # Auto-save every N seconds (0 = disabled) +``` + +--- + +## Project Structure + +``` +dhruva-nav/ +├── Cargo.toml +├── dhruva.toml # Configuration +├── build.rs # Protobuf compilation +├── proto/ +│ └── sangamio.proto # Symlink to sangam-io/proto +└── src/ + ├── main.rs # Entry point, main loop + ├── config.rs # Configuration loading + ├── error.rs # Error types + │ + ├── client.rs # SangamIO TCP/UDP client + ├── odometry.rs # Wheel encoder odometry + ├── mapper.rs # VastuSLAM integration + │ + ├── planning/ # Path planning module + │ ├── mod.rs # Module exports + │ ├── cost_map.rs # Cost map with obstacle inflation + │ ├── theta_star.rs # Theta* path planner + │ └── smoother.rs # Differential drive path smoother + │ + ├── exploration/ # Exploration module + │ ├── mod.rs # Module exports + │ ├── frontier.rs # Frontier detection & clustering + │ ├── scorer.rs # Frontier scoring & selection + │ ├── follower.rs # Pure pursuit path following + │ └── explorer.rs # Exploration state machine + │ + └── path.rs # Predefined path (kept for testing) +``` + +--- + +## Implementation Phases + +### Phase 2A: Cost Map & Distance Field + +**Goal:** Build cost map with obstacle inflation and wall distance penalties + +**Tasks:** +1. Implement `CostMap` structure with cost layers +2. Implement Brushfire algorithm for distance field computation +3. Add obstacle inflation (inscribed + safety margin) +4. Add wall penalty layer for path cost calculation +5. Implement `line_of_sight()` using Bresenham's algorithm + +**Validation:** +- Distance field correctly computed from obstacles +- Inscribed radius blocks cells within robot footprint +- Wall penalty decreases with distance from obstacles +- Line-of-sight correctly detects obstacles + +### Phase 2B: Theta* Path Planner + +**Goal:** Plan any-angle paths with wall avoidance + +**Tasks:** +1. Implement Theta* algorithm with priority queue +2. Integrate wall penalty into path cost function +3. Add path cost computation along line segments +4. Implement goal region detection (not just single cell) + +**Validation:** +- Paths are any-angle (not grid-constrained) +- Paths prefer corridor centers over wall-hugging +- Returns None for unreachable goals +- Performance: <500ms for 10m paths + +### Phase 2C: Path Smoother + +**Goal:** Post-process paths for differential drive kinematics + +**Tasks:** +1. Implement turn angle analysis at waypoints +2. Insert arc segments for medium turns (30-90°) +3. Insert point turns for sharp turns (>90°) +4. Verify smoothed path clearance from obstacles + +**Validation:** +- Arcs respect minimum turning radius (0.4m) +- Point turns inserted for sharp corners +- Smoothed paths remain collision-free + +### Phase 2D: Path Follower + +**Goal:** Follow planned paths with pure pursuit + +**Tasks:** +1. Implement `PathFollower` with pure pursuit algorithm +2. Handle different segment types (line, arc, point turn) +3. Add path blocking detection +4. Integrate with existing velocity control + +**Validation:** +- Robot follows paths smoothly +- Handles transitions between segment types +- Detects when obstacles block planned path +- Reaches waypoints within tolerance + +### Phase 2E: Frontier Detection + +**Goal:** Detect and cluster frontiers from occupancy grid + +**Tasks:** +1. Implement frontier cell detection (Floor + Unknown neighbor) +2. Implement BFS clustering for connected frontiers +3. Compute frontier centroids and sizes +4. Add frontier visualization to SVG output + +**Validation:** +- Frontiers correctly identified at free/unknown boundaries +- Clusters are properly formed (no fragmentation) +- Centroid calculation is accurate + +### Phase 2F: Exploration Integration + +**Goal:** Full autonomous exploration with return-to-start + +**Tasks:** +1. Implement `Explorer` state machine +2. Implement `FrontierScorer` with configurable weights +3. Add frontier blacklisting for unreachable targets +4. Implement return-to-start navigation on completion +5. Add exploration statistics tracking + +**Validation:** +- Explores entire environment autonomously +- Handles recovery from blocked paths +- Returns to starting position when done +- Terminates when no frontiers remain + +--- + +## Testing Strategy + +### Unit Tests + +```rust +#[cfg(test)] +mod tests { + // Frontier detection + #[test] + fn test_frontier_cell_detection() { ... } + + #[test] + fn test_frontier_clustering() { ... } + + // Path planning + #[test] + fn test_astar_simple_path() { ... } + + #[test] + fn test_astar_around_obstacle() { ... } + + #[test] + fn test_no_path_to_enclosed_goal() { ... } + + // Path following + #[test] + fn test_pure_pursuit_straight_line() { ... } + + #[test] + fn test_pure_pursuit_curve() { ... } +} +``` + +### Integration Tests with Mock Device + +```bash +# Terminal 1: Start SangamIO mock +cd sangam-io +cargo run --release -- --config mock.toml + +# Terminal 2: Run exploration +cd dhruva-nav +cargo run --release +``` + +**Test Scenarios:** + +| Scenario | Map | Expected Behavior | +|----------|-----|-------------------| +| Empty room | `empty_room.yaml` | Quick full coverage | +| Single obstacle | `room_with_box.yaml` | Navigate around obstacle | +| Multiple obstacles | `large_room_obstacles.yaml` | Complex frontier selection | +| Maze | `maze.yaml` | Backtracking, recovery | +| Narrow passages | `narrow_corridors.yaml` | Path planning through tight spaces | + +--- + +## Success Criteria + +### Functional Requirements + +- [ ] Detects frontiers correctly from occupancy grid +- [ ] Plans collision-free paths to frontiers +- [ ] Follows paths with smooth motion +- [ ] Handles path blocking and replanning +- [ ] Explores until no frontiers remain +- [ ] Achieves >90% coverage in test environments +- [ ] Stops safely on bumper/cliff triggers +- [ ] Saves map periodically and on completion + +### Performance Requirements + +- [ ] Frontier detection < 100ms for 10m x 10m map +- [ ] Path planning < 500ms for 10m path +- [ ] Control loop maintains 20Hz update rate +- [ ] Memory usage < 50MB for typical room + +### Robustness Requirements + +- [ ] Recovers from navigation failures +- [ ] Handles dynamic obstacles (replanning) +- [ ] Continues after safety stop and resume +- [ ] Handles connection loss gracefully + +--- + +## SIMD Optimizations + +DhruvaNav uses `std::simd` (portable SIMD) following VastuSLAM's patterns for high-performance algorithms. + +### VastuSLAM SIMD Patterns (Reference) + +VastuSLAM provides these SIMD primitives that DhruvaNav can reuse: + +```rust +// Core SIMD types (LANES = 4 for ARM NEON / SSE) +use std::simd::{f32x4, i32x4, u8x16}; + +// VastuSLAM SIMD utilities +use vastu_slam::core::simd::{ + PointCloud, // SoA point storage + RotationMatrix4, // Pre-computed rotation + transform_points_simd4, // Batch point transformation + world_to_grid_simd4, // Batch coordinate conversion + distance_squared_simd4, // Batch distance calculation + is_valid_coords_simd4, // Batch bounds checking +}; +``` + +**Key Patterns:** +1. **Structure-of-Arrays (SoA)**: Store X and Y separately for SIMD loads +2. **Padding to LANES**: Always pad arrays to multiples of 4 (f32x4) or 16 (u8x16) +3. **Pre-computed constants**: Broadcast scalars to SIMD vectors outside loops +4. **Scratch buffers**: Reuse allocations for repeated operations + +### DhruvaNav SIMD Opportunities + +| Component | SIMD Potential | Approach | +|-----------|---------------|----------| +| Cost Map - Distance Field | HIGH | Batch distance comparisons during Brushfire | +| Cost Map - Obstacle Inflation | HIGH | SIMD threshold checks for inflation radius | +| Frontier Detection | HIGH | u8x16 comparison for Floor+Unknown neighbors | +| Path Cost Integration | MEDIUM | Batch wall distance lookups along path | +| Theta* Heuristic | LOW | Single point, limited benefit | +| Path Follower | LOW | Single robot pose | + +### 1. SIMD Distance Field (Brushfire) + +```rust +use std::simd::{f32x4, cmp::SimdPartialOrd}; + +const LANES: usize = 4; + +/// SIMD-optimized distance field propagation +pub fn propagate_distances_simd( + distances: &mut [f32], + width: usize, + height: usize, +) { + let sqrt2 = f32x4::splat(1.414); + let one = f32x4::splat(1.0); + + // Process 4 cells at a time (row-wise) + for y in 1..height-1 { + let row_start = y * width; + + for x in (1..width-1).step_by(LANES) { + if x + LANES > width - 1 { break; } + + // Load current distances + let curr = f32x4::from_slice(&distances[row_start + x..]); + + // Load neighbor distances (requires careful indexing) + let north = f32x4::from_slice(&distances[row_start - width + x..]); + let south = f32x4::from_slice(&distances[row_start + width + x..]); + + // Compute minimum of (neighbor + cost) + let from_north = north + one; + let from_south = south + one; + + let min_dist = curr.simd_min(from_north).simd_min(from_south); + + // Store updated distances + min_dist.copy_to_slice(&mut distances[row_start + x..]); + } + } +} +``` + +### 2. SIMD Frontier Detection + +```rust +use std::simd::{u8x16, cmp::SimdPartialEq}; + +/// Batch check for frontier cells (Floor with Unknown neighbor) +pub fn detect_frontier_cells_simd( + cell_types: &[u8], + width: usize, + frontier_mask: &mut [bool], +) { + let floor_vec = u8x16::splat(CellType::Floor as u8); + let unknown_vec = u8x16::splat(CellType::Unknown as u8); + + // Process 16 cells at a time + for chunk_start in (0..cell_types.len()).step_by(16) { + if chunk_start + 16 > cell_types.len() { break; } + + let cells = u8x16::from_slice(&cell_types[chunk_start..]); + + // Check if cell is Floor + let is_floor = cells.simd_eq(floor_vec); + + // For each Floor cell, check neighbors for Unknown + // (neighbor checking done separately due to irregular access) + + let floor_mask = is_floor.to_bitmask(); + for i in 0..16 { + if floor_mask & (1 << i) != 0 { + let idx = chunk_start + i; + // Check 4 neighbors for Unknown + frontier_mask[idx] = has_unknown_neighbor(cell_types, idx, width); + } + } + } +} +``` + +### 3. SIMD Path Cost Integration + +```rust +/// SIMD-optimized wall penalty along path segment +pub fn integrate_wall_penalty_simd( + distance_field: &[f32], + path_xs: &[f32], // X coordinates along path (padded to LANES) + path_ys: &[f32], // Y coordinates along path + width: usize, + resolution: f32, + penalty_distance: f32, +) -> f32 { + let n = path_xs.len(); + debug_assert!(n % LANES == 0); + + let inv_res = f32x4::splat(1.0 / resolution); + let penalty_dist = f32x4::splat(penalty_distance); + let zero = f32x4::splat(0.0); + + let mut total_penalty = f32x4::splat(0.0); + + for i in (0..n).step_by(LANES) { + let px = f32x4::from_slice(&path_xs[i..]); + let py = f32x4::from_slice(&path_ys[i..]); + + // Convert to grid indices + let gx = (px * inv_res).floor(); + let gy = (py * inv_res).floor(); + + // Gather distances (scalar fallback for irregular access) + let mut dists = [0.0f32; LANES]; + for j in 0..LANES { + let idx = (gy.to_array()[j] as usize) * width + (gx.to_array()[j] as usize); + dists[j] = distance_field.get(idx).copied().unwrap_or(0.0); + } + let dist_vec = f32x4::from_array(dists); + + // Compute penalty: max(0, penalty_distance - distance) / penalty_distance + let penalty = (penalty_dist - dist_vec).simd_max(zero) / penalty_dist; + total_penalty += penalty; + } + + // Horizontal sum + total_penalty.reduce_sum() +} +``` + +### 4. SIMD Centroid Calculation + +```rust +/// SIMD-optimized frontier centroid calculation +pub fn compute_centroid_simd( + xs: &[i32], // Grid X coordinates (padded) + ys: &[i32], // Grid Y coordinates (padded) + count: usize, +) -> (f32, f32) { + let n = xs.len(); + debug_assert!(n % LANES == 0); + + let mut sum_x = i32x4::splat(0); + let mut sum_y = i32x4::splat(0); + + for i in (0..n).step_by(LANES) { + sum_x += i32x4::from_slice(&xs[i..]); + sum_y += i32x4::from_slice(&ys[i..]); + } + + let total_x = sum_x.reduce_sum() as f32; + let total_y = sum_y.reduce_sum() as f32; + + (total_x / count as f32, total_y / count as f32) +} +``` + +### 5. Scratch Buffers Pattern + +Following VastuSLAM's pattern, reuse allocations: + +```rust +/// Pre-allocated buffers for SIMD operations +pub struct PlannerScratch { + /// World coordinates (padded to LANES) + pub world_xs: Vec, + pub world_ys: Vec, + /// Grid coordinates + pub grid_xs: Vec, + pub grid_ys: Vec, + /// Distance values + pub distances: Vec, + /// Valid flags + pub valid: Vec, +} + +impl PlannerScratch { + pub fn new(capacity: usize) -> Self { + let padded = capacity.div_ceil(LANES) * LANES; + Self { + world_xs: vec![0.0; padded], + world_ys: vec![0.0; padded], + grid_xs: vec![0; padded], + grid_ys: vec![0; padded], + distances: vec![0.0; padded], + valid: vec![false; padded], + } + } + + pub fn ensure_capacity(&mut self, capacity: usize) { + let padded = capacity.div_ceil(LANES) * LANES; + if self.world_xs.len() < padded { + self.world_xs.resize(padded, 0.0); + self.world_ys.resize(padded, 0.0); + self.grid_xs.resize(padded, 0); + self.grid_ys.resize(padded, 0); + self.distances.resize(padded, 0.0); + self.valid.resize(padded, false); + } + } +} +``` + +### Performance Targets with SIMD + +| Operation | Scalar Target | SIMD Target | Speedup | +|-----------|---------------|-------------|---------| +| Distance field (100x100) | 50ms | 15ms | 3x | +| Frontier detection | 20ms | 5ms | 4x | +| Path cost (100 samples) | 2ms | 0.5ms | 4x | +| Centroid (500 cells) | 0.5ms | 0.1ms | 5x | + +### Implementation Notes + +1. **Nightly Rust required**: Uses `#![feature(portable_simd)]` +2. **Fallback**: `std::simd` auto-generates scalar code when SIMD unavailable +3. **ARM NEON**: Primary target (Allwinner A33 in CRL-200S robot) +4. **Testing**: Compare SIMD vs scalar results in unit tests (like VastuSLAM) + +--- + +## Dependencies + +```toml +[dependencies] +vastu-slam = { path = "../vastu-slam" } +prost = "0.13" +toml = "0.9" +serde = { version = "1", features = ["derive"] } +tracing = "0.1" +tracing-subscriber = { version = "0.3", features = ["env-filter"] } +thiserror = "2" + +# NEW for exploration +priority-queue = "2" # A* open set +rustc-hash = "2" # Fast HashSet for visited nodes + +[build-dependencies] +prost-build = "0.13" +``` + +--- + +## Open Questions + +1. **Frontier Refresh Rate:** How often should frontiers be recalculated during navigation? + - Option A: Every lidar scan (most accurate, highest CPU) + - Option B: Fixed interval (1-2 seconds) + - Option C: Only when reaching waypoints + +2. **Unknown Space Treatment:** Should the robot plan paths through unknown space? + - Option A: Only through known-free (safest) + - Option B: Allow unknown if no free path (more aggressive) + +3. **Coverage Metric:** How to measure exploration completeness? + - Option A: Percentage of grid cells explored + - Option B: Ratio of explored area to bounding box + - Option C: No more frontiers above minimum size + +4. **Return to Start:** Should robot return to starting position after exploration? + - Could be useful for docking in vacuum cleaner context + +--- + +## References + +- Yamauchi, B. (1997). "A frontier-based approach for autonomous exploration" +- VastuSLAM documentation: `vastu-slam/README.md` +- SangamIO protocol: `sangam-io/proto/sangamio.proto` diff --git a/dhruva-nav/proto/sangamio.proto b/dhruva-nav/proto/sangamio.proto new file mode 120000 index 0000000..d28816c --- /dev/null +++ b/dhruva-nav/proto/sangamio.proto @@ -0,0 +1 @@ +../../sangam-io/proto/sangamio.proto \ No newline at end of file diff --git a/dhruva-nav/rust-toolchain.toml b/dhruva-nav/rust-toolchain.toml new file mode 100644 index 0000000..5d56faf --- /dev/null +++ b/dhruva-nav/rust-toolchain.toml @@ -0,0 +1,2 @@ +[toolchain] +channel = "nightly" diff --git a/dhruva-nav/src/client.rs b/dhruva-nav/src/client.rs new file mode 100644 index 0000000..2af5f5f --- /dev/null +++ b/dhruva-nav/src/client.rs @@ -0,0 +1,349 @@ +//! SangamIO TCP client for receiving sensor data and sending commands. +//! +//! Adapted from dhruva-slam/src/io/sangam_client.rs + +use crate::error::{DhruvaError, Result}; +use prost::Message as ProstMessage; +use std::collections::HashMap; +use std::io::Write; +use std::net::{TcpStream, UdpSocket}; +use std::time::Duration; + +// Include generated protobuf types +pub mod proto { + #[allow(clippy::enum_variant_names)] + pub mod sangamio { + include!(concat!(env!("OUT_DIR"), "/sangamio.rs")); + } +} + +/// Sensor value types (converted from protobuf) +#[derive(Debug, Clone)] +pub enum SensorValue { + Bool(bool), + U32(u32), + PointCloud2D(Vec), +} + +/// A single LiDAR point: (angle_rad, distance_m, quality) +pub type LidarPoint = (f32, f32, u8); + +/// Parsed message from SangamIO +#[derive(Debug)] +pub struct SensorMessage { + pub group_id: String, + pub values: HashMap, +} + +impl SensorMessage { + /// Create from protobuf message + fn from_proto(msg: proto::sangamio::Message) -> Result { + match msg.payload { + Some(proto::sangamio::message::Payload::SensorGroup(sg)) => { + let values: HashMap = sg + .values + .into_iter() + .filter_map(|(k, v)| { + let val = match v.value { + Some(proto::sangamio::sensor_value::Value::BoolVal(b)) => { + SensorValue::Bool(b) + } + Some(proto::sangamio::sensor_value::Value::U32Val(u)) => { + SensorValue::U32(u) + } + Some(proto::sangamio::sensor_value::Value::PointcloudVal(pc)) => { + SensorValue::PointCloud2D( + pc.points + .into_iter() + .map(|p| (p.angle_rad, p.distance_m, p.quality as u8)) + .collect(), + ) + } + _ => return None, + }; + Some((k, val)) + }) + .collect(); + + Ok(Self { + group_id: sg.group_id, + values, + }) + } + Some(proto::sangamio::message::Payload::Command(_)) => { + Err(DhruvaError::Protocol("Received command from server".into())) + } + None => Err(DhruvaError::Protocol("Empty payload".into())), + } + } + + /// Check if this is a sensor status message + pub fn is_sensor_status(&self) -> bool { + self.group_id == "sensor_status" + } + + /// Check if this is a lidar message + pub fn is_lidar(&self) -> bool { + self.group_id == "lidar" + } + + /// Extract lidar point cloud if this is a lidar message + pub fn as_lidar(&self) -> Option<&Vec> { + if !self.is_lidar() { + return None; + } + if let Some(SensorValue::PointCloud2D(points)) = self.values.get("scan") { + Some(points) + } else { + None + } + } + + /// Extract wheel encoder ticks (left, right) + pub fn encoder_ticks(&self) -> Option<(u16, u16)> { + if !self.is_sensor_status() { + return None; + } + let left = match self.values.get("wheel_left") { + Some(SensorValue::U32(v)) => *v as u16, + _ => return None, + }; + let right = match self.values.get("wheel_right") { + Some(SensorValue::U32(v)) => *v as u16, + _ => return None, + }; + Some((left, right)) + } + + /// Extract bumper state (left, right) + pub fn bumper_state(&self) -> Option<(bool, bool)> { + if !self.is_sensor_status() { + return None; + } + let left = match self.values.get("bumper_left") { + Some(SensorValue::Bool(v)) => *v, + _ => false, + }; + let right = match self.values.get("bumper_right") { + Some(SensorValue::Bool(v)) => *v, + _ => false, + }; + Some((left, right)) + } + + /// Extract cliff sensor states (left_side, left_front, right_front, right_side) + pub fn cliff_state(&self) -> Option<(bool, bool, bool, bool)> { + if !self.is_sensor_status() { + return None; + } + let left_side = match self.values.get("cliff_left_side") { + Some(SensorValue::Bool(v)) => *v, + _ => false, + }; + let left_front = match self.values.get("cliff_left_front") { + Some(SensorValue::Bool(v)) => *v, + _ => false, + }; + let right_front = match self.values.get("cliff_right_front") { + Some(SensorValue::Bool(v)) => *v, + _ => false, + }; + let right_side = match self.values.get("cliff_right_side") { + Some(SensorValue::Bool(v)) => *v, + _ => false, + }; + Some((left_side, left_front, right_front, right_side)) + } +} + +/// Default buffer size (64KB) +const DEFAULT_BUFFER_SIZE: usize = 65536; + +/// TCP/UDP client for SangamIO daemon +pub struct SangamClient { + stream: TcpStream, + udp_socket: UdpSocket, + buffer: Vec, +} + +impl SangamClient { + /// Connect with timeout (uses same port for TCP commands and UDP sensor receiving) + pub fn connect_timeout(addr: &str, timeout: Duration) -> Result { + let sock_addr: std::net::SocketAddr = addr + .parse() + .map_err(|e| DhruvaError::Config(format!("Invalid address: {}", e)))?; + + // Use the same port for UDP as TCP (they're different protocols, can coexist) + Self::connect_with_udp_port(addr, timeout, sock_addr.port()) + } + + /// Connect with timeout and custom UDP port + pub fn connect_with_udp_port(addr: &str, timeout: Duration, udp_port: u16) -> Result { + let addr: std::net::SocketAddr = addr + .parse() + .map_err(|e| DhruvaError::Config(format!("Invalid address: {}", e)))?; + let stream = TcpStream::connect_timeout(&addr, timeout)?; + + // Bind UDP socket to receive sensor data from SangamIO + // TCP and UDP can use the same port number (different protocols) + let udp_bind_addr = format!("0.0.0.0:{}", udp_port); + let udp_socket = UdpSocket::bind(&udp_bind_addr).map_err(|e| { + DhruvaError::Config(format!("Failed to bind UDP to {}: {}", udp_bind_addr, e)) + })?; + udp_socket.set_nonblocking(true)?; + + tracing::info!("UDP socket bound to port {} for sensor data", udp_port); + + Ok(Self { + stream, + udp_socket, + buffer: vec![0u8; DEFAULT_BUFFER_SIZE], + }) + } + + /// Set read timeout for TCP (UDP remains non-blocking for buffer draining) + pub fn set_timeout(&mut self, timeout: Option) -> Result<()> { + self.stream.set_read_timeout(timeout)?; + // Keep UDP non-blocking to allow draining the buffer efficiently + // The main loop will drain all available messages before processing + self.udp_socket.set_nonblocking(true)?; + Ok(()) + } + + /// Receive sensor message from UDP (non-blocking) + pub fn recv_udp(&mut self) -> Result> { + match self.udp_socket.recv(&mut self.buffer) { + Ok(len) => { + if len < 4 { + return Ok(None); + } + // Read length prefix + let msg_len = u32::from_be_bytes([ + self.buffer[0], + self.buffer[1], + self.buffer[2], + self.buffer[3], + ]) as usize; + + if len < 4 + msg_len { + return Err(DhruvaError::Protocol("Incomplete UDP message".into())); + } + + // Decode protobuf + let proto_msg = proto::sangamio::Message::decode(&self.buffer[4..4 + msg_len])?; + SensorMessage::from_proto(proto_msg).map(Some) + } + Err(e) if e.kind() == std::io::ErrorKind::WouldBlock => Ok(None), + Err(e) if e.kind() == std::io::ErrorKind::TimedOut => Ok(None), + Err(e) => Err(DhruvaError::Connection(e)), + } + } + + /// Send drive velocity command + pub fn send_drive_command(&mut self, linear: f32, angular: f32) -> Result<()> { + use proto::sangamio::{ + ComponentAction, ComponentControl, Message as ProtoMessage, + command::Command as CommandPayload, component_action::ActionType, + }; + + let mut config = HashMap::new(); + config.insert( + "linear".to_string(), + proto::sangamio::SensorValue { + value: Some(proto::sangamio::sensor_value::Value::F32Val(linear)), + }, + ); + config.insert( + "angular".to_string(), + proto::sangamio::SensorValue { + value: Some(proto::sangamio::sensor_value::Value::F32Val(angular)), + }, + ); + + let command = ProtoMessage { + topic: "command".to_string(), + payload: Some(proto::sangamio::message::Payload::Command( + proto::sangamio::Command { + command: Some(CommandPayload::ComponentControl(ComponentControl { + id: "drive".to_string(), + action: Some(ComponentAction { + r#type: ActionType::Configure as i32, + config, + }), + })), + }, + )), + }; + + self.send_proto(&command) + } + + /// Send stop command (set velocity to zero) + pub fn send_stop(&mut self) -> Result<()> { + self.send_drive_command(0.0, 0.0) + } + + /// Enable the drive component (must call before sending velocity commands) + pub fn enable_drive(&mut self) -> Result<()> { + use proto::sangamio::{ + ComponentAction, ComponentControl, Message as ProtoMessage, + command::Command as CommandPayload, component_action::ActionType, + }; + + let command = ProtoMessage { + topic: "command".to_string(), + payload: Some(proto::sangamio::message::Payload::Command( + proto::sangamio::Command { + command: Some(CommandPayload::ComponentControl(ComponentControl { + id: "drive".to_string(), + action: Some(ComponentAction { + r#type: ActionType::Enable as i32, + config: HashMap::new(), + }), + })), + }, + )), + }; + + self.send_proto(&command) + } + + /// Enable the lidar component + pub fn enable_lidar(&mut self) -> Result<()> { + use proto::sangamio::{ + ComponentAction, ComponentControl, Message as ProtoMessage, + command::Command as CommandPayload, component_action::ActionType, + }; + + let command = ProtoMessage { + topic: "command".to_string(), + payload: Some(proto::sangamio::message::Payload::Command( + proto::sangamio::Command { + command: Some(CommandPayload::ComponentControl(ComponentControl { + id: "lidar".to_string(), + action: Some(ComponentAction { + r#type: ActionType::Enable as i32, + config: HashMap::new(), + }), + })), + }, + )), + }; + + self.send_proto(&command) + } + + /// Send raw protobuf message + fn send_proto(&mut self, msg: &proto::sangamio::Message) -> Result<()> { + let encoded = msg.encode_to_vec(); + let len = encoded.len() as u32; + + // Write length prefix (big-endian) + self.stream.write_all(&len.to_be_bytes())?; + // Write payload + self.stream.write_all(&encoded)?; + self.stream.flush()?; + + Ok(()) + } +} diff --git a/dhruva-nav/src/config.rs b/dhruva-nav/src/config.rs new file mode 100644 index 0000000..43ea704 --- /dev/null +++ b/dhruva-nav/src/config.rs @@ -0,0 +1,275 @@ +//! Configuration loading for DhruvaNav + +use crate::error::{DhruvaError, Result}; +use serde::Deserialize; +use std::path::Path; + +/// Main configuration structure +#[derive(Clone, Debug, Deserialize)] +pub struct DhruvaConfig { + pub connection: ConnectionConfig, + pub robot: RobotConfig, + #[serde(default)] + pub exploration: ExplorationConfig, + pub output: OutputConfig, +} + +/// Network connection settings +#[derive(Clone, Debug, Deserialize)] +pub struct ConnectionConfig { + /// Robot IP address (default: 127.0.0.1 for local mock) + #[serde(default = "default_robot_ip")] + pub robot_ip: String, + + /// TCP port number (default: 5555) + #[serde(default = "default_port")] + pub port: u16, + + /// Connection timeout in milliseconds (default: 5000) + #[serde(default = "default_timeout")] + pub timeout_ms: u64, +} + +/// Robot physical parameters +#[derive(Clone, Debug, Deserialize)] +pub struct RobotConfig { + /// Distance between wheels in meters (default: 0.233) + #[serde(default = "default_wheel_base")] + pub wheel_base: f32, + + /// Encoder ticks per meter of travel (default: 4464.0) + #[serde(default = "default_ticks_per_meter")] + pub ticks_per_meter: f32, + + /// Maximum linear velocity in m/s (default: 0.2) + #[serde(default = "default_max_linear")] + pub max_linear_vel: f32, + + /// Maximum angular velocity in rad/s (default: 0.5) + #[serde(default = "default_max_angular")] + pub max_angular_vel: f32, +} + +/// Output configuration +#[derive(Clone, Debug, Deserialize)] +pub struct OutputConfig { + /// Path to save the map file + #[serde(default = "default_map_path")] + pub map_path: String, + + /// Path to save SVG visualization + #[serde(default = "default_svg_path")] + pub svg_path: String, +} + +/// Exploration configuration +#[derive(Clone, Debug, Deserialize)] +pub struct ExplorationConfig { + /// Robot radius for obstacle inflation (meters) + #[serde(default = "default_robot_radius")] + pub robot_radius: f32, + + /// Safety margin beyond robot radius (meters) + #[serde(default = "default_safety_margin")] + pub safety_margin: f32, + + /// Wall penalty distance for path planning (meters) + #[serde(default = "default_wall_penalty_distance")] + pub wall_penalty_distance: f32, + + /// Minimum frontier cluster size + #[serde(default = "default_min_frontier_size")] + pub min_frontier_size: usize, + + /// Frontier refresh interval (seconds) + #[serde(default = "default_frontier_refresh")] + pub frontier_refresh_interval: f32, + + /// Return to start position when complete + #[serde(default = "default_return_to_start")] + pub return_to_start: bool, + + /// Goal tolerance for waypoints (meters) + #[serde(default = "default_goal_tolerance")] + pub goal_tolerance: f32, + + // --- Recovery and stuck detection parameters --- + /// Maximum replans allowed for a single frontier before blacklisting + #[serde(default = "default_max_replans_per_frontier")] + pub max_replans_per_frontier: usize, + + /// Timeout for making progress toward frontier (seconds) + #[serde(default = "default_frontier_timeout_secs")] + pub frontier_timeout_secs: f32, + + /// Minimum distance to consider progress made (meters) + #[serde(default = "default_min_progress_distance")] + pub min_progress_distance: f32, + + /// Cooldown before blacklisted frontier can be retried (seconds) + #[serde(default = "default_blacklist_cooldown_secs")] + pub blacklist_cooldown_secs: f32, + + /// Distance threshold to consider robot stuck (meters) + #[serde(default = "default_stuck_distance_threshold")] + pub stuck_distance_threshold: f32, + + /// Time window for stuck detection (seconds) + #[serde(default = "default_stuck_time_window_secs")] + pub stuck_time_window_secs: f32, + + /// Distance to back up during recovery (meters) + #[serde(default = "default_recovery_backup_distance")] + pub recovery_backup_distance: f32, + + /// Angle to rotate during recovery (radians) + #[serde(default = "default_recovery_rotation_rad")] + pub recovery_rotation_rad: f32, + + /// Maximum recovery attempts per frontier before blacklisting + #[serde(default = "default_max_recovery_attempts")] + pub max_recovery_attempts: usize, +} + +impl Default for ExplorationConfig { + fn default() -> Self { + Self { + robot_radius: default_robot_radius(), + safety_margin: default_safety_margin(), + wall_penalty_distance: default_wall_penalty_distance(), + min_frontier_size: default_min_frontier_size(), + frontier_refresh_interval: default_frontier_refresh(), + return_to_start: default_return_to_start(), + goal_tolerance: default_goal_tolerance(), + // Recovery and stuck detection + max_replans_per_frontier: default_max_replans_per_frontier(), + frontier_timeout_secs: default_frontier_timeout_secs(), + min_progress_distance: default_min_progress_distance(), + blacklist_cooldown_secs: default_blacklist_cooldown_secs(), + stuck_distance_threshold: default_stuck_distance_threshold(), + stuck_time_window_secs: default_stuck_time_window_secs(), + recovery_backup_distance: default_recovery_backup_distance(), + recovery_rotation_rad: default_recovery_rotation_rad(), + max_recovery_attempts: default_max_recovery_attempts(), + } + } +} + +// Default value functions +fn default_robot_ip() -> String { + "127.0.0.1".to_string() +} +fn default_port() -> u16 { + 5555 +} +fn default_timeout() -> u64 { + 5000 +} +fn default_wheel_base() -> f32 { + 0.233 +} +fn default_ticks_per_meter() -> f32 { + 4464.0 +} +fn default_max_linear() -> f32 { + 0.2 +} +fn default_max_angular() -> f32 { + 0.5 +} +fn default_map_path() -> String { + "output/map.vastu".to_string() +} +fn default_svg_path() -> String { + "output/map.svg".to_string() +} + +// Exploration defaults +fn default_robot_radius() -> f32 { + 0.10 +} +fn default_safety_margin() -> f32 { + 0.15 +} // 25cm total clearance from robot center +fn default_wall_penalty_distance() -> f32 { + 0.30 +} // Penalize paths within 30cm of walls +fn default_min_frontier_size() -> usize { + 5 +} +fn default_frontier_refresh() -> f32 { + 1.5 +} +fn default_return_to_start() -> bool { + true +} +fn default_goal_tolerance() -> f32 { + 0.15 +} + +// Recovery and stuck detection defaults +fn default_max_replans_per_frontier() -> usize { + 3 +} +fn default_frontier_timeout_secs() -> f32 { + 60.0 +} +fn default_min_progress_distance() -> f32 { + 0.10 +} +fn default_blacklist_cooldown_secs() -> f32 { + 120.0 +} +fn default_stuck_distance_threshold() -> f32 { + 0.02 +} +fn default_stuck_time_window_secs() -> f32 { + 3.0 +} +fn default_recovery_backup_distance() -> f32 { + 0.10 +} +fn default_recovery_rotation_rad() -> f32 { + 0.5 +} +fn default_max_recovery_attempts() -> usize { + 2 +} + +impl Default for DhruvaConfig { + fn default() -> Self { + Self { + connection: ConnectionConfig { + robot_ip: default_robot_ip(), + port: default_port(), + timeout_ms: default_timeout(), + }, + robot: RobotConfig { + wheel_base: default_wheel_base(), + ticks_per_meter: default_ticks_per_meter(), + max_linear_vel: default_max_linear(), + max_angular_vel: default_max_angular(), + }, + exploration: ExplorationConfig::default(), + output: OutputConfig { + map_path: default_map_path(), + svg_path: default_svg_path(), + }, + } + } +} + +impl DhruvaConfig { + /// Load configuration from a TOML file + pub fn load(path: &Path) -> Result { + let content = std::fs::read_to_string(path) + .map_err(|e| DhruvaError::Config(format!("Failed to read config file: {}", e)))?; + let config: DhruvaConfig = toml::from_str(&content)?; + Ok(config) + } + + /// Get the full address string for connection + pub fn address(&self) -> String { + format!("{}:{}", self.connection.robot_ip, self.connection.port) + } +} diff --git a/dhruva-nav/src/error.rs b/dhruva-nav/src/error.rs new file mode 100644 index 0000000..bfe138a --- /dev/null +++ b/dhruva-nav/src/error.rs @@ -0,0 +1,33 @@ +//! Error types for DhruvaNav + +use thiserror::Error; + +/// DhruvaNav error type +#[derive(Error, Debug)] +pub enum DhruvaError { + #[error("Connection failed: {0}")] + Connection(#[from] std::io::Error), + + #[error("Protocol error: {0}")] + Protocol(String), + + #[error("Configuration error: {0}")] + Config(String), + + #[error("SLAM error: {0}")] + Slam(String), +} + +impl From for DhruvaError { + fn from(e: prost::DecodeError) -> Self { + DhruvaError::Protocol(e.to_string()) + } +} + +impl From for DhruvaError { + fn from(e: toml::de::Error) -> Self { + DhruvaError::Config(e.to_string()) + } +} + +pub type Result = std::result::Result; diff --git a/dhruva-nav/src/exploration/explorer.rs b/dhruva-nav/src/exploration/explorer.rs new file mode 100644 index 0000000..a33ce91 --- /dev/null +++ b/dhruva-nav/src/exploration/explorer.rs @@ -0,0 +1,1277 @@ +//! Autonomous exploration state machine. +//! +//! Integrates frontier detection, path planning, and path following +//! for complete autonomous exploration of unknown environments. + +use std::collections::{HashMap, HashSet}; +use std::time::{Duration, Instant}; + +use vastu_slam::{GridStorage, Pose2D, WorldPoint}; + +use crate::planning::{CostMap, PathSmoother, SmoothedPath, ThetaStarPlanner}; +use crate::utils::normalize_angle; + +use super::follower::{FollowerConfig, PathFollower}; +use super::frontier::{Frontier, FrontierConfig, FrontierDetector, FrontierId}; + +/// Configuration for the explorer. +#[derive(Clone, Debug)] +pub struct ExplorerConfig { + /// Frontier detection configuration + pub frontier: FrontierConfig, + /// Path follower configuration + pub follower: FollowerConfig, + /// Robot radius for cost map (meters) + pub robot_radius: f32, + /// Safety margin beyond robot radius (meters) + pub safety_margin: f32, + /// Wall penalty distance (meters) + pub wall_penalty_distance: f32, + /// Interval between frontier re-evaluation (seconds) + pub frontier_refresh_interval: f32, + /// Maximum number of planning failures before giving up on a frontier + pub max_planning_failures: usize, + /// Whether to return to start position when exploration is complete + pub return_to_start: bool, + + // --- Recovery and stuck detection parameters --- + /// Maximum replans allowed for a single frontier before blacklisting + pub max_replans_per_frontier: usize, + /// Timeout for making progress toward frontier (seconds) + pub frontier_timeout_secs: f32, + /// Minimum distance to consider progress made (meters) + pub min_progress_distance: f32, + /// Cooldown before blacklisted frontier can be retried (seconds) + pub blacklist_cooldown_secs: f32, + /// Distance threshold to consider robot stuck (meters) + pub stuck_distance_threshold: f32, + /// Time window for stuck detection (seconds) + pub stuck_time_window_secs: f32, + /// Distance to back up during recovery (meters) + pub recovery_backup_distance: f32, + /// Angle to rotate during recovery (radians) + pub recovery_rotation_rad: f32, + /// Maximum recovery attempts per frontier before blacklisting + pub max_recovery_attempts: usize, +} + +impl Default for ExplorerConfig { + fn default() -> Self { + Self { + frontier: FrontierConfig::default(), + follower: FollowerConfig::default(), + robot_radius: 0.10, + safety_margin: 0.05, + wall_penalty_distance: 0.20, + frontier_refresh_interval: 1.5, + max_planning_failures: 3, + return_to_start: true, + // Recovery and stuck detection defaults + max_replans_per_frontier: 3, + frontier_timeout_secs: 60.0, + min_progress_distance: 0.10, + blacklist_cooldown_secs: 30.0, + stuck_distance_threshold: 0.02, + stuck_time_window_secs: 3.0, + recovery_backup_distance: 0.10, + recovery_rotation_rad: 0.5, + max_recovery_attempts: 2, + } + } +} + +/// Phase of recovery behavior. +#[derive(Clone, Copy, Debug, PartialEq, Eq)] +pub enum RecoveryPhase { + /// Backing up from obstacle + BackingUp, + /// Rotating to new heading + Rotating, + /// Recovery complete + Done, +} + +/// State of the exploration process. +#[derive(Clone, Copy, Debug, PartialEq, Eq)] +pub enum ExplorationState { + /// Initial state, waiting to start + Idle, + /// Detecting frontiers and selecting target + SelectingFrontier, + /// Planning path to selected frontier + Planning, + /// Following path to frontier + Navigating, + /// Re-planning due to obstacle or blocked path + Replanning, + /// Executing recovery behavior (backup + rotate) + Recovering, + /// Escaping to a clear area before retrying exhausted frontiers + EscapingToRetry, + /// Returning to starting position + ReturningHome, + /// Exploration complete + Complete, + /// Exploration failed (no path to any frontier) + Failed, +} + +/// Result of an exploration step. +#[derive(Clone, Debug)] +pub struct ExplorationStep { + /// Current state + pub state: ExplorationState, + /// Velocity command (linear, angular) + pub velocity: (f32, f32), + /// Current target frontier (if any) + pub target_frontier: Option, + /// Number of frontiers remaining + pub frontiers_remaining: usize, +} + +/// Autonomous explorer. +pub struct Explorer { + config: ExplorerConfig, + /// Current exploration state + state: ExplorationState, + /// Frontier detector + frontier_detector: FrontierDetector, + /// Path planner + planner: ThetaStarPlanner, + /// Path smoother + smoother: PathSmoother, + /// Path follower + follower: PathFollower, + /// Current cost map + cost_map: Option, + /// Current frontiers + frontiers: Vec, + /// Current target frontier index + current_frontier_idx: usize, + /// Current path + current_path: Option, + /// Starting position + start_pose: Option, + /// Last frontier refresh time + last_frontier_refresh: Instant, + /// Planning failure count for current frontier + planning_failures: usize, + + // --- Per-frontier tracking --- + /// ID of current frontier being navigated to + current_frontier_id: Option, + /// Time when navigation to current frontier started + frontier_start_time: Option, + /// Number of replans for current frontier + replan_count_for_frontier: usize, + /// Number of recovery attempts for current frontier + recovery_attempts_for_frontier: usize, + + // --- Progress tracking --- + /// Time of last progress toward goal + last_progress_time: Instant, + /// Pose at last progress check + last_progress_pose: Option, + /// Last recorded distance to goal (for progress detection) + last_distance_to_goal: Option, + + // --- Stuck detection --- + /// Start time for stuck detection window + stuck_check_start: Instant, + /// Pose at start of stuck detection window + stuck_check_pose: Option, + + // --- Frontier blacklist --- + /// Blacklisted frontiers with timestamp when blacklisted + blacklisted_frontiers: HashMap, + /// Permanently blacklisted frontiers (exhausted too many times) + permanent_blacklist: HashSet, + /// Count of exhaustion cycles per frontier + exhaustion_count: HashMap, + /// Whether we've tried escaping to a clear area before permanent blacklist + tried_escape_for_exhausted: bool, + + // --- Recovery state --- + /// Current phase of recovery behavior + recovery_phase: RecoveryPhase, + /// Pose at start of recovery + recovery_start_pose: Option, +} + +impl Explorer { + /// Create a new explorer with configuration. + pub fn new(config: ExplorerConfig) -> Self { + let now = Instant::now(); + Self { + frontier_detector: FrontierDetector::new(config.frontier.clone()), + planner: ThetaStarPlanner::with_defaults(), + smoother: PathSmoother::with_defaults(), + follower: PathFollower::new(config.follower.clone()), + config, + state: ExplorationState::Idle, + cost_map: None, + frontiers: Vec::new(), + current_frontier_idx: 0, + current_path: None, + start_pose: None, + last_frontier_refresh: now, + planning_failures: 0, + // Per-frontier tracking + current_frontier_id: None, + frontier_start_time: None, + replan_count_for_frontier: 0, + recovery_attempts_for_frontier: 0, + // Progress tracking + last_progress_time: now, + last_progress_pose: None, + last_distance_to_goal: None, + // Stuck detection + stuck_check_start: now, + stuck_check_pose: None, + // Blacklist + blacklisted_frontiers: HashMap::new(), + permanent_blacklist: HashSet::new(), + exhaustion_count: HashMap::new(), + tried_escape_for_exhausted: false, + // Recovery state + recovery_phase: RecoveryPhase::Done, + recovery_start_pose: None, + } + } + + /// Start exploration from current position. + pub fn start(&mut self, current_pose: Pose2D) { + let now = Instant::now(); + self.start_pose = Some(current_pose); + self.state = ExplorationState::SelectingFrontier; + self.last_frontier_refresh = now; + self.planning_failures = 0; + // Reset per-frontier tracking + self.current_frontier_id = None; + self.frontier_start_time = None; + self.replan_count_for_frontier = 0; + self.recovery_attempts_for_frontier = 0; + // Reset progress tracking + self.last_progress_time = now; + self.last_progress_pose = Some(current_pose); + self.last_distance_to_goal = None; + // Reset stuck detection + self.stuck_check_start = now; + self.stuck_check_pose = Some(current_pose); + // Clear blacklist on fresh start + self.blacklisted_frontiers.clear(); + self.permanent_blacklist.clear(); + self.exhaustion_count.clear(); + self.tried_escape_for_exhausted = false; + // Reset recovery state + self.recovery_phase = RecoveryPhase::Done; + self.recovery_start_pose = None; + tracing::info!( + "Exploration started at ({:.2}, {:.2})", + current_pose.x, + current_pose.y + ); + } + + /// Check if exploration is complete. + pub fn is_complete(&self) -> bool { + matches!( + self.state, + ExplorationState::Complete | ExplorationState::Failed + ) + } + + /// Update exploration with new map data. + /// + /// Returns velocity command and state information. + pub fn update(&mut self, grid: &GridStorage, current_pose: Pose2D) -> ExplorationStep { + // Update cost map from grid + // Use robot_radius + safety_margin as the inscribed radius for path planning + // This ensures paths stay at least safety_margin away from all obstacles + let planning_radius = self.config.robot_radius + self.config.safety_margin; + let cost_map = CostMap::from_grid( + grid, + planning_radius, // Larger inscribed zone (blocked) + 0.10, // Additional inflation for wall penalty + self.config.wall_penalty_distance, + ); + self.cost_map = Some(cost_map); + + // Check if we need to refresh frontiers + let should_refresh = self.last_frontier_refresh.elapsed() + > Duration::from_secs_f32(self.config.frontier_refresh_interval); + + match self.state { + ExplorationState::Idle => ExplorationStep { + state: self.state, + velocity: (0.0, 0.0), + target_frontier: None, + frontiers_remaining: 0, + }, + + ExplorationState::SelectingFrontier => self.select_frontier(grid, current_pose), + + ExplorationState::Planning => self.plan_path(current_pose), + + ExplorationState::Navigating => self.navigate(grid, current_pose, should_refresh), + + ExplorationState::Replanning => self.replan(current_pose), + + ExplorationState::ReturningHome => self.return_home(current_pose), + + ExplorationState::Recovering => self.execute_recovery(current_pose), + + ExplorationState::EscapingToRetry => self.escape_to_retry(current_pose), + + ExplorationState::Complete | ExplorationState::Failed => ExplorationStep { + state: self.state, + velocity: (0.0, 0.0), + target_frontier: None, + frontiers_remaining: 0, + }, + } + } + + /// Select a frontier to explore. + fn select_frontier(&mut self, grid: &GridStorage, current_pose: Pose2D) -> ExplorationStep { + // Clean up expired blacklist entries first (before borrowing cost_map) + self.clean_blacklist(); + + let cost_map = self.cost_map.as_ref().unwrap(); + + // Detect frontiers + let all_frontiers = self.frontier_detector.detect(grid, cost_map, current_pose); + self.last_frontier_refresh = Instant::now(); + + // Filter out blacklisted and permanently blacklisted frontiers + let available_frontiers: Vec = all_frontiers + .into_iter() + .filter(|f| { + !self.is_blacklisted(&f.id()) && !self.permanent_blacklist.contains(&f.id()) + }) + .collect(); + + let blacklisted_count = self.blacklisted_frontiers.len(); + if blacklisted_count > 0 { + tracing::debug!( + "Filtered frontiers: {} available, {} blacklisted", + available_frontiers.len(), + blacklisted_count + ); + } + + // If all frontiers are blacklisted, increment exhaustion count and handle escape/permanent blacklist + if available_frontiers.is_empty() && blacklisted_count > 0 { + tracing::warn!( + "All {} frontiers blacklisted, clearing blacklist and retrying", + blacklisted_count + ); + + // Increment exhaustion count for all currently blacklisted frontiers + let blacklisted_ids: Vec<_> = self.blacklisted_frontiers.keys().cloned().collect(); + let mut any_exhausted = false; + for frontier_id in &blacklisted_ids { + let count = self + .exhaustion_count + .entry(frontier_id.clone()) + .or_insert(0); + *count += 1; + if *count >= 3 { + any_exhausted = true; + } + } + + // If frontiers are exhausted and we haven't tried escaping, escape first + if any_exhausted && !self.tried_escape_for_exhausted && self.start_pose.is_some() { + tracing::warn!( + "Frontiers exhausted from current position, escaping to start to retry" + ); + self.tried_escape_for_exhausted = true; + // Clear temporary blacklist so we can retry after escaping + self.blacklisted_frontiers.clear(); + self.state = ExplorationState::EscapingToRetry; + self.follower.clear_path(); + return ExplorationStep { + state: self.state, + velocity: (0.0, 0.0), + target_frontier: None, + frontiers_remaining: blacklisted_count, + }; + } + + // Already tried escaping or no start pose, permanently blacklist exhausted frontiers + for frontier_id in blacklisted_ids { + let count = *self.exhaustion_count.get(&frontier_id).unwrap_or(&0); + if count >= 3 { + tracing::warn!( + "Frontier at ({}, {}) exhausted {} times, permanently blacklisting", + frontier_id.centroid.x, + frontier_id.centroid.y, + count + ); + self.permanent_blacklist.insert(frontier_id); + } + } + + // Clear temporary blacklist + self.blacklisted_frontiers.clear(); + + // Re-detect, filtering out permanently blacklisted + let new_frontiers = self.frontier_detector.detect(grid, cost_map, current_pose); + self.frontiers = new_frontiers + .into_iter() + .filter(|f| !self.permanent_blacklist.contains(&f.id())) + .collect(); + + if self.frontiers.is_empty() && !self.permanent_blacklist.is_empty() { + tracing::info!( + "No reachable frontiers remaining ({} permanently blacklisted)", + self.permanent_blacklist.len() + ); + } + } else { + self.frontiers = available_frontiers; + } + + if self.frontiers.is_empty() { + // No more frontiers - exploration complete or return home + if self.config.return_to_start && self.start_pose.is_some() { + tracing::info!("No frontiers remaining, returning to start"); + self.state = ExplorationState::ReturningHome; + } else { + tracing::info!("Exploration complete: no frontiers remaining"); + self.state = ExplorationState::Complete; + } + + return ExplorationStep { + state: self.state, + velocity: (0.0, 0.0), + target_frontier: None, + frontiers_remaining: 0, + }; + } + + tracing::debug!("Found {} frontiers", self.frontiers.len()); + self.current_frontier_idx = 0; + self.planning_failures = 0; + + // Reset per-frontier tracking for the new frontier + let frontier_id = self.frontiers[0].id(); + self.reset_frontier_tracking(frontier_id); + + self.state = ExplorationState::Planning; + + ExplorationStep { + state: self.state, + velocity: (0.0, 0.0), + target_frontier: Some(self.frontiers[0].world_centroid), + frontiers_remaining: self.frontiers.len(), + } + } + + /// Plan path to current frontier. + fn plan_path(&mut self, current_pose: Pose2D) -> ExplorationStep { + let cost_map = self.cost_map.as_ref().unwrap(); + + if self.current_frontier_idx >= self.frontiers.len() { + // Tried all frontiers, none reachable + tracing::warn!("No reachable frontiers found"); + self.state = ExplorationState::Failed; + + return ExplorationStep { + state: self.state, + velocity: (0.0, 0.0), + target_frontier: None, + frontiers_remaining: 0, + }; + } + + let frontier = &self.frontiers[self.current_frontier_idx]; + + // Get navigation target, handling case where no reachable point exists + let target = + match self + .frontier_detector + .navigation_target(frontier, cost_map, current_pose) + { + Some(t) => t, + None => { + tracing::debug!( + "No reachable point for frontier {}, trying next", + self.current_frontier_idx + ); + self.current_frontier_idx += 1; + return self.plan_path(current_pose); // Recursive call to try next frontier + } + }; + + tracing::debug!( + "Planning path to frontier {} at ({:.2}, {:.2})", + self.current_frontier_idx, + target.x, + target.y + ); + + // Plan path + match self.planner.plan(cost_map, current_pose, target) { + Some(path) => { + // Check for valid path + if path.waypoints.len() < 2 { + tracing::warn!( + "Path has only {} waypoints, skipping frontier", + path.waypoints.len() + ); + self.current_frontier_idx += 1; + return self.plan_path(current_pose); + } + + // Smooth the path + let smoothed = self.smoother.smooth(&path); + + // Check for valid smoothed path + if smoothed.segments.is_empty() { + tracing::warn!("Smoothed path has no segments, skipping frontier"); + self.current_frontier_idx += 1; + return self.plan_path(current_pose); + } + + tracing::info!( + "Path planned: {} waypoints -> {} segments, {:.2}m, {:.1}s", + path.waypoints.len(), + smoothed.segments.len(), + smoothed.total_length, + smoothed.estimated_time + ); + + // Log first few waypoints for debugging + for (i, wp) in path.waypoints.iter().take(3).enumerate() { + tracing::debug!(" waypoint {}: ({:.2}, {:.2})", i, wp.x, wp.y); + } + + self.follower.set_path(smoothed.clone()); + self.current_path = Some(smoothed); + self.planning_failures = 0; + self.state = ExplorationState::Navigating; + + ExplorationStep { + state: self.state, + velocity: (0.0, 0.0), + target_frontier: Some(target), + frontiers_remaining: self.frontiers.len() - self.current_frontier_idx, + } + } + None => { + tracing::debug!( + "Failed to plan path to frontier {}", + self.current_frontier_idx + ); + + self.planning_failures += 1; + + if self.planning_failures >= self.config.max_planning_failures { + // Try next frontier + self.current_frontier_idx += 1; + self.planning_failures = 0; + } + + ExplorationStep { + state: self.state, + velocity: (0.0, 0.0), + target_frontier: None, + frontiers_remaining: self.frontiers.len() - self.current_frontier_idx, + } + } + } + } + + /// Navigate along current path. + fn navigate( + &mut self, + grid: &GridStorage, + current_pose: Pose2D, + should_refresh: bool, + ) -> ExplorationStep { + // Check if path following is complete + if self.follower.is_complete() { + tracing::info!("Reached frontier, selecting next"); + self.state = ExplorationState::SelectingFrontier; + + return ExplorationStep { + state: self.state, + velocity: (0.0, 0.0), + target_frontier: None, + frontiers_remaining: self.frontiers.len(), + }; + } + + // Get current goal for progress tracking + let goal = self.follower.goal(); + + // --- STUCK DETECTION --- + // Check if robot is physically stuck (not moving despite commands) + // Do this before borrowing cost_map to avoid borrow checker issues + if self.check_stuck(current_pose) { + tracing::warn!("Stuck detected, initiating recovery"); + self.state = ExplorationState::Recovering; + self.recovery_phase = RecoveryPhase::BackingUp; + self.recovery_start_pose = None; // Will be set on first recovery call + self.follower.clear_path(); + return ExplorationStep { + state: self.state, + velocity: (0.0, 0.0), + target_frontier: goal, + frontiers_remaining: self.frontiers.len(), + }; + } + + // --- PROGRESS TIMEOUT CHECK --- + // Check if making progress toward goal + // Do this before borrowing cost_map to avoid borrow checker issues + if let Some(goal_point) = goal + && !self.check_progress(current_pose, goal_point) + { + // No progress timeout - blacklist frontier and try another + tracing::warn!("Progress timeout, blacklisting frontier"); + if let Some(ref frontier_id) = self.current_frontier_id.clone() { + self.blacklist_frontier(frontier_id.clone()); + } + self.state = ExplorationState::SelectingFrontier; + self.follower.clear_path(); + return ExplorationStep { + state: self.state, + velocity: (0.0, 0.0), + target_frontier: None, + frontiers_remaining: self.frontiers.len(), + }; + } + + // Now borrow cost_map for remaining operations + let cost_map = self.cost_map.as_ref().unwrap(); + + // SAFETY CHECK 1: Check if current position is dangerously close to obstacles + let current_pos = WorldPoint::new(current_pose.x, current_pose.y); + let current_coord = cost_map.world_to_grid(current_pos); + let current_distance = cost_map.obstacle_distance(current_coord); + // Minimum safe distance is the physical robot radius (not planning radius) + let min_safe_distance = self.config.robot_radius / cost_map.resolution(); + + if current_distance < min_safe_distance { + tracing::warn!( + "Robot too close to obstacle! dist={:.2} cells ({:.2}m), min={:.2} cells ({:.2}m), triggering replan", + current_distance, + current_distance * cost_map.resolution(), + min_safe_distance, + min_safe_distance * cost_map.resolution() + ); + // Stop immediately and replan + self.state = ExplorationState::Replanning; + return ExplorationStep { + state: self.state, + velocity: (0.0, 0.0), + target_frontier: None, + frontiers_remaining: self.frontiers.len(), + }; + } + + // Compute velocity first - this determines if we're moving forward or turning + let mut velocity = self.follower.compute_velocity(current_pose); + + // SAFETY CHECK 2: Check immediate path ahead in robot's heading direction + // Only check when moving forward (linear velocity > 0), not when turning in place. + // This prevents replan loops when the robot needs to turn to face a new path. + if velocity.0 > 0.01 { + let check_distance = self.config.robot_radius + 0.05; // 15cm ahead + let heading_x = current_pose.x + check_distance * current_pose.theta.cos(); + let heading_y = current_pose.y + check_distance * current_pose.theta.sin(); + let heading_coord = cost_map.world_to_grid(WorldPoint::new(heading_x, heading_y)); + + if !cost_map.is_traversable(heading_coord) { + // Increment replan counter for this frontier + self.replan_count_for_frontier += 1; + tracing::warn!( + "Obstacle directly ahead at ({:.2}, {:.2}), replan #{} for frontier", + heading_x, + heading_y, + self.replan_count_for_frontier + ); + + // Check if too many replans for this frontier + if self.replan_count_for_frontier >= self.config.max_replans_per_frontier { + // Too many replans - need to physically move away from obstacle + // Trigger recovery (backup + rotate) before trying new frontier + tracing::warn!( + "Max replans ({}) reached, triggering recovery to escape obstacle", + self.config.max_replans_per_frontier + ); + self.state = ExplorationState::Recovering; + self.recovery_phase = RecoveryPhase::BackingUp; + self.recovery_start_pose = None; + self.follower.clear_path(); + return ExplorationStep { + state: self.state, + velocity: (0.0, 0.0), + target_frontier: None, + frontiers_remaining: self.frontiers.len(), + }; + } + + // Obstacle in our path - need to replan to find alternative route + self.state = ExplorationState::Replanning; + self.follower.clear_path(); + return ExplorationStep { + state: self.state, + velocity: (0.0, 0.0), + target_frontier: None, + frontiers_remaining: self.frontiers.len(), + }; + } + } + + // SAFETY: Scale down velocity when within 2x safety margin of obstacles + // This gives more reaction time near walls + let safe_distance = + (self.config.robot_radius + self.config.safety_margin) / cost_map.resolution(); + if current_distance < safe_distance * 2.0 { + // Scale velocity: at safe_distance -> 50% speed, at 2x -> 100% speed + let scale = (current_distance / safe_distance / 2.0).clamp(0.3, 1.0); + velocity.0 *= scale; + tracing::debug!( + "Near obstacle (dist={:.2}m, safe={:.2}m), scaling velocity to {:.0}%", + current_distance * cost_map.resolution(), + safe_distance * cost_map.resolution(), + scale * 100.0 + ); + } + + // Log velocity at debug level + if velocity.0.abs() > 0.001 || velocity.1.abs() > 0.001 { + tracing::debug!( + "Navigate velocity: linear={:.3}, angular={:.3}", + velocity.0, + velocity.1 + ); + } else { + tracing::warn!( + "Navigate returning zero velocity at ({:.2}, {:.2})", + current_pose.x, + current_pose.y + ); + } + + // Check if we should refresh frontiers (might find a better one) + if should_refresh { + let new_frontiers = self.frontier_detector.detect(grid, cost_map, current_pose); + self.last_frontier_refresh = Instant::now(); + + if new_frontiers.is_empty() { + // Map fully explored while navigating + if self.config.return_to_start && self.start_pose.is_some() { + self.state = ExplorationState::ReturningHome; + self.follower.clear_path(); + } else { + self.state = ExplorationState::Complete; + } + } else if !new_frontiers.is_empty() && self.frontiers.is_empty() { + // Found new frontiers after thinking we were done + self.frontiers = new_frontiers; + } + } + + ExplorationStep { + state: self.state, + velocity, + target_frontier: goal, + frontiers_remaining: self.frontiers.len(), + } + } + + /// Replan due to blocked path. + fn replan(&mut self, current_pose: Pose2D) -> ExplorationStep { + self.follower.clear_path(); + self.state = ExplorationState::Planning; + self.plan_path(current_pose) + } + + /// Return to starting position. + fn return_home(&mut self, current_pose: Pose2D) -> ExplorationStep { + let start = match self.start_pose { + Some(pose) => WorldPoint::new(pose.x, pose.y), + None => { + self.state = ExplorationState::Complete; + return ExplorationStep { + state: self.state, + velocity: (0.0, 0.0), + target_frontier: None, + frontiers_remaining: 0, + }; + } + }; + + // Check if already home + let current_pos = WorldPoint::new(current_pose.x, current_pose.y); + let distance_to_start = current_pos.distance(&start); + + if distance_to_start < self.config.follower.goal_tolerance { + tracing::info!("Returned to start position"); + self.state = ExplorationState::Complete; + + return ExplorationStep { + state: self.state, + velocity: (0.0, 0.0), + target_frontier: None, + frontiers_remaining: 0, + }; + } + + // Follow path home + // Note: We trust the path planner (Theta*) to find obstacle-free paths. + // No obstacle checking here - if robot hits something, bumper triggers safety stop. + if self.follower.has_path() { + if self.follower.is_complete() { + self.state = ExplorationState::Complete; + return ExplorationStep { + state: self.state, + velocity: (0.0, 0.0), + target_frontier: None, + frontiers_remaining: 0, + }; + } + + let velocity = self.follower.compute_velocity(current_pose); + return ExplorationStep { + state: self.state, + velocity, + target_frontier: Some(start), + frontiers_remaining: 0, + }; + } + + // Plan path home + let cost_map = self.cost_map.as_ref().unwrap(); + match self.planner.plan(cost_map, current_pose, start) { + Some(path) => { + let smoothed = self.smoother.smooth(&path); + self.follower.set_path(smoothed); + tracing::info!("Path to start planned, {:.2}m", path.length); + } + None => { + tracing::error!("Cannot find path back to start"); + self.state = ExplorationState::Failed; + } + } + + ExplorationStep { + state: self.state, + velocity: (0.0, 0.0), + target_frontier: Some(start), + frontiers_remaining: 0, + } + } + + /// Navigate toward start position to escape from trapped area, then retry frontiers. + fn escape_to_retry(&mut self, current_pose: Pose2D) -> ExplorationStep { + let start = match self.start_pose { + Some(pose) => WorldPoint::new(pose.x, pose.y), + None => { + // No start pose, go back to selecting frontier + tracing::warn!("No start pose for escape, returning to frontier selection"); + self.state = ExplorationState::SelectingFrontier; + return ExplorationStep { + state: self.state, + velocity: (0.0, 0.0), + target_frontier: None, + frontiers_remaining: self.frontiers.len(), + }; + } + }; + + // Check if we've reached near the start position + let current_pos = WorldPoint::new(current_pose.x, current_pose.y); + let distance_to_start = current_pos.distance(&start); + + // Use a larger tolerance (0.5m) since we just need to get to a different area + if distance_to_start < 0.5 { + tracing::info!( + "Escaped to near start position ({:.2}m away), resetting exhaustion and retrying frontiers", + distance_to_start + ); + // Reset exhaustion counts so frontiers get another chance from this position + self.exhaustion_count.clear(); + self.blacklisted_frontiers.clear(); + self.state = ExplorationState::SelectingFrontier; + + return ExplorationStep { + state: self.state, + velocity: (0.0, 0.0), + target_frontier: None, + frontiers_remaining: 0, + }; + } + + // Follow path to start + if self.follower.has_path() { + if self.follower.is_complete() { + // Path complete but not close enough yet - replan + self.follower.clear_path(); + } else { + let velocity = self.follower.compute_velocity(current_pose); + return ExplorationStep { + state: self.state, + velocity, + target_frontier: Some(start), + frontiers_remaining: self.frontiers.len(), + }; + } + } + + // Plan path to start + let cost_map = self.cost_map.as_ref().unwrap(); + match self.planner.plan(cost_map, current_pose, start) { + Some(path) => { + let smoothed = self.smoother.smooth(&path); + tracing::info!("Escaping to start: path planned, {:.2}m", path.length); + self.follower.set_path(smoothed); + } + None => { + // Can't find path to start - just go back to selecting frontier + tracing::warn!("Cannot plan escape path, retrying frontiers from current position"); + self.exhaustion_count.clear(); + self.blacklisted_frontiers.clear(); + self.state = ExplorationState::SelectingFrontier; + } + } + + ExplorationStep { + state: self.state, + velocity: (0.0, 0.0), + target_frontier: Some(start), + frontiers_remaining: self.frontiers.len(), + } + } + + // --- Helper methods for stuck detection and recovery --- + + /// Check if robot is stuck (hasn't moved significantly in time window). + fn check_stuck(&mut self, current_pose: Pose2D) -> bool { + let now = Instant::now(); + let time_window = Duration::from_secs_f32(self.config.stuck_time_window_secs); + + // Initialize stuck check pose if needed + if self.stuck_check_pose.is_none() { + self.stuck_check_pose = Some(current_pose); + self.stuck_check_start = now; + return false; + } + + // Check if time window has elapsed + if now.duration_since(self.stuck_check_start) < time_window { + return false; + } + + // Calculate distance moved since start of window + let start_pose = self.stuck_check_pose.unwrap(); + let dx = current_pose.x - start_pose.x; + let dy = current_pose.y - start_pose.y; + let distance_moved = (dx * dx + dy * dy).sqrt(); + + // Check if stuck + let is_stuck = distance_moved < self.config.stuck_distance_threshold; + + if is_stuck { + tracing::warn!( + "Robot stuck! Moved only {:.3}m in {:.1}s (threshold: {:.3}m)", + distance_moved, + self.config.stuck_time_window_secs, + self.config.stuck_distance_threshold + ); + } + + // Reset window for next check + self.stuck_check_pose = Some(current_pose); + self.stuck_check_start = now; + + is_stuck + } + + /// Check if making progress toward goal. Returns false if timeout reached. + fn check_progress(&mut self, current_pose: Pose2D, goal: WorldPoint) -> bool { + let now = Instant::now(); + let timeout = Duration::from_secs_f32(self.config.frontier_timeout_secs); + + // Calculate current distance to goal + let current_pos = WorldPoint::new(current_pose.x, current_pose.y); + let distance_to_goal = current_pos.distance(&goal); + + // Check if we made progress + if let Some(last_distance) = self.last_distance_to_goal { + let progress = last_distance - distance_to_goal; + if progress > self.config.min_progress_distance { + // Made progress, reset timer + self.last_progress_time = now; + self.last_progress_pose = Some(current_pose); + self.last_distance_to_goal = Some(distance_to_goal); + return true; + } + } else { + // First check, initialize + self.last_distance_to_goal = Some(distance_to_goal); + self.last_progress_time = now; + self.last_progress_pose = Some(current_pose); + return true; + } + + // Check if timeout + if now.duration_since(self.last_progress_time) >= timeout { + tracing::warn!( + "No progress timeout! No progress toward goal in {:.1}s (dist: {:.2}m)", + self.config.frontier_timeout_secs, + distance_to_goal + ); + return false; + } + + // Update distance tracking + self.last_distance_to_goal = Some(distance_to_goal); + true + } + + /// Blacklist a frontier. + fn blacklist_frontier(&mut self, frontier_id: FrontierId) { + tracing::info!( + "Blacklisting frontier at ({}, {})", + frontier_id.centroid.x, + frontier_id.centroid.y + ); + self.blacklisted_frontiers + .insert(frontier_id, Instant::now()); + } + + /// Check if a frontier is blacklisted (and cooldown hasn't expired). + fn is_blacklisted(&self, frontier_id: &FrontierId) -> bool { + if let Some(blacklist_time) = self.blacklisted_frontiers.get(frontier_id) { + let cooldown = Duration::from_secs_f32(self.config.blacklist_cooldown_secs); + let expired = blacklist_time.elapsed() >= cooldown; + !expired + } else { + false + } + } + + /// Clean up expired blacklist entries. + fn clean_blacklist(&mut self) { + let cooldown = Duration::from_secs_f32(self.config.blacklist_cooldown_secs); + self.blacklisted_frontiers + .retain(|_, time| time.elapsed() < cooldown); + } + + /// Reset tracking for a new frontier. + fn reset_frontier_tracking(&mut self, frontier_id: FrontierId) { + let now = Instant::now(); + self.current_frontier_id = Some(frontier_id); + self.frontier_start_time = Some(now); + self.replan_count_for_frontier = 0; + self.recovery_attempts_for_frontier = 0; + self.last_progress_time = now; + self.last_progress_pose = None; + self.last_distance_to_goal = None; + self.stuck_check_start = now; + self.stuck_check_pose = None; + } + + /// Execute recovery behavior (backup + rotate). + fn execute_recovery(&mut self, current_pose: Pose2D) -> ExplorationStep { + match self.recovery_phase { + RecoveryPhase::BackingUp => { + // Initialize recovery if first call + if self.recovery_start_pose.is_none() { + self.recovery_start_pose = Some(current_pose); + tracing::info!( + "Starting recovery: backing up {:.2}m", + self.config.recovery_backup_distance + ); + } + + // Calculate distance backed up + let start = self.recovery_start_pose.unwrap(); + let dx = current_pose.x - start.x; + let dy = current_pose.y - start.y; + let backed_up = (dx * dx + dy * dy).sqrt(); + + if backed_up >= self.config.recovery_backup_distance { + // Done backing up, start rotating + tracing::debug!("Backup complete ({:.2}m), starting rotation", backed_up); + self.recovery_phase = RecoveryPhase::Rotating; + self.recovery_start_pose = Some(current_pose); + + ExplorationStep { + state: self.state, + velocity: (0.0, 0.0), // Stop before rotating + target_frontier: None, + frontiers_remaining: self.frontiers.len(), + } + } else { + // Continue backing up (negative linear velocity) + ExplorationStep { + state: self.state, + velocity: (-0.1, 0.0), // Back up at 0.1 m/s + target_frontier: None, + frontiers_remaining: self.frontiers.len(), + } + } + } + + RecoveryPhase::Rotating => { + // Calculate angle rotated (normalized to handle wraparound) + let start = self.recovery_start_pose.unwrap(); + let angle_diff = current_pose.theta - start.theta; + let angle_rotated = normalize_angle(angle_diff).abs(); + + if angle_rotated >= self.config.recovery_rotation_rad { + // Done rotating, recovery complete + tracing::info!( + "Recovery complete (rotated {:.1}°)", + angle_rotated.to_degrees() + ); + self.recovery_phase = RecoveryPhase::Done; + self.recovery_start_pose = None; + self.recovery_attempts_for_frontier += 1; + + // Check if too many recovery attempts + if self.recovery_attempts_for_frontier >= self.config.max_recovery_attempts { + tracing::warn!( + "Max recovery attempts ({}) reached, blacklisting frontier", + self.config.max_recovery_attempts + ); + if let Some(ref frontier_id) = self.current_frontier_id.clone() { + self.blacklist_frontier(frontier_id.clone()); + } + self.state = ExplorationState::SelectingFrontier; + } else { + // Try replanning + self.state = ExplorationState::Replanning; + } + + ExplorationStep { + state: self.state, + velocity: (0.0, 0.0), + target_frontier: None, + frontiers_remaining: self.frontiers.len(), + } + } else { + // Continue rotating + ExplorationStep { + state: self.state, + velocity: (0.0, 0.4), // Rotate at 0.4 rad/s + target_frontier: None, + frontiers_remaining: self.frontiers.len(), + } + } + } + + RecoveryPhase::Done => { + // Shouldn't happen, transition to replanning + self.state = ExplorationState::Replanning; + ExplorationStep { + state: self.state, + velocity: (0.0, 0.0), + target_frontier: None, + frontiers_remaining: self.frontiers.len(), + } + } + } + } + + /// Get current path for visualization. + pub fn current_path(&self) -> Option<&SmoothedPath> { + self.current_path.as_ref() + } +} + +#[cfg(test)] +mod tests { + use super::*; + use vastu_slam::{CellType, GridCoord}; + + fn create_partial_grid() -> GridStorage { + // 60x60 grid, larger to avoid inflation issues + let mut grid = GridStorage::new(60, 60, 0.05, WorldPoint::ZERO); + + // Mark explored area (floor) in a region + for y in 10..30 { + for x in 10..30 { + grid.set_type(GridCoord::new(x, y), CellType::Floor); + } + } + + // Add a wall in the middle of explored area + for x in 15..25 { + grid.set_type(GridCoord::new(x, 20), CellType::Wall); + } + + grid + } + + #[test] + fn test_explorer_creation() { + let explorer = Explorer::new(ExplorerConfig::default()); + assert!(!explorer.is_complete()); + } + + #[test] + fn test_explorer_start() { + let mut explorer = Explorer::new(ExplorerConfig::default()); + let pose = Pose2D::new(0.75, 0.75, 0.0); + + explorer.start(pose); + assert!(!explorer.is_complete()); + } + + #[test] + fn test_frontier_detection_on_update() { + // Use smaller robot radius for this test + let mut explorer = Explorer::new(ExplorerConfig { + robot_radius: 0.05, + safety_margin: 0.02, + wall_penalty_distance: 0.10, + ..Default::default() + }); + let grid = create_partial_grid(); + let pose = Pose2D::new(0.75, 0.75, 0.0); + + explorer.start(pose); + let step = explorer.update(&grid, pose); + + // Should have found frontiers + assert!( + step.frontiers_remaining > 0, + "Expected frontiers at Floor/Unknown boundary" + ); + } + + #[test] + fn test_complete_on_full_exploration() { + let mut explorer = Explorer::new(ExplorerConfig { + return_to_start: false, + robot_radius: 0.05, + safety_margin: 0.02, + wall_penalty_distance: 0.10, + ..Default::default() + }); + + // Fully explored grid (all floor) + let mut grid = GridStorage::new(30, 30, 0.05, WorldPoint::ZERO); + for y in 0..30 { + for x in 0..30 { + grid.set_type(GridCoord::new(x, y), CellType::Floor); + } + } + + let pose = Pose2D::new(0.75, 0.75, 0.0); + explorer.start(pose); + let step = explorer.update(&grid, pose); + + // Should complete immediately (no frontiers) + assert_eq!(step.state, ExplorationState::Complete); + assert!(explorer.is_complete()); + } +} diff --git a/dhruva-nav/src/exploration/follower.rs b/dhruva-nav/src/exploration/follower.rs new file mode 100644 index 0000000..7d727ce --- /dev/null +++ b/dhruva-nav/src/exploration/follower.rs @@ -0,0 +1,384 @@ +//! Path follower using pure pursuit algorithm. +//! +//! Follows planned paths by computing velocity commands that steer +//! the robot toward a lookahead point on the path. + +use vastu_slam::{Pose2D, WorldPoint}; + +use crate::planning::{PathSegment, SmoothedPath}; + +/// Configuration for path following. +#[derive(Clone, Debug)] +pub struct FollowerConfig { + /// Distance tolerance for reaching waypoints (meters) + pub waypoint_tolerance: f32, + /// Distance tolerance for reaching final goal (meters) + pub goal_tolerance: f32, + /// Maximum linear velocity (m/s) + pub max_linear_vel: f32, + /// Maximum angular velocity (rad/s) + pub max_angular_vel: f32, + /// Proportional gain for angular control + pub kp_angular: f32, +} + +impl Default for FollowerConfig { + fn default() -> Self { + Self { + waypoint_tolerance: 0.10, + goal_tolerance: 0.15, + max_linear_vel: 0.2, + max_angular_vel: 0.5, + kp_angular: 2.0, + } + } +} + +/// State of path following. +#[derive(Clone, Copy, Debug, PartialEq, Eq)] +pub enum FollowState { + /// Following the path + Following, + /// Performing a point turn + Turning, + /// Reached the goal + Complete, + /// Path is empty or invalid + NoPath, +} + +/// Path follower using pure pursuit. +pub struct PathFollower { + config: FollowerConfig, + /// Current path to follow + path: Option, + /// Current segment index + current_segment: usize, + /// Current state + state: FollowState, +} + +impl PathFollower { + /// Create a new path follower with configuration. + pub fn new(config: FollowerConfig) -> Self { + Self { + config, + path: None, + current_segment: 0, + state: FollowState::NoPath, + } + } + + /// Set a new path to follow. + pub fn set_path(&mut self, path: SmoothedPath) { + if path.segments.is_empty() { + tracing::warn!("set_path: received empty path (0 segments)"); + self.path = None; + self.current_segment = 0; + self.state = FollowState::NoPath; + } else { + tracing::info!( + "set_path: {} segments, {:.2}m total", + path.segments.len(), + path.total_length + ); + self.path = Some(path); + self.current_segment = 0; + self.state = FollowState::Following; + } + } + + /// Clear the current path. + pub fn clear_path(&mut self) { + self.path = None; + self.current_segment = 0; + self.state = FollowState::NoPath; + } + + /// Check if path following is complete. + pub fn is_complete(&self) -> bool { + self.state == FollowState::Complete + } + + /// Check if there's no path to follow. + pub fn has_path(&self) -> bool { + self.path.is_some() && !self.path.as_ref().unwrap().segments.is_empty() + } + + /// Get the final goal point. + pub fn goal(&self) -> Option { + self.path + .as_ref() + .and_then(|p| p.segments.last().map(|s| s.end_point())) + } + + /// Compute velocity command to follow the path. + /// + /// Returns (linear_velocity, angular_velocity). + pub fn compute_velocity(&mut self, current_pose: Pose2D) -> (f32, f32) { + let path = match &self.path { + Some(p) => p, + None => { + self.state = FollowState::NoPath; + tracing::warn!("compute_velocity: No path set on follower"); + return (0.0, 0.0); + } + }; + + if self.current_segment >= path.segments.len() { + self.state = FollowState::Complete; + tracing::debug!("Path complete: all {} segments done", path.segments.len()); + return (0.0, 0.0); + } + + let segment = &path.segments[self.current_segment]; + tracing::debug!( + "Following segment {}/{}: {:?}", + self.current_segment + 1, + path.segments.len(), + match segment { + PathSegment::Line { start, end } => format!( + "Line ({:.2},{:.2})->({:.2},{:.2})", + start.x, start.y, end.x, end.y + ), + PathSegment::PointTurn { to_angle, .. } => + format!("PointTurn to {:.1}°", to_angle.to_degrees()), + } + ); + let current_pos = WorldPoint::new(current_pose.x, current_pose.y); + + match segment { + PathSegment::Line { start, end } => { + self.follow_line(current_pose, current_pos, *start, *end) + } + PathSegment::PointTurn { + position, to_angle, .. + } => self.execute_point_turn(current_pose, *position, *to_angle), + } + } + + /// Follow a line segment - simplified direct approach. + fn follow_line( + &mut self, + current_pose: Pose2D, + current_pos: WorldPoint, + _start: WorldPoint, + end: WorldPoint, + ) -> (f32, f32) { + let distance_to_end = current_pos.distance(&end); + + // Check if we've reached the end of this segment + if distance_to_end < self.config.waypoint_tolerance { + tracing::debug!("Segment complete (dist {:.3}m)", distance_to_end); + self.advance_segment(); + return self.compute_velocity(current_pose); + } + + // Simple approach: drive directly toward the segment end point + // This is more robust than pure pursuit for our use case + self.drive_to_point(current_pose, end, distance_to_end) + } + + /// Execute a point turn (rotate in place). + fn execute_point_turn( + &mut self, + current_pose: Pose2D, + _position: WorldPoint, + target_angle: f32, + ) -> (f32, f32) { + // Compute angle error + let angle_error = Self::normalize_angle(target_angle - current_pose.theta); + + // Check if turn is complete (10° tolerance) + if angle_error.abs() < 0.17 { + self.advance_segment(); + return self.compute_velocity(current_pose); + } + + // Pure rotation with gentler control to prevent overshoot + self.state = FollowState::Turning; + let turn_max_angular = self.config.max_angular_vel * 0.5; // 50% of max for point turns + let angular_gain = 0.6; // Gentle gain for point turns + + let angular = (angular_gain * angle_error).clamp(-turn_max_angular, turn_max_angular); + + tracing::debug!( + "point_turn: target={:.1}°, current={:.1}°, error={:.1}°, angular={:.3}", + target_angle.to_degrees(), + current_pose.theta.to_degrees(), + angle_error.to_degrees(), + angular + ); + + (0.0, angular) + } + + /// Simple drive-to-point controller. + /// Uses proportional control with angle-dependent speed reduction. + fn drive_to_point( + &mut self, + current_pose: Pose2D, + target: WorldPoint, + distance: f32, + ) -> (f32, f32) { + self.state = FollowState::Following; + + // Compute angle to target + let dx = target.x - current_pose.x; + let dy = target.y - current_pose.y; + let target_angle = dy.atan2(dx); + let angle_error = Self::normalize_angle(target_angle - current_pose.theta); + let angle_error_deg = angle_error.to_degrees(); + let angle_error_abs = angle_error.abs(); + + // Proportional control with angle-dependent speed scaling + // Key insight: ALWAYS maintain some forward motion to prevent spinning loops + + // Angular velocity: proportional to angle error, clamped + let kp_angular = self.config.kp_angular; + let angular = (kp_angular * angle_error) + .clamp(-self.config.max_angular_vel, self.config.max_angular_vel); + + // Linear velocity: scale down based on angle error + // - At 0° error: full speed + // - At 90° error: 20% speed (still moving forward!) + // - At 180° error: 10% speed (prevents spinning in place) + let angle_factor = if angle_error_abs < 0.17 { + // < 10°: full speed + 1.0 + } else if angle_error_abs < 1.57 { + // 10-90°: linear interpolation from 1.0 to 0.2 + 1.0 - 0.8 * (angle_error_abs - 0.17) / (1.57 - 0.17) + } else { + // > 90°: minimum speed (10-20% to prevent pure spinning) + 0.1 + 0.1 * (std::f32::consts::PI - angle_error_abs) / (std::f32::consts::PI - 1.57) + }; + + // Distance-based speed: slow down as we approach target + let distance_speed = (distance * 0.8).min(self.config.max_linear_vel); + + // Minimum forward speed when far from target to prevent getting stuck + let min_forward_speed = if distance > 0.15 { 0.03 } else { 0.0 }; + + let linear = (distance_speed * angle_factor).max(min_forward_speed); + + tracing::debug!( + "drive_to_point: dist={:.2}m, angle_err={:.1}°, angle_factor={:.2}, vel=({:.3},{:.3})", + distance, + angle_error_deg, + angle_factor, + linear, + angular + ); + + (linear, angular) + } + + /// Advance to the next segment. + fn advance_segment(&mut self) { + self.current_segment += 1; + + if let Some(path) = &self.path { + if self.current_segment >= path.segments.len() { + self.state = FollowState::Complete; + } else { + self.state = FollowState::Following; + } + } + } + + /// Normalize angle to [-pi, pi]. + #[inline] + fn normalize_angle(angle: f32) -> f32 { + let mut a = angle; + while a > std::f32::consts::PI { + a -= 2.0 * std::f32::consts::PI; + } + while a < -std::f32::consts::PI { + a += 2.0 * std::f32::consts::PI; + } + a + } +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::planning::PathSegment; + + fn create_test_path() -> SmoothedPath { + SmoothedPath { + segments: vec![ + PathSegment::Line { + start: WorldPoint::new(0.0, 0.0), + end: WorldPoint::new(1.0, 0.0), + }, + PathSegment::Line { + start: WorldPoint::new(1.0, 0.0), + end: WorldPoint::new(1.0, 1.0), + }, + ], + total_length: 2.0, + estimated_time: 10.0, + } + } + + #[test] + fn test_follower_creation() { + let follower = PathFollower::new(FollowerConfig::default()); + assert!(!follower.has_path()); + } + + #[test] + fn test_set_path() { + let mut follower = PathFollower::new(FollowerConfig::default()); + let path = create_test_path(); + + follower.set_path(path); + assert!(follower.has_path()); + } + + #[test] + fn test_follow_straight_line() { + let mut follower = PathFollower::new(FollowerConfig::default()); + let path = SmoothedPath { + segments: vec![PathSegment::Line { + start: WorldPoint::new(0.0, 0.0), + end: WorldPoint::new(1.0, 0.0), + }], + total_length: 1.0, + estimated_time: 5.0, + }; + + follower.set_path(path); + + // Robot at origin, facing forward + let pose = Pose2D::new(0.0, 0.0, 0.0); + let (linear, angular) = follower.compute_velocity(pose); + + // Should move forward with minimal turning + assert!(linear > 0.0); + assert!(angular.abs() < 0.1); + } + + #[test] + fn test_complete_detection() { + let mut follower = PathFollower::new(FollowerConfig::default()); + let path = SmoothedPath { + segments: vec![PathSegment::Line { + start: WorldPoint::new(0.0, 0.0), + end: WorldPoint::new(0.1, 0.0), + }], + total_length: 0.1, + estimated_time: 0.5, + }; + + follower.set_path(path); + + // Robot near the end + let pose = Pose2D::new(0.09, 0.0, 0.0); + let (_linear, _angular) = follower.compute_velocity(pose); + + assert!(follower.is_complete()); + } +} diff --git a/dhruva-nav/src/exploration/frontier.rs b/dhruva-nav/src/exploration/frontier.rs new file mode 100644 index 0000000..c85a6cb --- /dev/null +++ b/dhruva-nav/src/exploration/frontier.rs @@ -0,0 +1,517 @@ +//! Frontier detection for autonomous exploration. +//! +//! Identifies boundaries between explored and unexplored space, +//! clusters them into frontier regions, and scores them for selection. + +use std::collections::{HashSet, VecDeque}; + +use vastu_slam::{CellType, GridCoord, GridStorage, Pose2D, WorldPoint}; + +use crate::planning::CostMap; + +/// Configuration for frontier detection. +#[derive(Clone, Debug)] +pub struct FrontierConfig { + /// Minimum number of cells for a valid frontier cluster + pub min_cluster_size: usize, + /// Maximum number of frontiers to consider + pub max_frontiers: usize, + /// Weight for frontier size in scoring + pub size_weight: f32, + /// Weight for distance penalty in scoring + pub distance_weight: f32, + /// Weight for information gain (unexplored neighbors) + pub info_gain_weight: f32, + /// Distance threshold for clustering frontier cells + pub cluster_distance: i32, +} + +impl Default for FrontierConfig { + fn default() -> Self { + Self { + min_cluster_size: 5, + max_frontiers: 20, + // Weights tuned for normalized scoring (all components in [0,1] range) + // Higher distance_weight prioritizes nearby frontiers to reduce backtracking + size_weight: 0.5, + distance_weight: 1.5, + info_gain_weight: 1.0, + cluster_distance: 3, + } + } +} + +/// Unique identifier for a frontier based on its centroid. +/// Used for blacklisting and tracking frontiers across updates. +#[derive(Clone, Debug, Hash, Eq, PartialEq)] +pub struct FrontierId { + pub centroid: GridCoord, +} + +/// A frontier region (cluster of frontier cells). +#[derive(Clone, Debug)] +pub struct Frontier { + /// Centroid of the frontier (in grid coordinates) + pub centroid: GridCoord, + /// Centroid in world coordinates + pub world_centroid: WorldPoint, + /// Size (number of cells) + pub size: usize, + /// Estimated information gain (unexplored cells nearby) + pub info_gain: f32, + /// Euclidean distance to robot (meters) + pub distance: f32, + /// Score (higher is better, set by normalize_scores) + pub score: f32, +} + +impl Frontier { + /// Get a unique identifier for this frontier. + pub fn id(&self) -> FrontierId { + FrontierId { + centroid: self.centroid, + } + } +} + +/// Frontier detector for exploration. +pub struct FrontierDetector { + config: FrontierConfig, +} + +impl FrontierDetector { + /// Create a new frontier detector with configuration. + pub fn new(config: FrontierConfig) -> Self { + Self { config } + } + + /// Detect all frontiers in the grid. + /// + /// Returns frontiers sorted by score (highest first). + pub fn detect( + &self, + grid: &GridStorage, + cost_map: &CostMap, + robot_pose: Pose2D, + ) -> Vec { + // Step 1: Find all frontier cells + let frontier_cells = self.find_frontier_cells(grid, cost_map); + + if frontier_cells.is_empty() { + return Vec::new(); + } + + // Step 2: Cluster frontier cells + let clusters = self.cluster_frontiers(&frontier_cells); + + // Step 3: Filter and create frontiers (with raw info_gain/distance) + let mut frontiers: Vec = clusters + .into_iter() + .filter(|cluster| cluster.len() >= self.config.min_cluster_size) + .map(|cluster| self.create_frontier(cluster, grid, cost_map, robot_pose)) + .collect(); + + // Step 3.5: Filter unreachable frontiers + // Removes frontiers where no valid navigation target exists + // (e.g., frontiers too close to walls discovered after initial detection) + let pre_filter_count = frontiers.len(); + frontiers.retain(|frontier| { + self.navigation_target(frontier, cost_map, robot_pose) + .is_some() + }); + if frontiers.len() < pre_filter_count { + tracing::debug!( + "Filtered {} unreachable frontiers (kept {})", + pre_filter_count - frontiers.len(), + frontiers.len() + ); + } + + // Step 4: Normalize scores across all frontiers + // This makes weights meaningful by scaling info_gain, distance, size to [0,1] + self.normalize_scores(&mut frontiers); + + // Step 5: Sort by normalized score (descending) + frontiers.sort_by(|a, b| { + b.score + .partial_cmp(&a.score) + .unwrap_or(std::cmp::Ordering::Equal) + }); + + // Step 6: Limit number of frontiers + frontiers.truncate(self.config.max_frontiers); + + frontiers + } + + /// Find all frontier cells (floor cells adjacent to unknown). + /// + /// Note: Frontier cells may be in the inflation zone (near unknown space). + /// The path planner will find a safe approach point. + fn find_frontier_cells(&self, grid: &GridStorage, _cost_map: &CostMap) -> Vec { + let mut frontier_cells = Vec::new(); + let width = grid.width(); + let height = grid.height(); + + // 4-connected neighbors for frontier detection + let neighbors = [ + GridCoord::new(-1, 0), + GridCoord::new(1, 0), + GridCoord::new(0, -1), + GridCoord::new(0, 1), + ]; + + for y in 1..(height as i32 - 1) { + for x in 1..(width as i32 - 1) { + let coord = GridCoord::new(x, y); + + // Must be a floor cell + if grid.get_type(coord) != CellType::Floor { + continue; + } + + // Check if adjacent to unknown + let is_frontier = neighbors.iter().any(|offset| { + let neighbor = GridCoord::new(coord.x + offset.x, coord.y + offset.y); + grid.get_type(neighbor) == CellType::Unknown + }); + + if is_frontier { + frontier_cells.push(coord); + } + } + } + + frontier_cells + } + + /// Cluster frontier cells using flood-fill. + fn cluster_frontiers(&self, frontier_cells: &[GridCoord]) -> Vec> { + if frontier_cells.is_empty() { + return Vec::new(); + } + + // Create set for fast lookup + let frontier_set: HashSet = frontier_cells.iter().copied().collect(); + let mut visited: HashSet = HashSet::new(); + let mut clusters = Vec::new(); + + // 8-connected neighbors for clustering + let neighbors = [ + GridCoord::new(-1, 0), + GridCoord::new(1, 0), + GridCoord::new(0, -1), + GridCoord::new(0, 1), + GridCoord::new(-1, -1), + GridCoord::new(1, -1), + GridCoord::new(-1, 1), + GridCoord::new(1, 1), + ]; + + for &start_cell in frontier_cells { + if visited.contains(&start_cell) { + continue; + } + + // BFS to find connected frontier cells + let mut cluster = Vec::new(); + let mut queue = VecDeque::new(); + queue.push_back(start_cell); + visited.insert(start_cell); + + while let Some(current) = queue.pop_front() { + cluster.push(current); + + for &_offset in &neighbors { + // Check cells within cluster distance + for dy in -self.config.cluster_distance..=self.config.cluster_distance { + for dx in -self.config.cluster_distance..=self.config.cluster_distance { + let candidate = GridCoord::new(current.x + dx, current.y + dy); + if frontier_set.contains(&candidate) && !visited.contains(&candidate) { + visited.insert(candidate); + queue.push_back(candidate); + } + } + } + } + } + + if !cluster.is_empty() { + clusters.push(cluster); + } + } + + clusters + } + + /// Create a Frontier from a cluster of cells. + /// Note: score is set to 0.0 here and will be computed by normalize_scores(). + fn create_frontier( + &self, + cells: Vec, + grid: &GridStorage, + cost_map: &CostMap, + robot_pose: Pose2D, + ) -> Frontier { + let size = cells.len(); + + // Compute centroid + let sum_x: i32 = cells.iter().map(|c| c.x).sum(); + let sum_y: i32 = cells.iter().map(|c| c.y).sum(); + let centroid = GridCoord::new(sum_x / size as i32, sum_y / size as i32); + + // Convert to world coordinates + let world_centroid = cost_map.grid_to_world(centroid); + + // Compute information gain (count unknown cells near frontier) + let info_gain = self.estimate_info_gain(&cells, grid); + + // Compute distance to robot + let robot_pos = WorldPoint::new(robot_pose.x, robot_pose.y); + let distance = robot_pos.distance(&world_centroid); + + // Score will be set by normalize_scores() after all frontiers are created + let score = 0.0; + + Frontier { + centroid, + world_centroid, + size, + info_gain, + distance, + score, + } + } + + /// Estimate information gain for a frontier. + fn estimate_info_gain(&self, cells: &[GridCoord], grid: &GridStorage) -> f32 { + let mut unknown_count = 0; + let mut checked = HashSet::new(); + + // Sample cells to check (for efficiency, check subset) + let sample_step = (cells.len() / 10).max(1); + + for (i, &cell) in cells.iter().enumerate() { + if i % sample_step != 0 { + continue; + } + + // Check 5x5 neighborhood + for dy in -2..=2 { + for dx in -2..=2 { + let neighbor = GridCoord::new(cell.x + dx, cell.y + dy); + if checked.insert(neighbor) && grid.get_type(neighbor) == CellType::Unknown { + unknown_count += 1; + } + } + } + } + + unknown_count as f32 + } + + /// Normalize frontier scores to make weights meaningful. + /// + /// Normalizes info_gain, distance, and size to [0, 1] range before applying + /// configured weights. This ensures distance can compete with info_gain + /// regardless of their raw value ranges. + fn normalize_scores(&self, frontiers: &mut [Frontier]) { + if frontiers.is_empty() { + return; + } + + const EPSILON: f32 = 1e-6; + + // Find min/max for each component + let (min_info, max_info) = frontiers + .iter() + .map(|f| f.info_gain) + .fold((f32::MAX, f32::MIN), |(min, max), v| { + (min.min(v), max.max(v)) + }); + + let (min_size, max_size) = frontiers + .iter() + .map(|f| (f.size as f32).sqrt()) + .fold((f32::MAX, f32::MIN), |(min, max), v| { + (min.min(v), max.max(v)) + }); + + let (min_dist, max_dist) = frontiers + .iter() + .map(|f| f.distance) + .fold((f32::MAX, f32::MIN), |(min, max), v| { + (min.min(v), max.max(v)) + }); + + // Compute ranges (avoid division by zero) + let info_range = (max_info - min_info).max(EPSILON); + let size_range = (max_size - min_size).max(EPSILON); + let dist_range = (max_dist - min_dist).max(EPSILON); + + // Compute normalized scores for each frontier + for frontier in frontiers.iter_mut() { + let norm_info = (frontier.info_gain - min_info) / info_range; + let norm_size = ((frontier.size as f32).sqrt() - min_size) / size_range; + let norm_dist = (frontier.distance - min_dist) / dist_range; + + frontier.score = self.config.size_weight * norm_size + + self.config.info_gain_weight * norm_info + - self.config.distance_weight * norm_dist; + } + } + + /// Get the navigation target for a frontier. + /// + /// Returns a traversable point near the frontier that the robot can safely reach. + /// Uses a multi-stage approach: + /// 1. Try small safety offset toward robot + /// 2. Try centroid directly + /// 3. BFS to find closest traversable cell (up to 5m radius) + /// + /// Returns `None` if no traversable point is found. + pub fn navigation_target( + &self, + frontier: &Frontier, + cost_map: &CostMap, + robot_pose: Pose2D, + ) -> Option { + let centroid = frontier.world_centroid; + let robot_pos = WorldPoint::new(robot_pose.x, robot_pose.y); + + // Calculate direction from centroid toward robot (into explored space) + let dx = robot_pos.x - centroid.x; + let dy = robot_pos.y - centroid.y; + let dist_to_centroid = (dx * dx + dy * dy).sqrt(); + + // Stage 1: Try small safety offset (5cm) toward robot + let safety_offset = 0.05; + let min_path_distance = 0.20; + + if dist_to_centroid > min_path_distance + safety_offset { + let scale = safety_offset / dist_to_centroid; + let safe_target = WorldPoint::new(centroid.x + dx * scale, centroid.y + dy * scale); + + let grid_coord = cost_map.world_to_grid(safe_target); + if cost_map.is_traversable(grid_coord) { + tracing::debug!( + "Frontier target: centroid ({:.2},{:.2}) -> safe ({:.2},{:.2}), dist={:.2}", + centroid.x, + centroid.y, + safe_target.x, + safe_target.y, + dist_to_centroid + ); + return Some(safe_target); + } + } + + // Stage 2: Try centroid directly + let centroid_coord = cost_map.world_to_grid(centroid); + if cost_map.is_traversable(centroid_coord) { + tracing::debug!( + "Frontier target: using centroid directly ({:.2},{:.2})", + centroid.x, + centroid.y + ); + return Some(centroid); + } + + // Stage 3: BFS to find closest reachable cell (up to 5m) + let max_search_radius = 5.0; // meters + if let Some(reachable) = cost_map.find_closest_reachable_world(centroid, max_search_radius) + { + let distance_from_centroid = centroid.distance(&reachable); + tracing::info!( + "Frontier target: found reachable point {:.2}m from centroid at ({:.2},{:.2})", + distance_from_centroid, + reachable.x, + reachable.y + ); + return Some(reachable); + } + + // No reachable point found within search radius + tracing::warn!( + "Frontier target: no reachable point found within {:.1}m of ({:.2},{:.2})", + max_search_radius, + centroid.x, + centroid.y + ); + None + } +} + +#[cfg(test)] +mod tests { + use super::*; + + fn create_test_grid() -> GridStorage { + // 50x50 grid with partial exploration (larger grid to avoid inflation issues) + let mut grid = GridStorage::new(50, 50, 0.05, WorldPoint::ZERO); + + // Mark explored area (floor) in center region + for y in 10..40 { + for x in 10..25 { + grid.set_type(GridCoord::new(x, y), CellType::Floor); + } + } + + // Rest is unknown (default) + grid + } + + #[test] + fn test_frontier_detection() { + let grid = create_test_grid(); + // Use smaller robot radius for test to avoid all frontiers being in inflation zone + let cost_map = CostMap::from_grid(&grid, 0.05, 0.02, 0.10); + let detector = FrontierDetector::new(FrontierConfig::default()); + + let robot_pose = Pose2D::new(0.75, 1.0, 0.0); + let frontiers = detector.detect(&grid, &cost_map, robot_pose); + + // Should find frontiers at the boundary of explored area + assert!( + !frontiers.is_empty(), + "Expected frontiers at Floor/Unknown boundary" + ); + } + + #[test] + fn test_no_frontiers_fully_explored() { + // Fully explored grid (all floor) + let mut grid = GridStorage::new(20, 20, 0.05, WorldPoint::ZERO); + for y in 0..20 { + for x in 0..20 { + grid.set_type(GridCoord::new(x, y), CellType::Floor); + } + } + + let cost_map = CostMap::from_grid(&grid, 0.05, 0.02, 0.10); + let detector = FrontierDetector::new(FrontierConfig::default()); + + let robot_pose = Pose2D::new(0.5, 0.5, 0.0); + let frontiers = detector.detect(&grid, &cost_map, robot_pose); + + // No frontiers (no unknown cells) + assert!(frontiers.is_empty()); + } + + #[test] + fn test_frontier_scoring() { + let grid = create_test_grid(); + let cost_map = CostMap::from_grid(&grid, 0.05, 0.02, 0.10); + let detector = FrontierDetector::new(FrontierConfig::default()); + + let robot_pose = Pose2D::new(0.75, 1.0, 0.0); + let frontiers = detector.detect(&grid, &cost_map, robot_pose); + + if frontiers.len() > 1 { + // Frontiers should be sorted by score (descending) + for i in 0..frontiers.len() - 1 { + assert!(frontiers[i].score >= frontiers[i + 1].score); + } + } + } +} diff --git a/dhruva-nav/src/exploration/mod.rs b/dhruva-nav/src/exploration/mod.rs new file mode 100644 index 0000000..ad2a9b9 --- /dev/null +++ b/dhruva-nav/src/exploration/mod.rs @@ -0,0 +1,14 @@ +//! Autonomous exploration module. +//! +//! This module provides frontier-based exploration capabilities: +//! - Frontier detection and clustering +//! - Path following with pure pursuit +//! - Exploration state machine + +mod explorer; +mod follower; +mod frontier; + +pub use explorer::{ExplorationState, ExplorationStep, Explorer, ExplorerConfig}; +pub use follower::FollowerConfig; +pub use frontier::FrontierConfig; diff --git a/dhruva-nav/src/main.rs b/dhruva-nav/src/main.rs new file mode 100644 index 0000000..c7788f5 --- /dev/null +++ b/dhruva-nav/src/main.rs @@ -0,0 +1,266 @@ +//! DhruvaNav - Navigation Controller for VacuumTiger +//! +//! A navigation controller that connects to SangamIO, performs autonomous +//! frontier-based exploration, and builds maps using VastuSLAM. +//! +//! ## Multi-Threaded Architecture +//! +//! DhruvaNav uses three threads for concurrent operation: +//! +//! - **Sensor Thread** (~100Hz): Reads UDP sensor data, updates odometry, +//! checks safety sensors, sends velocity commands +//! - **Mapping Thread** (~5-10Hz): Receives lidar scans, performs scan matching, +//! updates the occupancy grid map +//! - **Exploration Thread** (~20Hz): Reads the map, detects frontiers, +//! plans paths, computes velocity commands + +#![feature(portable_simd)] + +mod client; +mod config; +mod error; +mod exploration; +mod odometry; +mod planning; +mod shared; +mod threads; +mod utils; + +use config::DhruvaConfig; +use error::{DhruvaError, Result}; +use shared::{SharedDebugViz, SharedGrid, SharedState, SharedTrajectory}; +use threads::spawn_threads; + +use std::path::Path; +use std::sync::{Arc, RwLock}; +use std::time::Duration; +use tracing::{error, info, warn}; +use vastu_slam::{GridStorage, MapConfig, Pose2D}; + +fn main() -> Result<()> { + // Initialize logging + tracing_subscriber::fmt() + .with_env_filter( + tracing_subscriber::EnvFilter::from_default_env() + .add_directive("dhruva_nav=info".parse().unwrap()), + ) + .init(); + + // Parse command line arguments + let args: Vec = std::env::args().collect(); + + let config = if args.len() > 1 && !args[1].starts_with("--") { + // Load config from file + let config_path = Path::new(&args[1]); + info!("Loading configuration from {:?}", config_path); + DhruvaConfig::load(config_path)? + } else { + // Check for --robot argument + let robot_ip = args + .iter() + .position(|a| a == "--robot") + .and_then(|i| args.get(i + 1)) + .cloned(); + + let mut config = if Path::new("dhruva.toml").exists() { + info!("Loading configuration from dhruva.toml"); + DhruvaConfig::load(Path::new("dhruva.toml"))? + } else { + info!("Using default configuration"); + DhruvaConfig::default() + }; + + // Override robot IP if provided + if let Some(ip) = robot_ip { + info!("Using robot IP: {}", ip); + config.connection.robot_ip = ip; + } + + config + }; + + info!("DhruvaNav v{}", env!("CARGO_PKG_VERSION")); + info!( + "Connecting to {}:{}", + config.connection.robot_ip, config.connection.port + ); + + // Log configuration + let total_clearance = config.exploration.robot_radius + config.exploration.safety_margin; + info!( + "Wall clearance: {:.2}m (robot radius {:.2}m + safety margin {:.2}m)", + total_clearance, config.exploration.robot_radius, config.exploration.safety_margin + ); + + // Initialize shared state + // Robot starts at origin - map is built relative to initial position + let start_pose = Pose2D::new(0.0, 0.0, 0.0); + info!("Start pose: origin (0, 0, 0°)"); + + let shared_state = Arc::new(SharedState::new(start_pose)); + + // Initialize shared grid storage + let map_config = MapConfig::default(); + let initial_grid = GridStorage::new( + map_config.grid.initial_width, + map_config.grid.initial_height, + map_config.grid.resolution, + vastu_slam::WorldPoint::ZERO, + ); + let shared_grid: SharedGrid = Arc::new(RwLock::new(initial_grid)); + + // Spawn worker threads + info!("Starting multi-threaded exploration..."); + let handles = spawn_threads( + config.clone(), + Arc::clone(&shared_state), + Arc::clone(&shared_grid), + )?; + + // Main thread: Monitor and wait for completion + let check_interval = Duration::from_millis(500); + + loop { + std::thread::sleep(check_interval); + + // Check for safety stop + if shared_state.is_safety_stop() { + warn!( + "Safety stop: {}", + shared_state + .safety_reason() + .unwrap_or_else(|| "unknown".to_string()) + ); + break; + } + + // Check for exploration complete + if shared_state.is_exploration_complete() { + info!("Exploration completed successfully"); + break; + } + + // Check if threads are still alive + if handles.sensor.is_finished() + || handles.mapping.is_finished() + || handles.exploration.is_finished() + { + warn!("A worker thread exited unexpectedly"); + break; + } + } + + // Signal shutdown to all threads + shared_state.signal_shutdown(); + + // Wait for threads to finish (with timeout) + info!("Waiting for threads to finish..."); + + let join_timeout = Duration::from_secs(5); + let join_start = std::time::Instant::now(); + + // Join threads + if let Err(e) = handles.sensor.join() { + error!("Sensor thread panicked: {:?}", e); + } + + if join_start.elapsed() < join_timeout + && let Err(e) = handles.mapping.join() + { + error!("Mapping thread panicked: {:?}", e); + } + + if join_start.elapsed() < join_timeout + && let Err(e) = handles.exploration.join() + { + error!("Exploration thread panicked: {:?}", e); + } + + // Save the map with trajectory and debug visualization + info!("Saving map..."); + save_map( + &config, + &shared_grid, + &handles.trajectory, + &handles.debug_viz, + )?; + + info!("DhruvaNav finished"); + Ok(()) +} + +/// Save the map to files. +fn save_map( + config: &DhruvaConfig, + shared_grid: &SharedGrid, + shared_trajectory: &SharedTrajectory, + shared_debug_viz: &SharedDebugViz, +) -> Result<()> { + let grid_guard = shared_grid + .read() + .map_err(|e| DhruvaError::Slam(format!("Failed to lock grid: {}", e)))?; + let grid: &GridStorage = &grid_guard; + + // Get statistics + let counts = grid.count_by_type(); + let explored_area = (counts.floor + counts.wall) as f32 * grid.resolution() * grid.resolution(); + info!( + "Map statistics: {:.2}m² explored, {} floor cells, {} wall cells", + explored_area, counts.floor, counts.wall + ); + + // Create output directory if needed + let map_path = Path::new(&config.output.map_path); + if let Some(parent) = map_path.parent() { + std::fs::create_dir_all(parent) + .map_err(|e| DhruvaError::Slam(format!("Failed to create output directory: {}", e)))?; + } + + // Save .vastu file + vastu_slam::io::save_vastu(grid, map_path) + .map_err(|e| DhruvaError::Slam(format!("Failed to save map: {}", e)))?; + info!("Map saved to {:?}", map_path); + + // Read trajectory for SVG visualization + let trajectory = shared_trajectory + .read() + .map_err(|e| DhruvaError::Slam(format!("Failed to lock trajectory: {}", e)))? + .clone(); + + // Read debug visualization data + let debug_viz = shared_debug_viz + .read() + .map_err(|e| DhruvaError::Slam(format!("Failed to lock debug viz: {}", e)))? + .clone(); + + info!( + "Visualization data: {} trajectory poses, {} frontier targets, {} waypoints", + trajectory.len(), + debug_viz.visited_frontiers.len(), + debug_viz.waypoints.len() + ); + + // Save SVG visualization with trajectory and debug info + let svg_path = Path::new(&config.output.svg_path); + let svg_config = vastu_slam::io::SvgConfig { + scale: 50.0, + trajectory_width: 2.0, + ..Default::default() + }; + + // Calculate markers at 0.5m intervals along trajectory + let markers = vastu_slam::io::markers_by_distance(&trajectory, 0.5); + + let visualizer = vastu_slam::io::SvgVisualizer::new(grid.clone(), svg_config) + .with_title("DhruvaNav Exploration") + .with_estimated(trajectory, markers) + .with_frontiers(debug_viz.visited_frontiers) + .with_waypoints(debug_viz.waypoints); + + visualizer + .save(svg_path) + .map_err(|e| DhruvaError::Slam(format!("Failed to save SVG: {}", e)))?; + info!("SVG saved to {:?}", svg_path); + + Ok(()) +} diff --git a/dhruva-nav/src/odometry.rs b/dhruva-nav/src/odometry.rs new file mode 100644 index 0000000..566e9d8 --- /dev/null +++ b/dhruva-nav/src/odometry.rs @@ -0,0 +1,137 @@ +//! Wheel encoder-based odometry calculator +//! +//! Computes robot pose from differential drive wheel encoders. + +use vastu_slam::Pose2D; + +use crate::utils::normalize_angle; + +/// Odometry calculator for differential drive robot +pub struct Odometry { + /// Distance between wheels in meters + wheel_base: f32, + /// Encoder ticks per meter of travel + ticks_per_meter: f32, + /// Previous left wheel encoder value + prev_left: u16, + /// Previous right wheel encoder value + prev_right: u16, + /// Current estimated pose + pose: Pose2D, + /// Whether we have received the first encoder reading + initialized: bool, +} + +impl Odometry { + /// Create a new odometry calculator + /// + /// # Arguments + /// * `wheel_base` - Distance between wheels in meters (CRL-200S: 0.233m) + /// * `ticks_per_meter` - Encoder ticks per meter (CRL-200S: 4464.0) + /// * `start_pose` - Initial pose of the robot + pub fn new(wheel_base: f32, ticks_per_meter: f32, start_pose: Pose2D) -> Self { + Self { + wheel_base, + ticks_per_meter, + prev_left: 0, + prev_right: 0, + pose: start_pose, + initialized: false, + } + } + + /// Update odometry with new encoder readings + /// + /// # Arguments + /// * `left` - Left wheel encoder tick count (u16, wrapping) + /// * `right` - Right wheel encoder tick count (u16, wrapping) + /// + /// # Returns + /// The updated pose + pub fn update(&mut self, left: u16, right: u16) -> Pose2D { + if !self.initialized { + // First reading - just store the values + self.prev_left = left; + self.prev_right = right; + self.initialized = true; + return self.pose; + } + + // Calculate tick deltas (handle u16 wraparound using wrapping_sub) + let dl = left.wrapping_sub(self.prev_left) as i16; + let dr = right.wrapping_sub(self.prev_right) as i16; + + // Update stored values + self.prev_left = left; + self.prev_right = right; + + // Skip if no movement + if dl == 0 && dr == 0 { + return self.pose; + } + + // Convert ticks to meters + let dist_left = dl as f32 / self.ticks_per_meter; + let dist_right = dr as f32 / self.ticks_per_meter; + + // Differential drive kinematics + let dist = (dist_left + dist_right) / 2.0; + let dtheta = (dist_right - dist_left) / self.wheel_base; + + // Update pose using mid-point integration + // For more accuracy with rotation, we use the average heading + let mid_theta = self.pose.theta + dtheta / 2.0; + self.pose.x += dist * mid_theta.cos(); + self.pose.y += dist * mid_theta.sin(); + self.pose.theta += dtheta; + + // Normalize theta to [-pi, pi] + self.pose.theta = normalize_angle(self.pose.theta); + + self.pose + } + + /// Get the current pose + pub fn pose(&self) -> Pose2D { + self.pose + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_straight_line() { + let mut odom = Odometry::new(0.233, 4464.0, Pose2D::new(0.0, 0.0, 0.0)); + + // Initialize + odom.update(0, 0); + + // Move forward 1 meter (4464 ticks on each wheel) + let pose = odom.update(4464, 4464); + + assert!((pose.x - 1.0).abs() < 0.01); + assert!(pose.y.abs() < 0.01); + assert!(pose.theta.abs() < 0.01); + } + + #[test] + fn test_rotation() { + let mut odom = Odometry::new(0.233, 4464.0, Pose2D::new(0.0, 0.0, 0.0)); + + // Initialize + odom.update(0, 0); + + // Rotate in place: right wheel forward, left wheel backward + // For a 90 degree turn: arc length = theta * radius = (pi/2) * (0.233/2) + // ticks = arc_length * ticks_per_meter + let arc_ticks = (std::f32::consts::FRAC_PI_2 * 0.233 / 2.0 * 4464.0) as u16; + let pose = odom.update(0u16.wrapping_sub(arc_ticks), arc_ticks); + + // Should have rotated approximately 90 degrees (pi/2 radians) + assert!(pose.x.abs() < 0.05); // Minimal translation + assert!(pose.y.abs() < 0.05); + assert!((pose.theta - std::f32::consts::FRAC_PI_2).abs() < 0.1); + } +} diff --git a/dhruva-nav/src/planning/cost_map.rs b/dhruva-nav/src/planning/cost_map.rs new file mode 100644 index 0000000..6a2ffde --- /dev/null +++ b/dhruva-nav/src/planning/cost_map.rs @@ -0,0 +1,594 @@ +//! Cost map with obstacle inflation and wall distance penalties. +//! +//! Provides efficient path planning by pre-computing traversability costs +//! and distance-to-obstacle fields using SIMD-optimized algorithms. + +use std::collections::{HashSet, VecDeque}; +use std::simd::{cmp::SimdPartialOrd, f32x4}; +use vastu_slam::{CellType, GridCoord, GridStorage, WorldPoint}; + +/// Number of SIMD lanes for f32 operations +const LANES: usize = 4; + +/// Cost thresholds for path planning +pub mod costs { + /// Safe to traverse, no penalty + pub const FREE: u8 = 0; + /// Close to wall, penalized but traversable + pub const NEAR_OBSTACLE: u8 = 50; + /// Within robot radius, should avoid + pub const INSCRIBED: u8 = 254; + /// Obstacle or unknown, blocked + pub const LETHAL: u8 = 255; +} + +/// Cost map for path planning with obstacle inflation and wall distance penalty. +/// +/// The cost map pre-computes: +/// 1. Traversability costs (lethal, inscribed, inflated, free) +/// 2. Distance field (distance to nearest obstacle in cells) +#[derive(Clone, Debug)] +pub struct CostMap { + /// Grid dimensions + width: usize, + height: usize, + /// Resolution in meters per cell + resolution: f32, + /// Grid origin in world coordinates + origin: WorldPoint, + /// Cost values: 0 = free, 255 = lethal obstacle + costs: Vec, + /// Distance to nearest obstacle (in cells) for each cell + distance_field: Vec, + /// Wall penalty distance in cells + wall_penalty_cells: usize, +} + +impl CostMap { + /// Build cost map from occupancy grid. + /// + /// # Arguments + /// * `grid` - Source occupancy grid + /// * `robot_radius` - Robot radius in meters + /// * `safety_margin` - Extra clearance beyond robot radius in meters + /// * `wall_penalty_dist` - Distance within which to penalize wall proximity in meters + pub fn from_grid( + grid: &GridStorage, + robot_radius: f32, + safety_margin: f32, + wall_penalty_dist: f32, + ) -> Self { + let width = grid.width(); + let height = grid.height(); + let resolution = grid.resolution(); + let (origin, _) = grid.bounds(); + + // Convert distances to cells + let robot_radius_cells = (robot_radius / resolution).ceil() as usize; + let safety_margin_cells = (safety_margin / resolution).ceil() as usize; + let wall_penalty_cells = (wall_penalty_dist / resolution).ceil() as usize; + + let total_cells = width * height; + let mut costs = vec![costs::FREE; total_cells]; + let mut distance_field = vec![f32::MAX; total_cells]; + + // Initialize: mark obstacles and build initial distance field + let mut obstacle_queue = VecDeque::with_capacity(total_cells / 10); + + for y in 0..height as i32 { + for x in 0..width as i32 { + let coord = GridCoord::new(x, y); + let cell_type = grid.get_type(coord); + let idx = (y as usize) * width + (x as usize); + + if cell_type.is_obstacle() || cell_type == CellType::Unknown { + costs[idx] = costs::LETHAL; + distance_field[idx] = 0.0; + obstacle_queue.push_back((x, y)); + } + } + } + + // Compute distance field using Brushfire algorithm + Self::compute_distance_field_brushfire( + &mut distance_field, + width, + height, + &mut obstacle_queue, + ); + + // Apply cost layers based on distance field + Self::apply_cost_layers( + &mut costs, + &distance_field, + width, + height, + robot_radius_cells, + safety_margin_cells, + wall_penalty_cells, + ); + + Self { + width, + height, + resolution, + origin, + costs, + distance_field, + wall_penalty_cells, + } + } + + /// Compute distance field using Brushfire (BFS) algorithm. + fn compute_distance_field_brushfire( + distance_field: &mut [f32], + width: usize, + height: usize, + queue: &mut VecDeque<(i32, i32)>, + ) { + let sqrt2 = 1.414f32; + + // 8-connected neighbors with distances + let neighbors = [ + (-1, 0, 1.0), + (1, 0, 1.0), + (0, -1, 1.0), + (0, 1, 1.0), + (-1, -1, sqrt2), + (1, -1, sqrt2), + (-1, 1, sqrt2), + (1, 1, sqrt2), + ]; + + while let Some((x, y)) = queue.pop_front() { + let current_idx = (y as usize) * width + (x as usize); + let current_dist = distance_field[current_idx]; + + for &(dx, dy, cost) in &neighbors { + let nx = x + dx; + let ny = y + dy; + + // Bounds check + if nx < 0 || ny < 0 || nx >= width as i32 || ny >= height as i32 { + continue; + } + + let neighbor_idx = (ny as usize) * width + (nx as usize); + let new_dist = current_dist + cost; + + if new_dist < distance_field[neighbor_idx] { + distance_field[neighbor_idx] = new_dist; + queue.push_back((nx, ny)); + } + } + } + } + + /// Apply cost layers based on distance field. + fn apply_cost_layers( + costs: &mut [u8], + distance_field: &[f32], + width: usize, + height: usize, + robot_radius_cells: usize, + safety_margin_cells: usize, + wall_penalty_cells: usize, + ) { + let inscribed_dist = robot_radius_cells as f32; + let inflation_dist = (robot_radius_cells + safety_margin_cells) as f32; + let penalty_dist = wall_penalty_cells as f32; + + // SIMD-optimized cost computation + let total_cells = width * height; + let inscribed_vec = f32x4::splat(inscribed_dist); + let inflation_vec = f32x4::splat(inflation_dist); + let penalty_vec = f32x4::splat(penalty_dist); + + // Process 4 cells at a time + let aligned_len = (total_cells / LANES) * LANES; + + for i in (0..aligned_len).step_by(LANES) { + // Skip if already lethal + if costs[i] == costs::LETHAL + || costs[i + 1] == costs::LETHAL + || costs[i + 2] == costs::LETHAL + || costs[i + 3] == costs::LETHAL + { + // Process individually for mixed lethal/non-lethal + for j in 0..LANES { + if costs[i + j] != costs::LETHAL { + costs[i + j] = Self::compute_cell_cost( + distance_field[i + j], + inscribed_dist, + inflation_dist, + penalty_dist, + ); + } + } + continue; + } + + let dist = f32x4::from_slice(&distance_field[i..]); + + // Check inscribed radius + let is_inscribed = dist.simd_le(inscribed_vec); + + // Check inflation radius + let is_inflated = dist.simd_le(inflation_vec); + + // Check wall penalty distance + let is_near_wall = dist.simd_le(penalty_vec); + + // Convert masks to costs + for j in 0..LANES { + if is_inscribed.test(j) { + costs[i + j] = costs::INSCRIBED; + } else if is_inflated.test(j) { + // Exponential decay in inflation zone + let d = distance_field[i + j]; + let ratio = (d - inscribed_dist) / (inflation_dist - inscribed_dist); + let cost = costs::INSCRIBED + - ((costs::INSCRIBED - costs::NEAR_OBSTACLE) as f32 * ratio) as u8; + costs[i + j] = cost.max(costs::NEAR_OBSTACLE); + } else if is_near_wall.test(j) { + // Linear penalty in wall proximity zone + let d = distance_field[i + j]; + let ratio = 1.0 - (d - inflation_dist) / (penalty_dist - inflation_dist); + let cost = (costs::NEAR_OBSTACLE as f32 * ratio.max(0.0)) as u8; + costs[i + j] = cost.max(1); + } + // Otherwise stays FREE (0) + } + } + + // Handle remaining cells + for i in aligned_len..total_cells { + if costs[i] != costs::LETHAL { + costs[i] = Self::compute_cell_cost( + distance_field[i], + inscribed_dist, + inflation_dist, + penalty_dist, + ); + } + } + } + + /// Compute cost for a single cell based on distance to obstacle. + #[inline] + fn compute_cell_cost( + distance: f32, + inscribed_dist: f32, + inflation_dist: f32, + penalty_dist: f32, + ) -> u8 { + if distance <= inscribed_dist { + costs::INSCRIBED + } else if distance <= inflation_dist { + let ratio = (distance - inscribed_dist) / (inflation_dist - inscribed_dist); + let cost = + costs::INSCRIBED - ((costs::INSCRIBED - costs::NEAR_OBSTACLE) as f32 * ratio) as u8; + cost.max(costs::NEAR_OBSTACLE) + } else if distance <= penalty_dist { + let ratio = 1.0 - (distance - inflation_dist) / (penalty_dist - inflation_dist); + let cost = (costs::NEAR_OBSTACLE as f32 * ratio.max(0.0)) as u8; + cost.max(1) + } else { + costs::FREE + } + } + + /// Get cost at grid coordinate. + #[inline] + pub fn cost(&self, coord: GridCoord) -> u8 { + if coord.x < 0 || coord.y < 0 { + return costs::LETHAL; + } + let x = coord.x as usize; + let y = coord.y as usize; + if x >= self.width || y >= self.height { + return costs::LETHAL; + } + self.costs[y * self.width + x] + } + + /// Get distance to nearest obstacle at grid coordinate (in cells). + #[inline] + pub fn obstacle_distance(&self, coord: GridCoord) -> f32 { + if coord.x < 0 || coord.y < 0 { + return 0.0; + } + let x = coord.x as usize; + let y = coord.y as usize; + if x >= self.width || y >= self.height { + return 0.0; + } + self.distance_field[y * self.width + x] + } + + /// Check if a cell is traversable (cost < INSCRIBED). + #[inline] + pub fn is_traversable(&self, coord: GridCoord) -> bool { + self.cost(coord) < costs::INSCRIBED + } + + /// Find the closest traversable cell to a target using BFS. + /// + /// Returns `None` if no traversable cell is found within `max_radius` cells. + /// + /// # Arguments + /// * `target` - The target coordinate to find a reachable point near + /// * `max_radius` - Maximum search radius in cells + pub fn find_closest_reachable_to( + &self, + target: GridCoord, + max_radius: i32, + ) -> Option { + // Check if target itself is traversable + if self.is_traversable(target) { + return Some(target); + } + + // BFS to find closest traversable cell + let mut queue = VecDeque::new(); + let mut visited = HashSet::new(); + + queue.push_back(target); + visited.insert((target.x, target.y)); + + // 8-connected neighbors for search + let neighbors = [ + (-1, 0), + (1, 0), + (0, -1), + (0, 1), + (-1, -1), + (1, -1), + (-1, 1), + (1, 1), + ]; + + while let Some(current) = queue.pop_front() { + // Check distance from target (use Chebyshev distance for rectangular search) + let dx = (current.x - target.x).abs(); + let dy = (current.y - target.y).abs(); + if dx > max_radius || dy > max_radius { + continue; // Outside search radius + } + + // Check if this cell is traversable + if self.is_traversable(current) { + return Some(current); + } + + // Add unvisited neighbors to queue + for (ndx, ndy) in &neighbors { + let nx = current.x + ndx; + let ny = current.y + ndy; + + // Bounds check + if nx < 0 || ny < 0 || nx >= self.width as i32 || ny >= self.height as i32 { + continue; + } + + if !visited.contains(&(nx, ny)) { + visited.insert((nx, ny)); + queue.push_back(GridCoord::new(nx, ny)); + } + } + } + + None + } + + /// Find the closest traversable cell and return as world coordinates. + /// + /// # Arguments + /// * `target` - Target point in world coordinates + /// * `max_radius_meters` - Maximum search radius in meters + pub fn find_closest_reachable_world( + &self, + target: WorldPoint, + max_radius_meters: f32, + ) -> Option { + let target_coord = self.world_to_grid(target); + let max_radius_cells = (max_radius_meters / self.resolution).ceil() as i32; + + self.find_closest_reachable_to(target_coord, max_radius_cells) + .map(|coord| self.grid_to_world(coord)) + } + + /// Check if line-of-sight is clear between two grid coordinates. + /// + /// Uses Bresenham's line algorithm to check all cells along the line. + pub fn line_of_sight(&self, from: GridCoord, to: GridCoord) -> bool { + let mut x0 = from.x; + let mut y0 = from.y; + let x1 = to.x; + let y1 = to.y; + + let dx = (x1 - x0).abs(); + let dy = (y1 - y0).abs(); + let sx = if x0 < x1 { 1 } else { -1 }; + let sy = if y0 < y1 { 1 } else { -1 }; + let mut err = dx - dy; + + loop { + // Check current cell + if self.cost(GridCoord::new(x0, y0)) >= costs::INSCRIBED { + return false; + } + + if x0 == x1 && y0 == y1 { + break; + } + + let e2 = 2 * err; + if e2 > -dy { + err -= dy; + x0 += sx; + } + if e2 < dx { + err += dx; + y0 += sy; + } + } + + true + } + + /// Compute wall penalty along a line segment. + /// + /// Returns the integrated penalty based on distance to walls. + pub fn integrate_wall_penalty(&self, from: GridCoord, to: GridCoord) -> f32 { + let mut penalty = 0.0; + let mut count = 0; + + let mut x0 = from.x; + let mut y0 = from.y; + let x1 = to.x; + let y1 = to.y; + + let dx = (x1 - x0).abs(); + let dy = (y1 - y0).abs(); + let sx = if x0 < x1 { 1 } else { -1 }; + let sy = if y0 < y1 { 1 } else { -1 }; + let mut err = dx - dy; + + let penalty_dist = self.wall_penalty_cells as f32; + + loop { + let coord = GridCoord::new(x0, y0); + let dist = self.obstacle_distance(coord); + + if dist < penalty_dist { + penalty += (penalty_dist - dist) / penalty_dist; + } + count += 1; + + if x0 == x1 && y0 == y1 { + break; + } + + let e2 = 2 * err; + if e2 > -dy { + err -= dy; + x0 += sx; + } + if e2 < dx { + err += dx; + y0 += sy; + } + } + + // Normalize by path length + if count > 0 { + penalty / count as f32 + } else { + 0.0 + } + } + + /// Convert world coordinates to grid coordinates. + #[inline] + pub fn world_to_grid(&self, point: WorldPoint) -> GridCoord { + let x = ((point.x - self.origin.x) / self.resolution).floor() as i32; + let y = ((point.y - self.origin.y) / self.resolution).floor() as i32; + GridCoord::new(x, y) + } + + /// Convert grid coordinates to world coordinates (cell center). + #[inline] + pub fn grid_to_world(&self, coord: GridCoord) -> WorldPoint { + WorldPoint::new( + self.origin.x + (coord.x as f32 + 0.5) * self.resolution, + self.origin.y + (coord.y as f32 + 0.5) * self.resolution, + ) + } + + /// Get grid resolution in meters. + #[inline] + pub fn resolution(&self) -> f32 { + self.resolution + } +} + +#[cfg(test)] +mod tests { + use super::*; + + fn create_test_grid() -> GridStorage { + let mut grid = GridStorage::new(20, 20, 0.05, WorldPoint::ZERO); + + // Add some walls + for x in 5..15 { + grid.set_type(GridCoord::new(x, 10), CellType::Wall); + } + + // Mark rest as floor + for y in 0..20 { + for x in 0..20 { + let coord = GridCoord::new(x, y); + if grid.get_type(coord) == CellType::Unknown { + grid.set_type(coord, CellType::Floor); + } + } + } + + grid + } + + #[test] + fn test_cost_map_creation() { + let grid = create_test_grid(); + let cost_map = CostMap::from_grid(&grid, 0.10, 0.05, 0.20); + + // Wall cells should be lethal + assert_eq!(cost_map.cost(GridCoord::new(10, 10)), costs::LETHAL); + + // Cells far from walls should be free or low cost + let far_cost = cost_map.cost(GridCoord::new(10, 0)); + assert!(far_cost < costs::INSCRIBED); + } + + #[test] + fn test_distance_field() { + let grid = create_test_grid(); + let cost_map = CostMap::from_grid(&grid, 0.10, 0.05, 0.20); + + // Wall cells have distance 0 + assert_eq!(cost_map.obstacle_distance(GridCoord::new(10, 10)), 0.0); + + // Adjacent cells have distance ~1 + let adj_dist = cost_map.obstacle_distance(GridCoord::new(10, 9)); + assert!(adj_dist > 0.9 && adj_dist < 1.1); + + // Distant cells have larger distance + let far_dist = cost_map.obstacle_distance(GridCoord::new(10, 0)); + assert!(far_dist > 5.0); + } + + #[test] + fn test_line_of_sight() { + let grid = create_test_grid(); + let cost_map = CostMap::from_grid(&grid, 0.10, 0.05, 0.20); + + // Line that doesn't cross wall + assert!(cost_map.line_of_sight(GridCoord::new(0, 0), GridCoord::new(5, 5))); + + // Line that crosses wall + assert!(!cost_map.line_of_sight(GridCoord::new(10, 5), GridCoord::new(10, 15))); + } + + #[test] + fn test_traversability() { + let grid = create_test_grid(); + let cost_map = CostMap::from_grid(&grid, 0.10, 0.05, 0.20); + + // Wall is not traversable + assert!(!cost_map.is_traversable(GridCoord::new(10, 10))); + + // Open space is traversable + assert!(cost_map.is_traversable(GridCoord::new(10, 0))); + } +} diff --git a/dhruva-nav/src/planning/mod.rs b/dhruva-nav/src/planning/mod.rs new file mode 100644 index 0000000..0b00e33 --- /dev/null +++ b/dhruva-nav/src/planning/mod.rs @@ -0,0 +1,14 @@ +//! Path planning module for autonomous navigation. +//! +//! This module provides: +//! - Cost map with obstacle inflation and wall distance penalties +//! - Theta* path planner for any-angle pathfinding +//! - Path smoother for differential drive kinematics + +mod cost_map; +mod smoother; +mod theta_star; + +pub use cost_map::CostMap; +pub use smoother::{PathSegment, PathSmoother, SmoothedPath}; +pub use theta_star::ThetaStarPlanner; diff --git a/dhruva-nav/src/planning/smoother.rs b/dhruva-nav/src/planning/smoother.rs new file mode 100644 index 0000000..470618b --- /dev/null +++ b/dhruva-nav/src/planning/smoother.rs @@ -0,0 +1,287 @@ +//! Path smoother for differential drive kinematics. +//! +//! Post-processes Theta* paths to ensure smooth motion by: +//! - Inserting arc segments for medium turns +//! - Inserting point turns for sharp angles +//! - Respecting minimum turning radius constraints + +use vastu_slam::WorldPoint; + +use super::theta_star::PlannedPath; + +/// Configuration for path smoothing. +#[derive(Clone, Debug)] +pub struct SmootherConfig { + /// Maximum linear velocity (m/s) + pub max_linear_vel: f32, + /// Maximum angular velocity (rad/s) + pub max_angular_vel: f32, + /// Angle threshold for inserting arcs (radians, ~30°) + pub arc_turn_threshold: f32, + /// Angle threshold for point turns (radians, ~90°) + pub sharp_turn_threshold: f32, +} + +impl Default for SmootherConfig { + fn default() -> Self { + Self { + max_linear_vel: 0.2, + max_angular_vel: 0.5, + arc_turn_threshold: 0.52, // ~30 degrees + sharp_turn_threshold: 1.57, // ~90 degrees + } + } +} + +impl SmootherConfig { + /// Compute minimum turning radius from velocity constraints. + pub fn min_turn_radius(&self) -> f32 { + self.max_linear_vel / self.max_angular_vel + } +} + +/// Type of path segment. +#[derive(Clone, Debug)] +pub enum PathSegment { + /// Straight line segment + Line { start: WorldPoint, end: WorldPoint }, + /// In-place rotation for sharp turns + PointTurn { + position: WorldPoint, + from_angle: f32, + to_angle: f32, + }, +} + +impl PathSegment { + /// Get the starting point of this segment. + pub fn start_point(&self) -> WorldPoint { + match self { + PathSegment::Line { start, .. } => *start, + PathSegment::PointTurn { position, .. } => *position, + } + } + + /// Get the ending point of this segment. + pub fn end_point(&self) -> WorldPoint { + match self { + PathSegment::Line { end, .. } => *end, + PathSegment::PointTurn { position, .. } => *position, + } + } + + /// Get the length of this segment in meters. + pub fn length(&self) -> f32 { + match self { + PathSegment::Line { start, end } => start.distance(end), + PathSegment::PointTurn { .. } => 0.0, // No linear distance + } + } +} + +/// Smoothed path with segments. +#[derive(Clone, Debug)] +pub struct SmoothedPath { + /// Path segments (lines, arcs, point turns) + pub segments: Vec, + /// Total path length in meters + pub total_length: f32, + /// Estimated traversal time in seconds + pub estimated_time: f32, +} + +/// Path smoother for differential drive robots. +pub struct PathSmoother { + config: SmootherConfig, +} + +impl PathSmoother { + /// Create a new path smoother with configuration. + pub fn new(config: SmootherConfig) -> Self { + Self { config } + } + + /// Create a new path smoother with default configuration. + pub fn with_defaults() -> Self { + Self::new(SmootherConfig::default()) + } + + /// Smooth a planned path for differential drive motion. + pub fn smooth(&self, path: &PlannedPath) -> SmoothedPath { + if path.waypoints.len() < 2 { + return SmoothedPath { + segments: Vec::new(), + total_length: 0.0, + estimated_time: 0.0, + }; + } + + let mut segments = Vec::new(); + let _min_radius = self.config.min_turn_radius(); + + // Process waypoints and analyze turn angles + for i in 0..path.waypoints.len() - 1 { + let current = path.waypoints[i]; + let next = path.waypoints[i + 1]; + + // Compute incoming and outgoing headings + let incoming_heading = if i > 0 { + let prev = path.waypoints[i - 1]; + (current.y - prev.y).atan2(current.x - prev.x) + } else { + (next.y - current.y).atan2(next.x - current.x) + }; + + let outgoing_heading = (next.y - current.y).atan2(next.x - current.x); + + // Compute turn angle + let turn_angle = Self::normalize_angle(outgoing_heading - incoming_heading); + let turn_angle_abs = turn_angle.abs(); + + // Decide what type of segment to create + if i > 0 && turn_angle_abs > self.config.sharp_turn_threshold { + // Sharp turn: insert point turn + segments.push(PathSegment::PointTurn { + position: current, + from_angle: incoming_heading, + to_angle: outgoing_heading, + }); + } else if i > 0 && turn_angle_abs > self.config.arc_turn_threshold { + // Medium turn: could insert arc, but for simplicity use point turn + // Full arc implementation requires more complex geometry + segments.push(PathSegment::PointTurn { + position: current, + from_angle: incoming_heading, + to_angle: outgoing_heading, + }); + } + + // Add line segment + segments.push(PathSegment::Line { + start: current, + end: next, + }); + } + + // Compute totals + let total_length: f32 = segments.iter().map(|s| s.length()).sum(); + let estimated_time = self.estimate_traversal_time(&segments); + + SmoothedPath { + segments, + total_length, + estimated_time, + } + } + + /// Estimate traversal time for the path. + fn estimate_traversal_time(&self, segments: &[PathSegment]) -> f32 { + let mut time = 0.0; + + for segment in segments { + match segment { + PathSegment::Line { start, end } => { + let distance = start.distance(end); + time += distance / self.config.max_linear_vel; + } + PathSegment::PointTurn { + from_angle, + to_angle, + .. + } => { + let angle_diff = Self::normalize_angle(to_angle - from_angle).abs(); + time += angle_diff / self.config.max_angular_vel; + } + } + } + + time + } + + /// Normalize angle to [-pi, pi]. + #[inline] + fn normalize_angle(angle: f32) -> f32 { + let mut a = angle; + while a > std::f32::consts::PI { + a -= 2.0 * std::f32::consts::PI; + } + while a < -std::f32::consts::PI { + a += 2.0 * std::f32::consts::PI; + } + a + } +} + +#[cfg(test)] +mod tests { + use super::*; + + fn create_test_path() -> PlannedPath { + PlannedPath { + waypoints: vec![ + WorldPoint::new(0.0, 0.0), + WorldPoint::new(1.0, 0.0), + WorldPoint::new(1.0, 1.0), + WorldPoint::new(0.0, 1.0), + ], + length: 3.0, + } + } + + #[test] + fn test_smooth_simple_path() { + let path = create_test_path(); + let smoother = PathSmoother::with_defaults(); + + let smoothed = smoother.smooth(&path); + + // Should have line segments and point turns + assert!(!smoothed.segments.is_empty()); + assert!(smoothed.total_length > 0.0); + assert!(smoothed.estimated_time > 0.0); + } + + #[test] + fn test_sharp_turn_detection() { + let path = PlannedPath { + waypoints: vec![ + WorldPoint::new(0.0, 0.0), + WorldPoint::new(1.0, 0.0), + WorldPoint::new(1.0, 1.0), // 90 degree turn + ], + length: 2.0, + }; + + let smoother = PathSmoother::with_defaults(); + let smoothed = smoother.smooth(&path); + + // Should detect the 90 degree turn + let has_point_turn = smoothed + .segments + .iter() + .any(|s| matches!(s, PathSegment::PointTurn { .. })); + assert!(has_point_turn); + } + + #[test] + fn test_straight_path_no_turns() { + let path = PlannedPath { + waypoints: vec![ + WorldPoint::new(0.0, 0.0), + WorldPoint::new(1.0, 0.0), + WorldPoint::new(2.0, 0.0), + ], + length: 2.0, + }; + + let smoother = PathSmoother::with_defaults(); + let smoothed = smoother.smooth(&path); + + // Should only have line segments + let all_lines = smoothed + .segments + .iter() + .all(|s| matches!(s, PathSegment::Line { .. })); + assert!(all_lines); + } +} diff --git a/dhruva-nav/src/planning/theta_star.rs b/dhruva-nav/src/planning/theta_star.rs new file mode 100644 index 0000000..5ffae81 --- /dev/null +++ b/dhruva-nav/src/planning/theta_star.rs @@ -0,0 +1,422 @@ +//! Theta* path planner for any-angle pathfinding. +//! +//! Theta* extends A* with line-of-sight checks to produce smoother, +//! shorter paths that aren't constrained to grid directions. + +use std::cmp::Ordering; +use std::collections::{BinaryHeap, HashMap}; + +use vastu_slam::{GridCoord, Pose2D, WorldPoint}; + +use super::cost_map::{CostMap, costs}; + +/// Configuration for the Theta* path planner. +#[derive(Clone, Debug)] +pub struct ThetaStarConfig { + /// Maximum iterations before giving up + pub max_iterations: usize, + /// Weight for wall proximity penalty in path cost + pub wall_penalty_weight: f32, + /// Goal tolerance in cells + pub goal_tolerance: i32, +} + +impl Default for ThetaStarConfig { + fn default() -> Self { + Self { + max_iterations: 50000, + wall_penalty_weight: 2.0, + goal_tolerance: 2, + } + } +} + +/// Result of path planning. +#[derive(Clone, Debug)] +pub struct PlannedPath { + /// Waypoints in world coordinates + pub waypoints: Vec, + /// Total path length in meters + pub length: f32, +} + +/// Node in the search graph. +#[derive(Clone, Debug)] +struct SearchNode { + coord: GridCoord, + f_score: f32, +} + +impl PartialEq for SearchNode { + fn eq(&self, other: &Self) -> bool { + self.coord == other.coord + } +} + +impl Eq for SearchNode {} + +impl Ord for SearchNode { + fn cmp(&self, other: &Self) -> Ordering { + // Reverse ordering for min-heap (lower f_score = higher priority) + other + .f_score + .partial_cmp(&self.f_score) + .unwrap_or(Ordering::Equal) + } +} + +impl PartialOrd for SearchNode { + fn partial_cmp(&self, other: &Self) -> Option { + Some(self.cmp(other)) + } +} + +/// Theta* path planner. +pub struct ThetaStarPlanner { + config: ThetaStarConfig, +} + +impl ThetaStarPlanner { + /// Create a new Theta* planner with configuration. + pub fn new(config: ThetaStarConfig) -> Self { + Self { config } + } + + /// Create a new Theta* planner with default configuration. + pub fn with_defaults() -> Self { + Self::new(ThetaStarConfig::default()) + } + + /// Plan a path from start pose to goal point. + /// + /// Returns `None` if no valid path exists. + pub fn plan(&self, cost_map: &CostMap, start: Pose2D, goal: WorldPoint) -> Option { + let start_coord = cost_map.world_to_grid(WorldPoint::new(start.x, start.y)); + let goal_coord = cost_map.world_to_grid(goal); + + // Check if start is valid, if not find nearest traversable cell + let actual_start = if !cost_map.is_traversable(start_coord) { + tracing::warn!("Start position is not traversable, searching for nearby valid cell"); + if let Some(valid_start) = Self::find_nearest_traversable(cost_map, start_coord, 10) { + valid_start + } else { + tracing::warn!("No traversable cell found near start position"); + return None; + } + } else { + start_coord + }; + + // Check if goal is valid + // Note: We don't use find_nearest_traversable for the goal because if the specified + // goal is blocked, we should fail explicitly rather than silently redirecting to a + // different location. The explorer/caller should handle goal validation. + if !cost_map.is_traversable(goal_coord) { + tracing::warn!( + "Goal position ({}, {}) is not traversable", + goal_coord.x, + goal_coord.y + ); + return None; + } + let actual_goal = goal_coord; + + // Run Theta* search + let path_coords = self.theta_star_search(cost_map, actual_start, actual_goal)?; + + // Convert to world coordinates and compute statistics + let waypoints: Vec = path_coords + .iter() + .map(|&coord| cost_map.grid_to_world(coord)) + .collect(); + + let length = self.compute_path_length(&waypoints); + + Some(PlannedPath { waypoints, length }) + } + + /// Theta* search algorithm. + fn theta_star_search( + &self, + cost_map: &CostMap, + start: GridCoord, + goal: GridCoord, + ) -> Option> { + let mut open_set = BinaryHeap::new(); + let mut g_score: HashMap = HashMap::new(); + let mut parent: HashMap = HashMap::new(); + let mut closed_set: HashMap = HashMap::new(); + + // Initialize start node + g_score.insert(start, 0.0); + parent.insert(start, start); // Start is its own parent + open_set.push(SearchNode { + coord: start, + f_score: Self::heuristic(start, goal), + }); + + let mut iterations = 0; + + // 8-connected neighbors + let neighbors = [ + GridCoord::new(-1, 0), + GridCoord::new(1, 0), + GridCoord::new(0, -1), + GridCoord::new(0, 1), + GridCoord::new(-1, -1), + GridCoord::new(1, -1), + GridCoord::new(-1, 1), + GridCoord::new(1, 1), + ]; + + while let Some(current_node) = open_set.pop() { + iterations += 1; + if iterations > self.config.max_iterations { + tracing::warn!("Theta* exceeded max iterations"); + return None; + } + + let current = current_node.coord; + + // Check if we've reached the goal (with tolerance) + if current.manhattan_distance(&goal) <= self.config.goal_tolerance { + return Some(self.reconstruct_path(&parent, current)); + } + + // Skip if already processed + if closed_set.contains_key(¤t) { + continue; + } + closed_set.insert(current, true); + + let current_g = *g_score.get(¤t).unwrap_or(&f32::MAX); + let current_parent = *parent.get(¤t).unwrap_or(¤t); + + // Explore neighbors + for &offset in &neighbors { + let neighbor = GridCoord::new(current.x + offset.x, current.y + offset.y); + + // Skip if not traversable + if cost_map.cost(neighbor) >= costs::INSCRIBED { + continue; + } + + // Skip if already processed + if closed_set.contains_key(&neighbor) { + continue; + } + + // Theta* key insight: check line-of-sight to grandparent + let (new_g, new_parent) = if current_parent != current + && cost_map.line_of_sight(current_parent, neighbor) + { + // Path 2: Direct path from grandparent (any-angle) + let parent_g = *g_score.get(¤t_parent).unwrap_or(&f32::MAX); + let edge_cost = self.path_cost(cost_map, current_parent, neighbor); + (parent_g + edge_cost, current_parent) + } else { + // Path 1: Through current node (grid-constrained) + let edge_cost = self.path_cost(cost_map, current, neighbor); + (current_g + edge_cost, current) + }; + + let existing_g = *g_score.get(&neighbor).unwrap_or(&f32::MAX); + + if new_g < existing_g { + g_score.insert(neighbor, new_g); + parent.insert(neighbor, new_parent); + + let f_score = new_g + Self::heuristic(neighbor, goal); + open_set.push(SearchNode { + coord: neighbor, + f_score, + }); + } + } + } + + // No path found + None + } + + /// Compute heuristic (Euclidean distance). + #[inline] + fn heuristic(from: GridCoord, to: GridCoord) -> f32 { + let dx = (to.x - from.x) as f32; + let dy = (to.y - from.y) as f32; + (dx * dx + dy * dy).sqrt() + } + + /// Compute path cost between two points including wall penalty. + fn path_cost(&self, cost_map: &CostMap, from: GridCoord, to: GridCoord) -> f32 { + let dx = (to.x - from.x) as f32; + let dy = (to.y - from.y) as f32; + let distance = (dx * dx + dy * dy).sqrt(); + + let wall_penalty = cost_map.integrate_wall_penalty(from, to); + + distance + self.config.wall_penalty_weight * wall_penalty * distance + } + + /// Reconstruct path from parent map. + fn reconstruct_path( + &self, + parent: &HashMap, + goal: GridCoord, + ) -> Vec { + let mut path = Vec::new(); + let mut current = goal; + + loop { + path.push(current); + match parent.get(¤t) { + Some(&p) if p != current => current = p, + _ => break, + } + } + + path.reverse(); + path + } + + /// Compute path length. + fn compute_path_length(&self, waypoints: &[WorldPoint]) -> f32 { + if waypoints.len() < 2 { + return 0.0; + } + + let mut length = 0.0; + + for i in 0..waypoints.len() - 1 { + let from = waypoints[i]; + let to = waypoints[i + 1]; + + let dx = to.x - from.x; + let dy = to.y - from.y; + length += (dx * dx + dy * dy).sqrt(); + } + + length + } + + /// Find the nearest traversable cell to a given coordinate. + /// Uses BFS to search in expanding rings around the target. + fn find_nearest_traversable( + cost_map: &CostMap, + center: GridCoord, + max_radius: i32, + ) -> Option { + // Check center first + if cost_map.is_traversable(center) { + return Some(center); + } + + // Search in expanding rings + for r in 1..=max_radius { + // Check all cells at distance r (Manhattan-ish) + for dx in -r..=r { + for dy in -r..=r { + // Only check cells on the "ring" boundary + if dx.abs() != r && dy.abs() != r { + continue; + } + let coord = GridCoord::new(center.x + dx, center.y + dy); + if cost_map.is_traversable(coord) { + return Some(coord); + } + } + } + } + + None + } +} + +#[cfg(test)] +mod tests { + use super::*; + use vastu_slam::{CellType, GridStorage}; + + fn create_test_grid() -> GridStorage { + let mut grid = GridStorage::new(50, 50, 0.05, WorldPoint::ZERO); + + // Mark all as floor + for y in 0..50 { + for x in 0..50 { + grid.set_type(GridCoord::new(x, y), CellType::Floor); + } + } + + // Add a wall in the middle + for x in 10..40 { + grid.set_type(GridCoord::new(x, 25), CellType::Wall); + } + + grid + } + + #[test] + fn test_simple_path() { + let grid = create_test_grid(); + let cost_map = CostMap::from_grid(&grid, 0.10, 0.05, 0.20); + let planner = ThetaStarPlanner::with_defaults(); + + let start = Pose2D::new(0.5, 0.5, 0.0); + let goal = WorldPoint::new(2.0, 0.5); + + let path = planner.plan(&cost_map, start, goal); + assert!(path.is_some()); + + let path = path.unwrap(); + assert!(path.waypoints.len() >= 2); + assert!(path.length > 0.0); + } + + #[test] + fn test_path_around_obstacle() { + let grid = create_test_grid(); + let cost_map = CostMap::from_grid(&grid, 0.10, 0.05, 0.20); + let planner = ThetaStarPlanner::with_defaults(); + + // Path that needs to go around the wall + let start = Pose2D::new(1.25, 1.0, 0.0); + let goal = WorldPoint::new(1.25, 1.5); + + let path = planner.plan(&cost_map, start, goal); + assert!(path.is_some()); + + let path = path.unwrap(); + // Path should go around, so it will be longer than direct line + assert!(path.length > 0.5); + } + + #[test] + fn test_no_path_to_blocked_goal() { + let mut grid = GridStorage::new(20, 20, 0.05, WorldPoint::ZERO); + + // Create an enclosed area + for y in 0..20 { + for x in 0..20 { + grid.set_type(GridCoord::new(x, y), CellType::Floor); + } + } + // Surround center with walls + for x in 8..12 { + grid.set_type(GridCoord::new(x, 8), CellType::Wall); + grid.set_type(GridCoord::new(x, 12), CellType::Wall); + } + for y in 8..12 { + grid.set_type(GridCoord::new(8, y), CellType::Wall); + grid.set_type(GridCoord::new(12, y), CellType::Wall); + } + + let cost_map = CostMap::from_grid(&grid, 0.10, 0.05, 0.20); + let planner = ThetaStarPlanner::with_defaults(); + + let start = Pose2D::new(0.25, 0.25, 0.0); + let goal = WorldPoint::new(0.5, 0.5); // Inside enclosed area + + let path = planner.plan(&cost_map, start, goal); + assert!(path.is_none()); + } +} diff --git a/dhruva-nav/src/shared.rs b/dhruva-nav/src/shared.rs new file mode 100644 index 0000000..90b59eb --- /dev/null +++ b/dhruva-nav/src/shared.rs @@ -0,0 +1,275 @@ +//! Shared state for multi-threaded architecture. +//! +//! Provides thread-safe shared state between: +//! - Sensor thread (UDP reading, odometry, safety, commands) +//! - Mapping thread (scan matching, map updates) +//! - Exploration thread (frontier detection, path planning) + +use std::sync::atomic::{AtomicBool, AtomicU32, AtomicU64, Ordering}; +use std::sync::{Arc, RwLock}; + +use vastu_slam::{GridStorage, Pose2D}; + +use crate::utils::normalize_angle; + +/// Atomic wrapper for f32 values. +/// Uses AtomicU32 with bit reinterpretation. +#[derive(Debug)] +pub struct AtomicF32(AtomicU32); + +impl AtomicF32 { + pub fn new(val: f32) -> Self { + Self(AtomicU32::new(val.to_bits())) + } + + pub fn load(&self, order: Ordering) -> f32 { + f32::from_bits(self.0.load(order)) + } + + pub fn store(&self, val: f32, order: Ordering) { + self.0.store(val.to_bits(), order); + } +} + +/// Atomic wrapper for Pose2D. +/// Packs x, y, theta into atomic u64 values for lock-free access. +/// Uses fixed-point representation: 16-bit integer + 16-bit fraction per component. +#[derive(Debug)] +pub struct AtomicPose { + // Pack x and y into one u64 (each as i32 in mm for precision) + xy: AtomicU64, + // Theta as fixed-point (multiplied by 10000) + theta: AtomicU32, +} + +impl AtomicPose { + pub fn new(pose: Pose2D) -> Self { + let x_mm = (pose.x * 1000.0) as i32; + let y_mm = (pose.y * 1000.0) as i32; + let xy = ((x_mm as u64) << 32) | (y_mm as u32 as u64); + let theta = (pose.theta * 10000.0) as i32 as u32; + + Self { + xy: AtomicU64::new(xy), + theta: AtomicU32::new(theta), + } + } + + pub fn load(&self, order: Ordering) -> Pose2D { + let xy = self.xy.load(order); + let x_mm = (xy >> 32) as i32; + let y_mm = xy as i32; + let theta_fp = self.theta.load(order) as i32; + + Pose2D::new( + x_mm as f32 / 1000.0, + y_mm as f32 / 1000.0, + theta_fp as f32 / 10000.0, + ) + } + + pub fn store(&self, pose: Pose2D, order: Ordering) { + let x_mm = (pose.x * 1000.0) as i32; + let y_mm = (pose.y * 1000.0) as i32; + let xy = ((x_mm as u64) << 32) | (y_mm as u32 as u64); + let theta = (pose.theta * 10000.0) as i32 as u32; + + self.xy.store(xy, order); + self.theta.store(theta, order); + } +} + +/// Shared state between all threads. +#[derive(Debug)] +pub struct SharedState { + /// Last corrected pose from scan matching (updated by mapping thread) + corrected_pose: AtomicPose, + + /// Raw odometry pose at the time of last correction + odometry_at_correction: AtomicPose, + + /// Current raw odometry pose (updated by sensor thread) + current_odometry: AtomicPose, + + /// Velocity command: linear (m/s) + pub linear_vel: AtomicF32, + + /// Velocity command: angular (rad/s) + pub angular_vel: AtomicF32, + + /// Safety stop flag + pub safety_stop: AtomicBool, + + /// Safety stop reason (if any) + pub safety_reason: RwLock>, + + /// Exploration complete flag + pub exploration_complete: AtomicBool, + + /// Shutdown signal for graceful termination + pub shutdown: AtomicBool, + + /// Number of scans processed (for status reporting) + pub scan_count: AtomicU32, + + /// Number of frontiers remaining + pub frontiers_remaining: AtomicU32, +} + +impl SharedState { + /// Create new shared state with initial pose. + pub fn new(initial_pose: Pose2D) -> Self { + Self { + corrected_pose: AtomicPose::new(initial_pose), + odometry_at_correction: AtomicPose::new(initial_pose), + current_odometry: AtomicPose::new(initial_pose), + linear_vel: AtomicF32::new(0.0), + angular_vel: AtomicF32::new(0.0), + safety_stop: AtomicBool::new(false), + safety_reason: RwLock::new(None), + exploration_complete: AtomicBool::new(false), + shutdown: AtomicBool::new(false), + scan_count: AtomicU32::new(0), + frontiers_remaining: AtomicU32::new(0), + } + } + + /// Get current pose (corrected pose + odometry delta since last correction). + /// + /// This provides a smooth, frequently-updated pose that incorporates: + /// - Scan matching corrections from the mapping thread (5Hz) + /// - Odometry updates from the sensor thread (110Hz) + pub fn pose(&self) -> Pose2D { + let corrected = self.corrected_pose.load(Ordering::Acquire); + let odom_at_correction = self.odometry_at_correction.load(Ordering::Acquire); + let current_odom = self.current_odometry.load(Ordering::Acquire); + + // Compute odometry delta since last correction + let dx = current_odom.x - odom_at_correction.x; + let dy = current_odom.y - odom_at_correction.y; + // Normalize dtheta to handle wraparound (e.g., -179° to +179° = 2°, not 358°) + let dtheta = normalize_angle(current_odom.theta - odom_at_correction.theta); + + // Apply delta to corrected pose (normalize final theta to [-π, π]) + Pose2D::new( + corrected.x + dx, + corrected.y + dy, + normalize_angle(corrected.theta + dtheta), + ) + } + + /// Update raw odometry pose (called by sensor thread at 110Hz). + pub fn set_odometry(&self, pose: Pose2D) { + self.current_odometry.store(pose, Ordering::Release); + } + + /// Apply scan matching correction (called by mapping thread at 5Hz). + /// + /// This updates the corrected pose and records the odometry at correction time, + /// so future pose() calls can apply the correct delta. + pub fn apply_correction(&self, corrected: Pose2D, odometry_at_scan: Pose2D) { + self.corrected_pose.store(corrected, Ordering::Release); + self.odometry_at_correction + .store(odometry_at_scan, Ordering::Release); + } + + /// Get velocity command (linear, angular). + pub fn velocity(&self) -> (f32, f32) { + ( + self.linear_vel.load(Ordering::Acquire), + self.angular_vel.load(Ordering::Acquire), + ) + } + + /// Set velocity command. + pub fn set_velocity(&self, linear: f32, angular: f32) { + self.linear_vel.store(linear, Ordering::Release); + self.angular_vel.store(angular, Ordering::Release); + } + + /// Trigger safety stop with reason. + pub fn trigger_safety_stop(&self, reason: String) { + if let Ok(mut guard) = self.safety_reason.write() { + *guard = Some(reason); + } + self.safety_stop.store(true, Ordering::Release); + } + + /// Check if safety stop is triggered. + pub fn is_safety_stop(&self) -> bool { + self.safety_stop.load(Ordering::Acquire) + } + + /// Get safety stop reason. + pub fn safety_reason(&self) -> Option { + self.safety_reason.read().ok().and_then(|g| g.clone()) + } + + /// Signal shutdown. + pub fn signal_shutdown(&self) { + self.shutdown.store(true, Ordering::Release); + } + + /// Check if shutdown is signaled. + pub fn should_shutdown(&self) -> bool { + self.shutdown.load(Ordering::Acquire) + } + + /// Mark exploration as complete. + pub fn set_exploration_complete(&self) { + self.exploration_complete.store(true, Ordering::Release); + } + + /// Check if exploration is complete. + pub fn is_exploration_complete(&self) -> bool { + self.exploration_complete.load(Ordering::Acquire) + } + + /// Increment scan count. + pub fn increment_scan_count(&self) { + self.scan_count.fetch_add(1, Ordering::Relaxed); + } + + /// Get scan count. + pub fn scan_count(&self) -> u32 { + self.scan_count.load(Ordering::Relaxed) + } + + /// Set frontiers remaining. + pub fn set_frontiers_remaining(&self, count: usize) { + self.frontiers_remaining + .store(count as u32, Ordering::Relaxed); + } +} + +/// Thread-safe map storage wrapper. +pub type SharedGrid = Arc>; + +/// Thread-safe trajectory storage for SVG visualization. +pub type SharedTrajectory = Arc>>; + +/// Debug visualization data for SVG output. +#[derive(Clone, Debug, Default)] +pub struct DebugVisualization { + /// Frontier centroids that were visited + pub visited_frontiers: Vec, + /// Waypoints from planned paths + pub waypoints: Vec, +} + +/// Thread-safe debug visualization data. +pub type SharedDebugViz = Arc>; + +/// Message types for inter-thread communication. +pub mod messages { + use super::*; + + /// Lidar scan data sent from sensor thread to mapping thread. + #[derive(Clone)] + pub struct LidarScanMsg { + /// Lidar points: (angle_rad, distance_m, quality) + pub points: Vec<(f32, f32, u8)>, + /// Encoder-based pose at scan time + pub encoder_pose: Pose2D, + } +} diff --git a/dhruva-nav/src/threads/exploration.rs b/dhruva-nav/src/threads/exploration.rs new file mode 100644 index 0000000..7a43c70 --- /dev/null +++ b/dhruva-nav/src/threads/exploration.rs @@ -0,0 +1,254 @@ +//! Exploration thread: Frontier detection and path planning. +//! +//! This thread reads from the shared grid and: +//! - Detects frontiers (boundaries between known and unknown space) +//! - Plans paths to frontiers using Theta* +//! - Follows paths using pure pursuit +//! - Updates velocity commands in shared state + +use std::sync::Arc; +use std::time::{Duration, Instant}; + +use crate::config::DhruvaConfig; +use crate::exploration::{ + ExplorationState, Explorer, ExplorerConfig, FollowerConfig, FrontierConfig, +}; +use crate::shared::{SharedDebugViz, SharedGrid, SharedState}; + +/// Exploration thread state and logic. +pub struct ExplorationThread { + shared_state: Arc, + shared_grid: SharedGrid, + shared_debug_viz: SharedDebugViz, + explorer: Explorer, + last_status_time: Instant, + status_interval: Duration, + started: bool, + min_scans_to_start: u32, +} + +impl ExplorationThread { + /// Create a new exploration thread. + pub fn new( + config: DhruvaConfig, + shared_state: Arc, + shared_grid: SharedGrid, + shared_debug_viz: SharedDebugViz, + ) -> Self { + // Configure exploration from config + let explorer_config = ExplorerConfig { + frontier: FrontierConfig { + min_cluster_size: config.exploration.min_frontier_size, + ..Default::default() + }, + follower: FollowerConfig { + goal_tolerance: config.exploration.goal_tolerance, + max_linear_vel: config.robot.max_linear_vel, + max_angular_vel: config.robot.max_angular_vel, + ..Default::default() + }, + robot_radius: config.exploration.robot_radius, + safety_margin: config.exploration.safety_margin, + wall_penalty_distance: config.exploration.wall_penalty_distance, + frontier_refresh_interval: config.exploration.frontier_refresh_interval, + return_to_start: config.exploration.return_to_start, + // Recovery and stuck detection parameters + max_replans_per_frontier: config.exploration.max_replans_per_frontier, + frontier_timeout_secs: config.exploration.frontier_timeout_secs, + min_progress_distance: config.exploration.min_progress_distance, + blacklist_cooldown_secs: config.exploration.blacklist_cooldown_secs, + stuck_distance_threshold: config.exploration.stuck_distance_threshold, + stuck_time_window_secs: config.exploration.stuck_time_window_secs, + recovery_backup_distance: config.exploration.recovery_backup_distance, + recovery_rotation_rad: config.exploration.recovery_rotation_rad, + max_recovery_attempts: config.exploration.max_recovery_attempts, + ..Default::default() + }; + + Self { + shared_state, + shared_grid, + shared_debug_viz, + explorer: Explorer::new(explorer_config), + last_status_time: Instant::now(), + status_interval: Duration::from_secs(3), + started: false, + min_scans_to_start: 10, + } + } + + /// Run the exploration thread main loop. + pub fn run(&mut self) { + tracing::info!("Exploration thread started"); + + // Target ~20Hz for exploration updates + let loop_interval = Duration::from_millis(50); + + loop { + let loop_start = Instant::now(); + + // Check for shutdown + if self.shared_state.should_shutdown() { + tracing::info!("Exploration thread shutting down"); + break; + } + + // Check for safety stop - set velocity to zero + if self.shared_state.is_safety_stop() { + self.shared_state.set_velocity(0.0, 0.0); + std::thread::sleep(Duration::from_millis(100)); + continue; + } + + // Wait for enough scans before starting exploration + if !self.started { + if self.shared_state.scan_count() < self.min_scans_to_start { + std::thread::sleep(Duration::from_millis(100)); + continue; + } + // Start exploration + let current_pose = self.shared_state.pose(); + self.explorer.start(current_pose); + self.started = true; + tracing::info!( + "Exploration starting after {} scans", + self.min_scans_to_start + ); + } + + // Get current pose + let current_pose = self.shared_state.pose(); + + // Read grid snapshot + let grid = match self.shared_grid.read() { + Ok(g) => g.clone(), // Clone to release lock quickly + Err(e) => { + tracing::error!("Failed to read grid: {}", e); + std::thread::sleep(loop_interval); + continue; + } + }; + + // Update exploration + let step = self.explorer.update(&grid, current_pose); + + // Update shared state + self.shared_state + .set_velocity(step.velocity.0, step.velocity.1); + self.shared_state + .set_frontiers_remaining(step.frontiers_remaining); + + // Update debug visualization with frontier and waypoint data + self.update_debug_viz(&step); + + // Check if exploration is complete + if self.explorer.is_complete() { + match step.state { + ExplorationState::Complete => { + tracing::info!("Exploration complete!"); + } + ExplorationState::Failed => { + tracing::warn!("Exploration failed: no reachable frontiers"); + } + _ => {} + } + self.shared_state.set_velocity(0.0, 0.0); + self.shared_state.set_exploration_complete(); + break; + } + + // Log status periodically + if self.last_status_time.elapsed() >= self.status_interval { + self.log_status(&step, current_pose); + self.last_status_time = Instant::now(); + } + + // Maintain target loop rate + let elapsed = loop_start.elapsed(); + if elapsed < loop_interval { + std::thread::sleep(loop_interval - elapsed); + } + } + + tracing::info!("Exploration thread exited"); + } + + /// Log exploration status. + fn log_status(&self, step: &crate::exploration::ExplorationStep, pose: vastu_slam::Pose2D) { + let scans = self.shared_state.scan_count(); + let state_str = match step.state { + ExplorationState::Idle => "Idle", + ExplorationState::SelectingFrontier => "Selecting frontier", + ExplorationState::Planning => "Planning path", + ExplorationState::Navigating => "Navigating", + ExplorationState::Replanning => "Replanning", + ExplorationState::Recovering => "Recovering", + ExplorationState::EscapingToRetry => "Escaping to retry", + ExplorationState::ReturningHome => "Returning home", + ExplorationState::Complete => "Complete", + ExplorationState::Failed => "Failed", + }; + + tracing::info!( + "Exploring: state={}, frontiers={}, pose=({:.2}, {:.2}, {:.1}°), scans={}", + state_str, + step.frontiers_remaining, + pose.x, + pose.y, + pose.theta.to_degrees(), + scans + ); + + if let Some(target) = step.target_frontier { + tracing::debug!("Target frontier: ({:.2}, {:.2})", target.x, target.y); + } + } + + /// Update debug visualization data for SVG output. + fn update_debug_viz(&self, step: &crate::exploration::ExplorationStep) { + // Record frontier targets when navigating + if let Some(target) = step.target_frontier + && let Ok(mut debug_viz) = self.shared_debug_viz.write() + { + // Add target frontier if not already recorded + let target_point = vastu_slam::WorldPoint::new(target.x, target.y); + if !debug_viz + .visited_frontiers + .iter() + .any(|p| (p.x - target_point.x).abs() < 0.1 && (p.y - target_point.y).abs() < 0.1) + { + debug_viz.visited_frontiers.push(target_point); + } + } + + // Record waypoints from the current path + if let Some(path) = self.explorer.current_path() + && let Ok(mut debug_viz) = self.shared_debug_viz.write() + { + for segment in &path.segments { + let start = segment.start_point(); + let end = segment.end_point(); + + // Add start point if not already recorded (avoiding duplicates) + let start_wp = vastu_slam::WorldPoint::new(start.x, start.y); + if !debug_viz + .waypoints + .iter() + .any(|p| (p.x - start_wp.x).abs() < 0.05 && (p.y - start_wp.y).abs() < 0.05) + { + debug_viz.waypoints.push(start_wp); + } + + // Add end point + let end_wp = vastu_slam::WorldPoint::new(end.x, end.y); + if !debug_viz + .waypoints + .iter() + .any(|p| (p.x - end_wp.x).abs() < 0.05 && (p.y - end_wp.y).abs() < 0.05) + { + debug_viz.waypoints.push(end_wp); + } + } + } + } +} diff --git a/dhruva-nav/src/threads/mapping.rs b/dhruva-nav/src/threads/mapping.rs new file mode 100644 index 0000000..0788119 --- /dev/null +++ b/dhruva-nav/src/threads/mapping.rs @@ -0,0 +1,174 @@ +//! Mapping thread: Scan matching and map updates. +//! +//! This thread receives lidar scans from the sensor thread and: +//! - Performs scan matching for pose correction +//! - Updates the occupancy grid map +//! - Copies the grid to shared storage for exploration + +use std::sync::Arc; +use std::sync::mpsc::Receiver; +use std::time::{Duration, Instant}; + +use vastu_slam::matching::CorrelativeMatcherConfig; +use vastu_slam::{LidarScan, MapConfig, OccupancyGridMap, Pose2D}; + +use crate::shared::{SharedGrid, SharedState, SharedTrajectory, messages::LidarScanMsg}; + +/// Mapping thread state and logic. +pub struct MappingThread { + shared_state: Arc, + shared_grid: SharedGrid, + shared_trajectory: SharedTrajectory, + lidar_rx: Receiver, + map: OccupancyGridMap, + matcher_config: CorrelativeMatcherConfig, + trajectory: Vec, + scan_count: usize, + min_match_score: f32, + last_grid_sync: Instant, + grid_sync_interval: Duration, +} + +impl MappingThread { + /// Create a new mapping thread. + pub fn new( + shared_state: Arc, + shared_grid: SharedGrid, + shared_trajectory: SharedTrajectory, + lidar_rx: Receiver, + ) -> Self { + // Initialize map with default configuration + let map_config = MapConfig::default(); + + // Use a balanced matcher config: better accuracy than fast() but still real-time + // The fast() config (3cm resolution, 180 points) causes map smearing + let matcher_config = CorrelativeMatcherConfig { + search_x: 0.20, // 20cm search window + search_y: 0.20, // 20cm search window + search_theta: 0.20, // ~11 degrees + linear_resolution: 0.02, // 2cm steps (finer than fast's 3cm) + angular_resolution: 0.02, // ~1.1 degrees (finer than fast's 3cm) + max_points: 0, // Use all points for better accuracy + // Note: sensor_offset is (0,0) - SangamIO now sends lidar data + // already transformed to robot center coordinates + sensor_offset: (0.0, 0.0), + ..CorrelativeMatcherConfig::default() + }; + + Self { + shared_state, + shared_grid, + shared_trajectory, + lidar_rx, + map: OccupancyGridMap::new(map_config), + matcher_config, + trajectory: Vec::new(), + scan_count: 0, + min_match_score: 0.55, // Slightly higher threshold + last_grid_sync: Instant::now(), + grid_sync_interval: Duration::from_millis(100), // Sync grid every 100ms + } + } + + /// Run the mapping thread main loop. + pub fn run(&mut self) { + tracing::info!("Mapping thread started"); + + loop { + // Check for shutdown + if self.shared_state.should_shutdown() { + tracing::info!("Mapping thread shutting down"); + // Final grid sync before shutdown + self.sync_grid(); + break; + } + + // Receive lidar scan with timeout + match self.lidar_rx.recv_timeout(Duration::from_millis(100)) { + Ok(scan_msg) => { + self.process_scan(scan_msg); + } + Err(std::sync::mpsc::RecvTimeoutError::Timeout) => { + // No scan available, sync grid if needed + if self.last_grid_sync.elapsed() >= self.grid_sync_interval { + self.sync_grid(); + } + continue; + } + Err(std::sync::mpsc::RecvTimeoutError::Disconnected) => { + tracing::warn!("Lidar channel disconnected, mapping thread exiting"); + break; + } + } + + // Sync grid periodically + if self.last_grid_sync.elapsed() >= self.grid_sync_interval { + self.sync_grid(); + } + } + } + + /// Process a lidar scan. + fn process_scan(&mut self, scan_msg: LidarScanMsg) { + let scan = to_lidar_scan(&scan_msg.points); + let encoder_pose = scan_msg.encoder_pose; + + self.scan_count += 1; + self.shared_state.increment_scan_count(); + + // For the first few scans, seed the map without matching + let corrected_pose = if self.scan_count <= 5 { + self.map.observe_lidar(&scan, encoder_pose); + encoder_pose + } else { + // Use scan matching for pose correction + let (_result, corrected, match_result) = + self.map + .observe_lidar_with_matching(&scan, encoder_pose, &self.matcher_config); + + if match_result.converged && match_result.score >= self.min_match_score { + tracing::trace!( + "Scan match accepted: score={:.3}, pose=({:.3}, {:.3})", + match_result.score, + corrected.x, + corrected.y + ); + corrected + } else { + tracing::debug!( + "Scan match rejected: score={:.3}, converged={}", + match_result.score, + match_result.converged + ); + encoder_pose + } + }; + + // Apply correction: store corrected pose and the odometry at correction time + // This allows pose() to apply odometry deltas between corrections + self.shared_state + .apply_correction(corrected_pose, encoder_pose); + self.trajectory.push(corrected_pose); + + // Push to shared trajectory for SVG visualization + if let Ok(mut traj) = self.shared_trajectory.write() { + traj.push(corrected_pose); + } + } + + /// Sync the internal grid to shared storage. + fn sync_grid(&mut self) { + // Clone the grid storage and update shared state + if let Ok(mut grid) = self.shared_grid.write() { + *grid = self.map.storage().clone(); + } + self.last_grid_sync = Instant::now(); + } +} + +/// Convert lidar points to VastuSLAM LidarScan. +fn to_lidar_scan(points: &[(f32, f32, u8)]) -> LidarScan { + let angles: Vec = points.iter().map(|(a, _, _)| *a).collect(); + let ranges: Vec = points.iter().map(|(_, d, _)| *d).collect(); + LidarScan::new(ranges, angles, 0.15, 8.0) +} diff --git a/dhruva-nav/src/threads/mod.rs b/dhruva-nav/src/threads/mod.rs new file mode 100644 index 0000000..8c88ad9 --- /dev/null +++ b/dhruva-nav/src/threads/mod.rs @@ -0,0 +1,110 @@ +//! Multi-threaded architecture for DhruvaNav. +//! +//! Separates concerns into three threads: +//! - Sensor thread: High-frequency UDP reading, odometry, safety, commands +//! - Mapping thread: Scan matching and map updates +//! - Exploration thread: Frontier detection and path planning + +mod exploration; +mod mapping; +mod sensor; + +pub use exploration::ExplorationThread; +pub use mapping::MappingThread; +pub use sensor::SensorThread; + +use std::sync::Arc; +use std::sync::mpsc; +use std::thread::{self, JoinHandle}; + +use crate::config::DhruvaConfig; +use crate::error::Result; +use crate::shared::{ + DebugVisualization, SharedDebugViz, SharedGrid, SharedState, SharedTrajectory, + messages::LidarScanMsg, +}; + +/// Thread handles for the multi-threaded system. +pub struct ThreadHandles { + pub sensor: JoinHandle<()>, + pub mapping: JoinHandle<()>, + pub exploration: JoinHandle<()>, + /// Shared trajectory for SVG visualization (accumulated by mapping thread) + pub trajectory: SharedTrajectory, + /// Shared debug visualization data (accumulated by exploration thread) + pub debug_viz: SharedDebugViz, +} + +/// Spawn all threads and return handles. +pub fn spawn_threads( + config: DhruvaConfig, + shared_state: Arc, + shared_grid: SharedGrid, +) -> Result { + // Create channel for lidar scans (bounded to prevent memory growth) + let (lidar_tx, lidar_rx) = mpsc::sync_channel::(10); + + // Create shared trajectory for SVG visualization + let shared_trajectory: SharedTrajectory = Arc::new(std::sync::RwLock::new(Vec::new())); + let mapping_trajectory = Arc::clone(&shared_trajectory); + + // Create shared debug visualization data + let shared_debug_viz: SharedDebugViz = + Arc::new(std::sync::RwLock::new(DebugVisualization::default())); + let exploration_debug_viz = Arc::clone(&shared_debug_viz); + + // Clone shared state for each thread + let sensor_state = Arc::clone(&shared_state); + let mapping_state = Arc::clone(&shared_state); + let exploration_state = Arc::clone(&shared_state); + + // Clone shared grid for exploration + let exploration_grid = Arc::clone(&shared_grid); + + // Clone config parts for each thread + let sensor_config = config.clone(); + let exploration_config = config.clone(); + + // Spawn sensor thread (highest priority - real-time constraints) + let sensor_handle = thread::Builder::new() + .name("sensor".into()) + .spawn(move || { + let mut sensor_thread = SensorThread::new(sensor_config, sensor_state, lidar_tx); + if let Err(e) = sensor_thread.run() { + tracing::error!("Sensor thread error: {}", e); + } + }) + .expect("Failed to spawn sensor thread"); + + // Spawn mapping thread + let mapping_handle = thread::Builder::new() + .name("mapping".into()) + .spawn(move || { + let mut mapping_thread = + MappingThread::new(mapping_state, shared_grid, mapping_trajectory, lidar_rx); + mapping_thread.run(); + }) + .expect("Failed to spawn mapping thread"); + + // Spawn exploration thread + let exploration_handle = thread::Builder::new() + .name("exploration".into()) + .spawn(move || { + let mut exploration_thread = ExplorationThread::new( + exploration_config, + exploration_state, + exploration_grid, + exploration_debug_viz, + ); + exploration_thread.run(); + }) + .expect("Failed to spawn exploration thread"); + + Ok(ThreadHandles { + sensor: sensor_handle, + mapping: mapping_handle, + exploration: exploration_handle, + trajectory: shared_trajectory, + debug_viz: shared_debug_viz, + }) +} diff --git a/dhruva-nav/src/threads/sensor.rs b/dhruva-nav/src/threads/sensor.rs new file mode 100644 index 0000000..7ddca7f --- /dev/null +++ b/dhruva-nav/src/threads/sensor.rs @@ -0,0 +1,240 @@ +//! Sensor thread: High-frequency UDP reading, odometry, safety, and command sending. +//! +//! This thread runs at ~100Hz and handles: +//! - Reading UDP sensor data (non-blocking) +//! - Updating odometry from wheel encoders +//! - Checking safety sensors (bumpers, cliffs) +//! - Sending lidar scans to mapping thread +//! - Sending velocity commands to robot + +use std::sync::Arc; +use std::sync::mpsc::SyncSender; +use std::time::{Duration, Instant}; + +use crate::client::SangamClient; +use crate::config::DhruvaConfig; +use crate::error::Result; +use crate::odometry::Odometry; +use crate::shared::{SharedState, messages::LidarScanMsg}; + +use vastu_slam::Pose2D; + +/// Sensor thread state and logic. +pub struct SensorThread { + config: DhruvaConfig, + shared_state: Arc, + lidar_tx: SyncSender, + client: Option, + odometry: Odometry, + /// Last encoder pose - cached for lidar synchronization. + /// Encoder data arrives at ~110Hz, lidar at ~5Hz. + /// When a lidar scan arrives, we use this cached pose (from the most recent + /// encoder update) instead of the current pose to avoid synchronization drift. + last_encoder_pose: Pose2D, +} + +impl SensorThread { + /// Create a new sensor thread. + pub fn new( + config: DhruvaConfig, + shared_state: Arc, + lidar_tx: SyncSender, + ) -> Self { + // Robot starts at origin - odometry accumulates from (0,0,0) + let start_pose = Pose2D::new(0.0, 0.0, 0.0); + + let odometry = Odometry::new( + config.robot.wheel_base, + config.robot.ticks_per_meter, + start_pose, + ); + + Self { + config, + shared_state, + lidar_tx, + client: None, + odometry, + last_encoder_pose: start_pose, + } + } + + /// Run the sensor thread main loop. + pub fn run(&mut self) -> Result<()> { + // Connect to SangamIO + self.connect()?; + + let command_interval = Duration::from_millis(50); // 20Hz command rate + let mut last_command_time = Instant::now(); + + tracing::info!("Sensor thread started"); + + // Main sensor loop + loop { + // Check for shutdown + if self.shared_state.should_shutdown() { + tracing::info!("Sensor thread shutting down"); + self.send_stop(); + break; + } + + // Check for safety stop + if self.shared_state.is_safety_stop() { + self.send_stop(); + // Continue running to maintain connection but don't process + std::thread::sleep(Duration::from_millis(100)); + continue; + } + + // Drain UDP buffer + self.process_udp_messages(); + + // Send velocity command at fixed interval + if last_command_time.elapsed() >= command_interval { + self.send_velocity_command(); + last_command_time = Instant::now(); + } + + // Small sleep to avoid busy-waiting (target ~100Hz) + std::thread::sleep(Duration::from_millis(5)); + } + + Ok(()) + } + + /// Connect to SangamIO daemon. + fn connect(&mut self) -> Result<()> { + let addr = self.config.address(); + tracing::info!("Sensor thread connecting to SangamIO at {}...", addr); + + let timeout = Duration::from_millis(self.config.connection.timeout_ms); + let mut client = SangamClient::connect_timeout(&addr, timeout)?; + + // Set non-blocking for UDP reads + client.set_timeout(Some(Duration::from_millis(10)))?; + + // Enable drive and lidar components + tracing::info!("Enabling drive and lidar..."); + client.enable_drive()?; + client.enable_lidar()?; + + // Wait for sensor data to start flowing + std::thread::sleep(Duration::from_millis(500)); + + self.client = Some(client); + tracing::info!("Sensor thread connected to SangamIO"); + + Ok(()) + } + + /// Process all available UDP messages. + fn process_udp_messages(&mut self) { + let client = match self.client.as_mut() { + Some(c) => c, + None => return, + }; + + let mut messages_processed = 0; + const MAX_MESSAGES_PER_ITERATION: usize = 50; + + // Buffer for the latest lidar scan + let mut pending_lidar: Option = None; + + loop { + match client.recv_udp() { + Ok(Some(msg)) => { + messages_processed += 1; + + // Update odometry from encoders + if let Some((left, right)) = msg.encoder_ticks() { + self.odometry.update(left, right); + // Cache pose for lidar synchronization + self.last_encoder_pose = self.odometry.pose(); + // Update shared odometry (mapping thread will apply corrections) + self.shared_state.set_odometry(self.last_encoder_pose); + } + + // Check safety sensors - bumpers + if let Some((bump_l, bump_r)) = msg.bumper_state() + && (bump_l || bump_r) + { + self.shared_state.trigger_safety_stop(format!( + "Bumper triggered: left={}, right={}", + bump_l, bump_r + )); + return; + } + + // Check safety sensors - cliffs + if let Some((cl_ls, cl_lf, cl_rf, cl_rs)) = msg.cliff_state() + && (cl_ls || cl_lf || cl_rf || cl_rs) + { + self.shared_state.trigger_safety_stop(format!( + "Cliff detected: ls={}, lf={}, rf={}, rs={}", + cl_ls, cl_lf, cl_rf, cl_rs + )); + return; + } + + // Buffer lidar scan (keep only the latest) + // Use cached encoder pose for synchronization - encoder data arrives + // at ~110Hz, lidar at ~5Hz, so last_encoder_pose is at most ~9ms old + if let Some(points) = msg.as_lidar() { + pending_lidar = Some(LidarScanMsg { + points: points.clone(), + encoder_pose: self.last_encoder_pose, + }); + } + + // Limit messages per iteration + if messages_processed >= MAX_MESSAGES_PER_ITERATION { + break; + } + } + Ok(None) => { + // No more data available + break; + } + Err(e) => { + tracing::error!("UDP receive error: {}", e); + self.shared_state + .trigger_safety_stop(format!("Connection error: {}", e)); + return; + } + } + } + + // Send the latest lidar scan to mapping thread + if let Some(scan_msg) = pending_lidar { + // Use try_send to avoid blocking if channel is full + if let Err(e) = self.lidar_tx.try_send(scan_msg) { + tracing::warn!("Failed to send lidar scan to mapping thread: {}", e); + } + } + } + + /// Send velocity command to robot. + fn send_velocity_command(&mut self) { + let client = match self.client.as_mut() { + Some(c) => c, + None => return, + }; + + let (linear, angular) = self.shared_state.velocity(); + + if let Err(e) = client.send_drive_command(linear, angular) { + tracing::error!("Failed to send drive command: {}", e); + self.shared_state + .trigger_safety_stop(format!("Command send failed: {}", e)); + } + } + + /// Send stop command. + fn send_stop(&mut self) { + if let Some(client) = self.client.as_mut() + && let Err(e) = client.send_stop() + { + tracing::error!("Failed to send stop command: {}", e); + } + } +} diff --git a/dhruva-nav/src/utils.rs b/dhruva-nav/src/utils.rs new file mode 100644 index 0000000..d1ba95c --- /dev/null +++ b/dhruva-nav/src/utils.rs @@ -0,0 +1,16 @@ +//! Shared utility functions + +use std::f32::consts::PI; + +/// Normalize angle to [-π, π] +#[inline] +pub fn normalize_angle(angle: f32) -> f32 { + let mut a = angle; + while a > PI { + a -= 2.0 * PI; + } + while a < -PI { + a += 2.0 * PI; + } + a +}