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
This commit is contained in:
codetiger
@@ -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
|
||||
@@ -0,0 +1,11 @@
|
||||
//! Build script for compiling protobuf definitions
|
||||
|
||||
fn main() -> Result<(), Box<dyn std::error::Error>> {
|
||||
// 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(())
|
||||
}
|
||||
@@ -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"
|
||||
File diff suppressed because it is too large
Load Diff
Symlink
+1
@@ -0,0 +1 @@
|
||||
../../sangam-io/proto/sangamio.proto
|
||||
@@ -0,0 +1,2 @@
|
||||
[toolchain]
|
||||
channel = "nightly"
|
||||
@@ -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<LidarPoint>),
|
||||
}
|
||||
|
||||
/// 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<String, SensorValue>,
|
||||
}
|
||||
|
||||
impl SensorMessage {
|
||||
/// Create from protobuf message
|
||||
fn from_proto(msg: proto::sangamio::Message) -> Result<Self> {
|
||||
match msg.payload {
|
||||
Some(proto::sangamio::message::Payload::SensorGroup(sg)) => {
|
||||
let values: HashMap<String, SensorValue> = 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<LidarPoint>> {
|
||||
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<u8>,
|
||||
}
|
||||
|
||||
impl SangamClient {
|
||||
/// Connect with timeout (uses same port for TCP commands and UDP sensor receiving)
|
||||
pub fn connect_timeout(addr: &str, timeout: Duration) -> Result<Self> {
|
||||
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<Self> {
|
||||
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<Duration>) -> 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<Option<SensorMessage>> {
|
||||
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(())
|
||||
}
|
||||
}
|
||||
@@ -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<Self> {
|
||||
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)
|
||||
}
|
||||
}
|
||||
@@ -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<prost::DecodeError> for DhruvaError {
|
||||
fn from(e: prost::DecodeError) -> Self {
|
||||
DhruvaError::Protocol(e.to_string())
|
||||
}
|
||||
}
|
||||
|
||||
impl From<toml::de::Error> for DhruvaError {
|
||||
fn from(e: toml::de::Error) -> Self {
|
||||
DhruvaError::Config(e.to_string())
|
||||
}
|
||||
}
|
||||
|
||||
pub type Result<T> = std::result::Result<T, DhruvaError>;
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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<SmoothedPath>,
|
||||
/// 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<WorldPoint> {
|
||||
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());
|
||||
}
|
||||
}
|
||||
@@ -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<Frontier> {
|
||||
// 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<Frontier> = 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<GridCoord> {
|
||||
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<Vec<GridCoord>> {
|
||||
if frontier_cells.is_empty() {
|
||||
return Vec::new();
|
||||
}
|
||||
|
||||
// Create set for fast lookup
|
||||
let frontier_set: HashSet<GridCoord> = frontier_cells.iter().copied().collect();
|
||||
let mut visited: HashSet<GridCoord> = 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<GridCoord>,
|
||||
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<WorldPoint> {
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
@@ -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<String> = 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(())
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
@@ -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<u8>,
|
||||
/// Distance to nearest obstacle (in cells) for each cell
|
||||
distance_field: Vec<f32>,
|
||||
/// 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<GridCoord> {
|
||||
// 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<WorldPoint> {
|
||||
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)));
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
@@ -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<PathSegment>,
|
||||
/// 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);
|
||||
}
|
||||
}
|
||||
@@ -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<WorldPoint>,
|
||||
/// 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<Ordering> {
|
||||
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<PlannedPath> {
|
||||
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<WorldPoint> = 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<Vec<GridCoord>> {
|
||||
let mut open_set = BinaryHeap::new();
|
||||
let mut g_score: HashMap<GridCoord, f32> = HashMap::new();
|
||||
let mut parent: HashMap<GridCoord, GridCoord> = HashMap::new();
|
||||
let mut closed_set: HashMap<GridCoord, bool> = 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<GridCoord, GridCoord>,
|
||||
goal: GridCoord,
|
||||
) -> Vec<GridCoord> {
|
||||
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<GridCoord> {
|
||||
// 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());
|
||||
}
|
||||
}
|
||||
@@ -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<Option<String>>,
|
||||
|
||||
/// 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<String> {
|
||||
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<RwLock<GridStorage>>;
|
||||
|
||||
/// Thread-safe trajectory storage for SVG visualization.
|
||||
pub type SharedTrajectory = Arc<RwLock<Vec<Pose2D>>>;
|
||||
|
||||
/// Debug visualization data for SVG output.
|
||||
#[derive(Clone, Debug, Default)]
|
||||
pub struct DebugVisualization {
|
||||
/// Frontier centroids that were visited
|
||||
pub visited_frontiers: Vec<vastu_slam::WorldPoint>,
|
||||
/// Waypoints from planned paths
|
||||
pub waypoints: Vec<vastu_slam::WorldPoint>,
|
||||
}
|
||||
|
||||
/// Thread-safe debug visualization data.
|
||||
pub type SharedDebugViz = Arc<RwLock<DebugVisualization>>;
|
||||
|
||||
/// 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,
|
||||
}
|
||||
}
|
||||
@@ -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<SharedState>,
|
||||
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<SharedState>,
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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<SharedState>,
|
||||
shared_grid: SharedGrid,
|
||||
shared_trajectory: SharedTrajectory,
|
||||
lidar_rx: Receiver<LidarScanMsg>,
|
||||
map: OccupancyGridMap,
|
||||
matcher_config: CorrelativeMatcherConfig,
|
||||
trajectory: Vec<Pose2D>,
|
||||
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<SharedState>,
|
||||
shared_grid: SharedGrid,
|
||||
shared_trajectory: SharedTrajectory,
|
||||
lidar_rx: Receiver<LidarScanMsg>,
|
||||
) -> 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<f32> = points.iter().map(|(a, _, _)| *a).collect();
|
||||
let ranges: Vec<f32> = points.iter().map(|(_, d, _)| *d).collect();
|
||||
LidarScan::new(ranges, angles, 0.15, 8.0)
|
||||
}
|
||||
@@ -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<SharedState>,
|
||||
shared_grid: SharedGrid,
|
||||
) -> Result<ThreadHandles> {
|
||||
// Create channel for lidar scans (bounded to prevent memory growth)
|
||||
let (lidar_tx, lidar_rx) = mpsc::sync_channel::<LidarScanMsg>(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,
|
||||
})
|
||||
}
|
||||
@@ -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<SharedState>,
|
||||
lidar_tx: SyncSender<LidarScanMsg>,
|
||||
client: Option<SangamClient>,
|
||||
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<SharedState>,
|
||||
lidar_tx: SyncSender<LidarScanMsg>,
|
||||
) -> 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<LidarScanMsg> = 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
Reference in New Issue
Block a user