feat(dhruva-slam): establish validated M6 Pro SLAM baseline
This commit is contained in:
Julien Denizot
@@ -175,25 +175,28 @@ impl AStarPlanner {
|
||||
.world_to_cell(goal.0, goal.1)
|
||||
.ok_or(PlanningError::GoalOutOfBounds)?;
|
||||
|
||||
// Check if start is free
|
||||
if !self.is_cell_free(start_cell) {
|
||||
// Try to find nearest free cell to start
|
||||
if let Some(free_start) = self.find_nearest_free(start_cell, 20) {
|
||||
return self.plan_cells(map, free_start, goal_cell, target_id);
|
||||
}
|
||||
return Err(PlanningError::StartInObstacle);
|
||||
}
|
||||
// Resolve start and goal independently.
|
||||
//
|
||||
// The robot may temporarily be inside an inflated obstacle after a bumper
|
||||
// update. Likewise, exploration targets are often frontier cells, which are
|
||||
// unknown and therefore intentionally non-traversable.
|
||||
//
|
||||
// Both endpoints must be projected onto traversable cells before running A*.
|
||||
let actual_start = if self.is_cell_free(start_cell) {
|
||||
start_cell
|
||||
} else {
|
||||
self.find_nearest_free(start_cell, 20)
|
||||
.ok_or(PlanningError::StartInObstacle)?
|
||||
};
|
||||
|
||||
// Check if goal is free
|
||||
let actual_goal = if self.is_cell_free(goal_cell) {
|
||||
goal_cell
|
||||
} else {
|
||||
// Find nearest free cell to goal
|
||||
self.find_nearest_free(goal_cell, 50)
|
||||
.ok_or(PlanningError::GoalInObstacle)?
|
||||
};
|
||||
|
||||
self.plan_cells(map, start_cell, actual_goal, target_id)
|
||||
self.plan_cells(map, actual_start, actual_goal, target_id)
|
||||
}
|
||||
|
||||
/// Plan path between grid cells.
|
||||
@@ -227,7 +230,6 @@ impl AStarPlanner {
|
||||
if iterations > self.config.max_iterations {
|
||||
return Err(PlanningError::MaxIterationsExceeded);
|
||||
}
|
||||
|
||||
if current.cell == goal {
|
||||
// Reconstruct and return path
|
||||
let path = self.reconstruct_path(&came_from, start, goal, target_id);
|
||||
@@ -267,34 +269,46 @@ impl AStarPlanner {
|
||||
self.origin_x = map.origin_x;
|
||||
self.origin_y = map.origin_y;
|
||||
|
||||
// Calculate inflation in cells (robot_radius + safety_margin)
|
||||
// Physical obstacles are inflated by robot radius + safety margin.
|
||||
let total_inflation = self.config.robot_radius + self.config.safety_margin;
|
||||
self.inflation_cells = (total_inflation / map.resolution).ceil() as i32;
|
||||
|
||||
// Start with all cells free
|
||||
let total_cells = self.grid_width * self.grid_height;
|
||||
self.inflated_grid.clear();
|
||||
self.inflated_grid.resize(total_cells, true);
|
||||
|
||||
// Mark inflated obstacles
|
||||
// Pass 1:
|
||||
// Inflate ONLY real occupied cells.
|
||||
//
|
||||
// Unknown space must not be inflated: it is not a physical obstacle,
|
||||
// it is simply space that has not yet been proven traversable.
|
||||
for y in 0..self.grid_height {
|
||||
for x in 0..self.grid_width {
|
||||
let idx = y * self.grid_width + x;
|
||||
let cell_value = map.cells[idx];
|
||||
|
||||
// Check if obstacle or unknown (depending on config)
|
||||
// Note: occupied = 100, unknown = 255, free = 0
|
||||
// Cell is occupied if value >= 100 BUT NOT if it's unknown (255)
|
||||
let is_occupied = cell_value >= 100 && cell_value != 255;
|
||||
let is_unknown_obstacle = cell_value == 255 && self.config.unknown_is_obstacle;
|
||||
let is_obstacle = is_occupied || is_unknown_obstacle;
|
||||
|
||||
if is_obstacle {
|
||||
// Inflate: mark surrounding cells as blocked
|
||||
if is_occupied {
|
||||
self.inflate_cell(x as i32, y as i32);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Pass 2:
|
||||
// Unknown cells themselves are non-traversable when requested,
|
||||
// but they are NOT inflated like physical obstacles.
|
||||
if self.config.unknown_is_obstacle {
|
||||
for y in 0..self.grid_height {
|
||||
for x in 0..self.grid_width {
|
||||
let idx = y * self.grid_width + x;
|
||||
|
||||
if map.cells[idx] == 255 {
|
||||
self.inflated_grid[idx] = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Mark cells around an obstacle as blocked.
|
||||
|
||||
@@ -438,12 +438,26 @@ impl OnlineSlam {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Ensure global map exists
|
||||
let robot_pose = Pose2D::new(robot_x, robot_y, robot_theta);
|
||||
|
||||
// Persist the contact obstacle in the active submap first.
|
||||
//
|
||||
// Previously bumper obstacles only modified the cached global map.
|
||||
// A later global-map rebuild from submaps could therefore erase them,
|
||||
// allowing the planner to drive into the same physical obstacle again.
|
||||
let persisted =
|
||||
self.submaps
|
||||
.mark_bumper_obstacle(&robot_pose, bumper_left, bumper_right, robot_radius);
|
||||
|
||||
// Ensure global map exists. If this triggers a rebuild, the persisted
|
||||
// obstacle above is already part of the source submap.
|
||||
if self.global_map.is_none() {
|
||||
self.rebuild_global_map();
|
||||
}
|
||||
|
||||
if let Some(ref mut map) = self.global_map {
|
||||
let marked_global = if let Some(ref mut map) = self.global_map {
|
||||
// Also mark the current global map so navigation sees the obstacle
|
||||
// immediately, without waiting for a future rebuild.
|
||||
map.mark_bumper_obstacle(
|
||||
robot_x,
|
||||
robot_y,
|
||||
@@ -452,18 +466,23 @@ impl OnlineSlam {
|
||||
bumper_right,
|
||||
robot_radius,
|
||||
);
|
||||
log::debug!(
|
||||
"Marked bumper obstacle at ({:.2}, {:.2}) theta={:.2}rad, left={}, right={}",
|
||||
robot_x,
|
||||
robot_y,
|
||||
robot_theta,
|
||||
bumper_left,
|
||||
bumper_right
|
||||
);
|
||||
|
||||
true
|
||||
} else {
|
||||
false
|
||||
}
|
||||
};
|
||||
|
||||
log::debug!(
|
||||
"Marked bumper obstacle at ({:.2}, {:.2}) theta={:.2}rad, left={}, right={}, persisted={}",
|
||||
robot_x,
|
||||
robot_y,
|
||||
robot_theta,
|
||||
bumper_left,
|
||||
bumper_right,
|
||||
persisted
|
||||
);
|
||||
|
||||
persisted || marked_global
|
||||
}
|
||||
|
||||
/// Match scan to current submap.
|
||||
@@ -782,29 +801,92 @@ impl OnlineSlam {
|
||||
self.pending_closures.clear();
|
||||
}
|
||||
|
||||
/// Apply pose corrections from optimized graph to keyframes.
|
||||
/// Apply pose corrections from optimized graph to keyframes and submaps.
|
||||
///
|
||||
/// Keyframes are optimized individually by the pose graph, while each
|
||||
/// occupancy submap is a rigid local grid. For each submap we therefore
|
||||
/// use its earliest keyframe as an anchor and apply that keyframe's rigid
|
||||
/// world-frame correction to the submap origin.
|
||||
fn apply_pose_corrections(&mut self) {
|
||||
// Update keyframe poses from optimized graph
|
||||
use std::collections::HashMap;
|
||||
|
||||
// submap_id -> (earliest keyframe id, rigid correction)
|
||||
//
|
||||
// correction C is defined so that:
|
||||
//
|
||||
// new_pose = C ⊕ old_pose
|
||||
//
|
||||
// therefore:
|
||||
//
|
||||
// C = new_pose ⊕ old_pose^-1
|
||||
let mut submap_corrections: HashMap<u64, (u64, Pose2D)> = HashMap::new();
|
||||
|
||||
// First compute corrections while keyframes still contain their
|
||||
// pre-optimization poses, then update the keyframes themselves.
|
||||
for node in self.pose_graph.nodes() {
|
||||
if let Some(kf_id) = node.keyframe_id
|
||||
&& let Some(kf) = self.keyframes.get_mut(kf_id)
|
||||
{
|
||||
kf.pose = node.pose;
|
||||
let Some(kf_id) = node.keyframe_id else {
|
||||
continue;
|
||||
};
|
||||
|
||||
let Some(kf) = self.keyframes.get_mut(kf_id) else {
|
||||
continue;
|
||||
};
|
||||
|
||||
let old_pose = kf.pose;
|
||||
let new_pose = node.pose;
|
||||
let submap_id = kf.submap_id;
|
||||
|
||||
let correction = new_pose.compose(&old_pose.inverse());
|
||||
|
||||
match submap_corrections.get(&submap_id) {
|
||||
Some((anchor_id, _)) if *anchor_id <= kf_id => {}
|
||||
_ => {
|
||||
submap_corrections.insert(submap_id, (kf_id, correction));
|
||||
}
|
||||
}
|
||||
|
||||
kf.pose = new_pose;
|
||||
}
|
||||
|
||||
// A submap grid is expressed in local coordinates. Move its world
|
||||
// origin using the correction of the earliest keyframe belonging to
|
||||
// that submap.
|
||||
let mut corrected_submaps = 0usize;
|
||||
|
||||
for (submap_id, (anchor_id, correction)) in submap_corrections {
|
||||
if self.submaps.apply_origin_correction(submap_id, &correction) {
|
||||
corrected_submaps += 1;
|
||||
|
||||
log::debug!(
|
||||
"Corrected submap {} from anchor keyframe {}: delta=({:.3}, {:.3}, {:.2}°)",
|
||||
submap_id,
|
||||
anchor_id,
|
||||
correction.x,
|
||||
correction.y,
|
||||
correction.theta.to_degrees()
|
||||
);
|
||||
} else {
|
||||
log::warn!(
|
||||
"Pose graph correction references missing submap {}",
|
||||
submap_id
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
// Update current pose if we have a latest node
|
||||
// The latest pose-graph node corresponds to the newest keyframe.
|
||||
if let Some(latest) = self.pose_graph.latest_node() {
|
||||
self.current_pose = latest.pose;
|
||||
self.prev_keyframe_pose = Some(latest.pose);
|
||||
}
|
||||
|
||||
// Invalidate global map cache (will be rebuilt with corrected poses)
|
||||
// Global occupancy map must be rebuilt using the corrected submap
|
||||
// origins before navigation sees the new geometry.
|
||||
self.global_map = None;
|
||||
|
||||
log::debug!(
|
||||
"Applied pose corrections to {} keyframes",
|
||||
self.keyframes.len()
|
||||
"Applied pose corrections to {} keyframes and {} submaps",
|
||||
self.keyframes.len(),
|
||||
corrected_submaps
|
||||
);
|
||||
}
|
||||
|
||||
|
||||
@@ -190,6 +190,40 @@ impl Submap {
|
||||
true
|
||||
}
|
||||
|
||||
/// Persist a bumper-detected obstacle into this submap.
|
||||
///
|
||||
/// Bumper events arrive in global/world coordinates, while the submap
|
||||
/// occupancy grid is expressed in submap-local coordinates. Convert the
|
||||
/// robot pose before marking so the obstacle follows the submap through
|
||||
/// future pose-graph corrections.
|
||||
pub fn mark_bumper_obstacle_global(
|
||||
&mut self,
|
||||
robot_pose: &Pose2D,
|
||||
bumper_left: bool,
|
||||
bumper_right: bool,
|
||||
robot_radius: f32,
|
||||
) -> bool {
|
||||
if self.state == SubmapState::Finished {
|
||||
return false;
|
||||
}
|
||||
|
||||
let local_pose = self.global_to_local(robot_pose);
|
||||
|
||||
self.grid.mark_bumper_obstacle(
|
||||
local_pose.x,
|
||||
local_pose.y,
|
||||
local_pose.theta,
|
||||
bumper_left,
|
||||
bumper_right,
|
||||
robot_radius,
|
||||
);
|
||||
|
||||
// The occupancy grid changed, so any cached occupied point cloud is stale.
|
||||
self.cached_pointcloud = None;
|
||||
|
||||
true
|
||||
}
|
||||
|
||||
/// Add a keyframe ID to this submap.
|
||||
pub fn add_keyframe(&mut self, keyframe_id: u64) {
|
||||
self.keyframe_ids.push(keyframe_id);
|
||||
@@ -305,6 +339,39 @@ impl SubmapManager {
|
||||
self.submaps.iter_mut().find(|s| s.id == id)
|
||||
}
|
||||
|
||||
/// Apply a rigid world-frame correction to a submap origin.
|
||||
///
|
||||
/// The occupancy grid itself is stored in submap-local coordinates, so
|
||||
/// pose-graph optimization must move the submap origin rather than rewrite
|
||||
/// every grid cell.
|
||||
pub fn apply_origin_correction(&mut self, id: u64, correction: &Pose2D) -> bool {
|
||||
if let Some(submap) = self.get_mut(id) {
|
||||
submap.origin = correction.compose(&submap.origin);
|
||||
true
|
||||
} else {
|
||||
false
|
||||
}
|
||||
}
|
||||
|
||||
/// Persist a bumper obstacle in the currently active submap.
|
||||
pub fn mark_bumper_obstacle(
|
||||
&mut self,
|
||||
robot_pose: &Pose2D,
|
||||
bumper_left: bool,
|
||||
bumper_right: bool,
|
||||
robot_radius: f32,
|
||||
) -> bool {
|
||||
match self.active_submap_mut() {
|
||||
Some(submap) => submap.mark_bumper_obstacle_global(
|
||||
robot_pose,
|
||||
bumper_left,
|
||||
bumper_right,
|
||||
robot_radius,
|
||||
),
|
||||
None => false,
|
||||
}
|
||||
}
|
||||
|
||||
/// Get all finished submaps.
|
||||
pub fn finished_submaps(&self) -> impl Iterator<Item = &Submap> {
|
||||
self.submaps.iter().filter(|s| s.is_finished())
|
||||
@@ -619,6 +686,52 @@ mod tests {
|
||||
assert_eq!(manager.finished_submaps().count(), 1);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_submap_origin_pose_graph_correction() {
|
||||
let config = SubmapManagerConfig::default();
|
||||
let mut manager = SubmapManager::new(config);
|
||||
|
||||
let scan = create_test_scan();
|
||||
|
||||
// Create one submap with a non-trivial world origin.
|
||||
let old_origin = Pose2D::new(1.0, 2.0, 0.30);
|
||||
manager.process_scan(&scan, &old_origin, 0);
|
||||
|
||||
// Simulate one anchor keyframe before and after pose-graph optimization.
|
||||
let old_anchor = Pose2D::new(1.40, 2.20, 0.45);
|
||||
let new_anchor = Pose2D::new(2.10, 1.70, 0.80);
|
||||
|
||||
// C is the rigid world-frame correction such that:
|
||||
// new_anchor = C ⊕ old_anchor
|
||||
let correction = new_anchor.compose(&old_anchor.inverse());
|
||||
|
||||
let reconstructed_anchor = correction.compose(&old_anchor);
|
||||
|
||||
approx::assert_relative_eq!(reconstructed_anchor.x, new_anchor.x, epsilon = 1e-5);
|
||||
approx::assert_relative_eq!(reconstructed_anchor.y, new_anchor.y, epsilon = 1e-5);
|
||||
approx::assert_relative_eq!(reconstructed_anchor.theta, new_anchor.theta, epsilon = 1e-5);
|
||||
|
||||
// The exact same rigid correction must be applied to the submap origin.
|
||||
let expected_origin = correction.compose(&old_origin);
|
||||
|
||||
assert!(manager.apply_origin_correction(0, &correction));
|
||||
|
||||
let corrected_origin = manager
|
||||
.submaps
|
||||
.iter()
|
||||
.find(|submap| submap.id == 0)
|
||||
.expect("submap 0 must exist")
|
||||
.origin;
|
||||
|
||||
approx::assert_relative_eq!(corrected_origin.x, expected_origin.x, epsilon = 1e-5);
|
||||
approx::assert_relative_eq!(corrected_origin.y, expected_origin.y, epsilon = 1e-5);
|
||||
approx::assert_relative_eq!(
|
||||
corrected_origin.theta,
|
||||
expected_origin.theta,
|
||||
epsilon = 1e-5
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_submap_manager_overlap() {
|
||||
let config = SubmapManagerConfig {
|
||||
|
||||
@@ -128,7 +128,7 @@ impl FrontierExploration {
|
||||
allow_diagonal: true,
|
||||
max_iterations: 50_000, // Lower limit for quick reachability check
|
||||
simplification_tolerance: 0.0, // Don't need simplified paths for reachability check
|
||||
unknown_is_obstacle: false, // Allow planning through unknown (we're exploring!)
|
||||
unknown_is_obstacle: true, // Exploration may target a frontier, but paths must stay in known-free space
|
||||
};
|
||||
|
||||
Self {
|
||||
@@ -157,16 +157,18 @@ impl FrontierExploration {
|
||||
let radius_sq = self.config.blocked_radius * self.config.blocked_radius;
|
||||
let now = std::time::Instant::now();
|
||||
|
||||
self.recently_visited.iter().any(|((vx, vy), visited_time)| {
|
||||
// Check if still in cooldown
|
||||
if now.duration_since(*visited_time).as_secs_f32() > self.visited_cooldown_secs {
|
||||
return false;
|
||||
}
|
||||
// Check if frontier is near this visited location
|
||||
let dx = frontier.centroid.x - vx;
|
||||
let dy = frontier.centroid.y - vy;
|
||||
(dx * dx + dy * dy) < radius_sq
|
||||
})
|
||||
self.recently_visited
|
||||
.iter()
|
||||
.any(|((vx, vy), visited_time)| {
|
||||
// Check if still in cooldown
|
||||
if now.duration_since(*visited_time).as_secs_f32() > self.visited_cooldown_secs {
|
||||
return false;
|
||||
}
|
||||
// Check if frontier is near this visited location
|
||||
let dx = frontier.centroid.x - vx;
|
||||
let dy = frontier.centroid.y - vy;
|
||||
(dx * dx + dy * dy) < radius_sq
|
||||
})
|
||||
}
|
||||
|
||||
/// Clean up expired entries from recently_visited list.
|
||||
@@ -502,12 +504,44 @@ impl FrontierExploration {
|
||||
(sum_x / n, sum_y / n)
|
||||
}
|
||||
|
||||
/// Check whether a world position is inside a previously blocked zone.
|
||||
fn is_location_blocked(&self, x: f32, y: f32) -> bool {
|
||||
let radius_sq = self.config.blocked_radius * self.config.blocked_radius;
|
||||
|
||||
self.blocked_locations.iter().any(|(bx, by)| {
|
||||
let dx = x - bx;
|
||||
let dy = y - by;
|
||||
(dx * dx + dy * dy) < radius_sq
|
||||
})
|
||||
}
|
||||
|
||||
/// Remember a blocked world position without accumulating duplicates.
|
||||
fn remember_blocked_location(&mut self, x: f32, y: f32) {
|
||||
if self.is_location_blocked(x, y) {
|
||||
log::debug!(
|
||||
"Blocked location ({:.2}, {:.2}) already known - not adding duplicate",
|
||||
x,
|
||||
y
|
||||
);
|
||||
return;
|
||||
}
|
||||
|
||||
self.blocked_locations.push((x, y));
|
||||
|
||||
log::debug!(
|
||||
"Remembered blocked location ({:.2}, {:.2}), {} blocked total",
|
||||
x,
|
||||
y,
|
||||
self.blocked_locations.len()
|
||||
);
|
||||
}
|
||||
|
||||
/// Check if a frontier is blocked (bumper triggered nearby or navigation failed).
|
||||
fn is_frontier_blocked(&self, frontier: &Frontier) -> bool {
|
||||
// Use a larger radius for blocking (0.5m) to account for centroid drift
|
||||
// The config.blocked_radius is for bumper obstacles, but navigation failures
|
||||
// need more margin since centroids shift as map updates
|
||||
let blocked_radius = self.config.blocked_radius.max(0.5);
|
||||
let blocked_radius = self.config.blocked_radius;
|
||||
let radius_sq = blocked_radius * blocked_radius;
|
||||
|
||||
let is_blocked = self.blocked_locations.iter().any(|(bx, by)| {
|
||||
@@ -529,17 +563,54 @@ impl FrontierExploration {
|
||||
is_blocked
|
||||
}
|
||||
|
||||
/// Check if a specific point is reachable via A* path planning.
|
||||
fn is_point_reachable(
|
||||
/// Find the actual reachable A* endpoint for a frontier candidate.
|
||||
///
|
||||
/// AStarPlanner may move a blocked/unknown goal to the nearest traversable
|
||||
/// cell. Therefore Ok(Path) alone does NOT mean the requested frontier
|
||||
/// point is actually reachable.
|
||||
fn reachable_approach_point(
|
||||
&mut self,
|
||||
x: f32,
|
||||
y: f32,
|
||||
robot_pose: &Pose2D,
|
||||
map: &CurrentMapData,
|
||||
) -> bool {
|
||||
) -> Option<(f32, f32)> {
|
||||
let start = (robot_pose.x, robot_pose.y);
|
||||
let goal = (x, y);
|
||||
self.planner.plan(map, start, goal, 0).is_ok()
|
||||
|
||||
let path = self.planner.plan(map, start, goal, 0).ok()?;
|
||||
let endpoint = path.last()?;
|
||||
|
||||
let dx = endpoint.x - x;
|
||||
let dy = endpoint.y - y;
|
||||
let endpoint_distance = (dx * dx + dy * dy).sqrt();
|
||||
|
||||
// Keep this consistent with the Navigator frontier completion guard:
|
||||
//
|
||||
// robot radius 0.18m
|
||||
// safety margin + 0.10m
|
||||
// waypoint slack + 0.15m
|
||||
// ------------------------
|
||||
// max approach 0.43m
|
||||
//
|
||||
// The last 15cm corresponds to the current navigation waypoint
|
||||
// threshold. We can move this into shared configuration later.
|
||||
let max_approach_distance = self.config.robot_radius + self.config.safety_margin + 0.15;
|
||||
|
||||
if endpoint_distance > max_approach_distance {
|
||||
log::debug!(
|
||||
"Rejecting frontier candidate ({:.2}, {:.2}): A* endpoint ({:.2}, {:.2}) is {:.2}m away (max {:.2}m)",
|
||||
x,
|
||||
y,
|
||||
endpoint.x,
|
||||
endpoint.y,
|
||||
endpoint_distance,
|
||||
max_approach_distance
|
||||
);
|
||||
return None;
|
||||
}
|
||||
|
||||
Some((endpoint.x, endpoint.y))
|
||||
}
|
||||
|
||||
/// Score a frontier for selection priority.
|
||||
@@ -558,19 +629,41 @@ impl FrontierExploration {
|
||||
distance_score * 0.8 + size_score * 0.2
|
||||
}
|
||||
|
||||
/// Find a reachable point on a frontier (centroid first, then edge points).
|
||||
/// Find a reachable approach point for a frontier.
|
||||
///
|
||||
/// The returned point is the ACTUAL final A* endpoint in known free
|
||||
/// space, not necessarily the frontier cell itself.
|
||||
fn find_reachable_point_on_frontier(
|
||||
&mut self,
|
||||
frontier: &Frontier,
|
||||
robot_pose: &Pose2D,
|
||||
map: &CurrentMapData,
|
||||
) -> Option<(f32, f32)> {
|
||||
// 1. Try centroid first (most common case)
|
||||
if self.is_point_reachable(frontier.centroid.x, frontier.centroid.y, robot_pose, map) {
|
||||
return Some((frontier.centroid.x, frontier.centroid.y));
|
||||
// 1. Try centroid first.
|
||||
if !self.is_location_blocked(frontier.centroid.x, frontier.centroid.y) {
|
||||
if let Some(endpoint) = self.reachable_approach_point(
|
||||
frontier.centroid.x,
|
||||
frontier.centroid.y,
|
||||
robot_pose,
|
||||
map,
|
||||
) {
|
||||
if (endpoint.0 - frontier.centroid.x).abs() > 0.01
|
||||
|| (endpoint.1 - frontier.centroid.y).abs() > 0.01
|
||||
{
|
||||
log::debug!(
|
||||
"Frontier centroid ({:.2}, {:.2}) approached via reachable endpoint ({:.2}, {:.2})",
|
||||
frontier.centroid.x,
|
||||
frontier.centroid.y,
|
||||
endpoint.0,
|
||||
endpoint.1
|
||||
);
|
||||
}
|
||||
|
||||
return Some(endpoint);
|
||||
}
|
||||
}
|
||||
|
||||
// 2. Try edge points sorted by distance to robot
|
||||
// 2. Try frontier cells sorted by distance to the robot.
|
||||
let mut edge_points: Vec<(f32, f32, f32)> = frontier
|
||||
.cells
|
||||
.iter()
|
||||
@@ -583,17 +676,24 @@ impl FrontierExploration {
|
||||
|
||||
edge_points.sort_by(|a, b| a.2.partial_cmp(&b.2).unwrap_or(std::cmp::Ordering::Equal));
|
||||
|
||||
// Try up to 5 closest edge points
|
||||
for (x, y, _) in edge_points.iter().take(5) {
|
||||
if self.is_point_reachable(*x, *y, robot_pose, map) {
|
||||
if self.is_location_blocked(*x, *y) {
|
||||
log::debug!("Skipping blocked frontier edge point ({:.2}, {:.2})", x, y);
|
||||
continue;
|
||||
}
|
||||
|
||||
if let Some(endpoint) = self.reachable_approach_point(*x, *y, robot_pose, map) {
|
||||
log::debug!(
|
||||
"Using edge point ({:.2}, {:.2}) instead of centroid ({:.2}, {:.2})",
|
||||
"Using frontier edge ({:.2}, {:.2}) via reachable endpoint ({:.2}, {:.2}) instead of centroid ({:.2}, {:.2})",
|
||||
x,
|
||||
y,
|
||||
endpoint.0,
|
||||
endpoint.1,
|
||||
frontier.centroid.x,
|
||||
frontier.centroid.y
|
||||
);
|
||||
return Some((*x, *y));
|
||||
|
||||
return Some(endpoint);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -663,9 +763,11 @@ impl FrontierExploration {
|
||||
// Navigate to the free cell adjacent to this unknown, not the unknown itself
|
||||
if let Some((target_x, target_y)) =
|
||||
self.find_adjacent_free_cell(map, cx, cy, width, resolution)
|
||||
&& self.is_point_reachable(target_x, target_y, robot_pose, map)
|
||||
&& !self.is_location_blocked(target_x, target_y)
|
||||
&& let Some(endpoint) =
|
||||
self.reachable_approach_point(target_x, target_y, robot_pose, map)
|
||||
{
|
||||
return Some((target_x, target_y));
|
||||
return Some(endpoint);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1059,7 +1161,8 @@ impl ExplorationStrategy for FrontierExploration {
|
||||
x,
|
||||
y
|
||||
);
|
||||
self.blocked_locations.push((x, y));
|
||||
|
||||
self.remember_blocked_location(x, y);
|
||||
|
||||
// Clear current target since it failed
|
||||
self.current_target = None;
|
||||
@@ -1076,8 +1179,10 @@ impl ExplorationStrategy for FrontierExploration {
|
||||
|
||||
// Add to recently visited cooldown list to prevent immediate re-selection
|
||||
// This handles the case where we reached "close enough" but didn't fully clear the frontier
|
||||
self.recently_visited
|
||||
.push(((target.centroid.x, target.centroid.y), std::time::Instant::now()));
|
||||
self.recently_visited.push((
|
||||
(target.centroid.x, target.centroid.y),
|
||||
std::time::Instant::now(),
|
||||
));
|
||||
}
|
||||
|
||||
// Clear current target - navigation completed
|
||||
|
||||
@@ -497,8 +497,9 @@ impl MotionController {
|
||||
|
||||
impl Drop for MotionController {
|
||||
fn drop(&mut self) {
|
||||
// Ensure motors are stopped when controller is dropped
|
||||
// Ensure motors are stopped and lidar is shut down.
|
||||
self.disable().ok();
|
||||
self.disable_lidar().ok();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -1076,8 +1076,16 @@ fn run_threaded_mode(
|
||||
// Connect motion controller to SangamIO (will auto-reconnect on operations)
|
||||
{
|
||||
let mut mc = shared_motion.lock().unwrap();
|
||||
|
||||
match mc.connect(&config.source.sangam_address) {
|
||||
Ok(()) => log::info!(" Motion controller connected to SangamIO"),
|
||||
Ok(()) => {
|
||||
log::info!(" Motion controller connected to SangamIO");
|
||||
|
||||
match mc.enable_lidar() {
|
||||
Ok(()) => log::info!(" Lidar enabled for SLAM"),
|
||||
Err(e) => log::warn!(" Failed to enable lidar: {}", e),
|
||||
}
|
||||
}
|
||||
Err(e) => log::warn!(
|
||||
" Motion controller initial connect failed (will retry): {}",
|
||||
e
|
||||
|
||||
@@ -14,7 +14,8 @@ use crate::algorithms::planning::{AStarConfig, AStarPlanner, PlanningError};
|
||||
use crate::core::math::normalize_angle;
|
||||
use crate::core::types::Pose2D;
|
||||
use crate::navigation::{
|
||||
MovementDirection, NavState, NavTarget, NavigationState, PathFailureReason, Waypoint,
|
||||
MovementDirection, NavState, NavTarget, NavTargetType, NavigationState, PathFailureReason,
|
||||
Waypoint,
|
||||
};
|
||||
use crate::state::CurrentMapData;
|
||||
|
||||
@@ -402,22 +403,67 @@ impl Navigator {
|
||||
if dist_to_waypoint < self.config.waypoint_reached_threshold {
|
||||
// Advance to next waypoint
|
||||
if !nav_state.advance_waypoint() {
|
||||
// Was last waypoint, check goal
|
||||
// We reached the final waypoint produced by A*.
|
||||
//
|
||||
// For frontier exploration, this endpoint may intentionally
|
||||
// differ from the requested frontier because obstacle
|
||||
// inflation can move the reachable goal away from the
|
||||
// unknown-space boundary.
|
||||
if target.target_type == NavTargetType::Frontier {
|
||||
let goal_dx = target.position.x - pose.x;
|
||||
let goal_dy = target.position.y - pose.y;
|
||||
let goal_dist = (goal_dx * goal_dx + goal_dy * goal_dy).sqrt();
|
||||
|
||||
// A frontier itself is in unknown space and may therefore
|
||||
// be unreachable by the robot center. Accept the final A*
|
||||
// endpoint only if it remains geometrically close enough
|
||||
// to the requested frontier.
|
||||
let max_frontier_distance = self.config.planner.robot_radius
|
||||
+ self.config.planner.safety_margin
|
||||
+ self.config.waypoint_reached_threshold;
|
||||
|
||||
if goal_dist <= max_frontier_distance {
|
||||
log::info!(
|
||||
"Frontier path complete at reachable endpoint ({:.2}m from requested frontier, max {:.2}m)",
|
||||
goal_dist,
|
||||
max_frontier_distance
|
||||
);
|
||||
|
||||
return self.complete_target(nav_state);
|
||||
}
|
||||
|
||||
log::warn!(
|
||||
"Reachable endpoint too far from frontier ({:.2}m > {:.2}m) - frontier considered unreachable",
|
||||
goal_dist,
|
||||
max_frontier_distance
|
||||
);
|
||||
|
||||
nav_state.set_failed(PathFailureReason::TargetInvalid);
|
||||
nav_state.set_status(format!(
|
||||
"Frontier unreachable: endpoint {:.2}m away (max {:.2}m)",
|
||||
goal_dist, max_frontier_distance
|
||||
));
|
||||
|
||||
return NavUpdate::stop().with_state_change();
|
||||
}
|
||||
|
||||
// For normal targets, check whether the requested position
|
||||
// itself has been reached.
|
||||
if target.is_position_reached(pose.x, pose.y) {
|
||||
if target.heading.is_some() && !target.is_heading_reached(pose.theta) {
|
||||
nav_state.nav_state = NavState::RotatingToHeading;
|
||||
return NavUpdate::stop().with_state_change();
|
||||
}
|
||||
|
||||
return self.complete_target(nav_state);
|
||||
}
|
||||
|
||||
// Path is complete but not at exact goal position.
|
||||
// For frontiers/exploration, accept "close enough" to avoid getting stuck
|
||||
// trying to reach goals in obstacle inflation zones.
|
||||
// Use 1.5x waypoint threshold (typically ~22cm with default 15cm threshold).
|
||||
// Path is complete but the requested target is still some
|
||||
// distance away.
|
||||
let goal_dx = target.position.x - pose.x;
|
||||
let goal_dy = target.position.y - pose.y;
|
||||
let goal_dist = (goal_dx * goal_dx + goal_dy * goal_dy).sqrt();
|
||||
|
||||
let close_enough_threshold = self.config.waypoint_reached_threshold * 1.5;
|
||||
|
||||
if goal_dist < close_enough_threshold {
|
||||
@@ -425,21 +471,30 @@ impl Navigator {
|
||||
"Path complete, accepting position as 'close enough' ({:.2}m from goal)",
|
||||
goal_dist
|
||||
);
|
||||
|
||||
if target.heading.is_some() && !target.is_heading_reached(pose.theta) {
|
||||
nav_state.nav_state = NavState::RotatingToHeading;
|
||||
return NavUpdate::stop().with_state_change();
|
||||
}
|
||||
|
||||
return self.complete_target(nav_state);
|
||||
}
|
||||
|
||||
// Never continue driving toward an already-completed final
|
||||
// waypoint. Replan toward the actual target instead.
|
||||
log::info!(
|
||||
"Path endpoint reached but target remains {:.2}m away - requesting replan",
|
||||
goal_dist
|
||||
);
|
||||
|
||||
nav_state.request_replan();
|
||||
return NavUpdate::stop().with_state_change();
|
||||
}
|
||||
|
||||
// Get new current waypoint
|
||||
// We advanced successfully: continue toward the next waypoint.
|
||||
let waypoint = match nav_state.current_waypoint() {
|
||||
Some(w) => *w,
|
||||
None => {
|
||||
// Edge case: should go to goal
|
||||
Waypoint::new(target.position.x, target.position.y)
|
||||
}
|
||||
None => Waypoint::new(target.position.x, target.position.y),
|
||||
};
|
||||
|
||||
return self.compute_velocity_to_waypoint(pose, &waypoint, &target);
|
||||
|
||||
@@ -369,6 +369,24 @@ impl NavigationState {
|
||||
self.nav_state = NavState::Planning;
|
||||
}
|
||||
|
||||
/// Request a fresh path after a SLAM pose-graph correction.
|
||||
///
|
||||
/// This is not a navigation failure, so it must not consume one of the
|
||||
/// obstacle/stuck replan attempts.
|
||||
pub fn request_replan_after_slam_correction(&mut self) {
|
||||
if self.target_stack.is_empty() {
|
||||
return;
|
||||
}
|
||||
|
||||
self.current_path = None;
|
||||
self.current_waypoint_index = 0;
|
||||
self.needs_replan = true;
|
||||
self.replan_attempts = 0;
|
||||
self.failure_reason = None;
|
||||
self.nav_state = NavState::Planning;
|
||||
self.path_dirty = true;
|
||||
}
|
||||
|
||||
/// Check if a replan is needed.
|
||||
pub fn needs_replan(&self) -> bool {
|
||||
self.needs_replan
|
||||
|
||||
@@ -370,6 +370,12 @@ pub struct SharedState {
|
||||
|
||||
/// Mapping feature progress (for UI streaming).
|
||||
pub mapping_progress: MappingProgressState,
|
||||
|
||||
/// Number of completed SLAM pose-graph optimizations.
|
||||
///
|
||||
/// This is published together with a freshly rebuilt map so navigation
|
||||
/// can invalidate paths expressed in the previous map geometry.
|
||||
pub slam_optimization_count: u64,
|
||||
}
|
||||
|
||||
impl Default for SharedState {
|
||||
@@ -392,6 +398,7 @@ impl SharedState {
|
||||
exploration_state: ExplorationState::default(),
|
||||
navigation_state: NavigationState::default(),
|
||||
mapping_progress: MappingProgressState::default(),
|
||||
slam_optimization_count: 0,
|
||||
}
|
||||
}
|
||||
|
||||
@@ -434,6 +441,7 @@ impl SharedState {
|
||||
self.robot_status.keyframe_count = 0;
|
||||
self.robot_status.loop_closures = 0;
|
||||
self.robot_status.map_area_m2 = 0.0;
|
||||
self.slam_optimization_count = 0;
|
||||
self.current_map_id = map_id;
|
||||
self.current_map_name = map_name;
|
||||
self.current_map = None;
|
||||
|
||||
@@ -338,75 +338,75 @@ fn run_exploration_loop(
|
||||
}
|
||||
}
|
||||
|
||||
// Get map data for exploration
|
||||
let map = match current_map {
|
||||
Some(m) => {
|
||||
// We have map data, mark initial scan as complete
|
||||
if initial_scan_state != InitialScanState::Complete {
|
||||
log::info!("Initial scan complete, map data available");
|
||||
initial_scan_state = InitialScanState::Complete;
|
||||
// Perform a full initial 360° scan before frontier planning.
|
||||
// Do not stop the scan early just because the first map snapshot appeared.
|
||||
match initial_scan_state {
|
||||
InitialScanState::NotStarted => {
|
||||
log::info!("Starting initial 360° scan rotation");
|
||||
|
||||
initial_scan_state = InitialScanState::Rotating {
|
||||
start_time: Instant::now(),
|
||||
};
|
||||
|
||||
if let Ok(mut state) = shared_state.write() {
|
||||
state.exploration_state.status =
|
||||
"Initial scan: rotating to build map".to_string();
|
||||
}
|
||||
m
|
||||
|
||||
if let Err(e) = motion_controller
|
||||
.lock()
|
||||
.unwrap()
|
||||
.set_velocity(0.0, config.motion.angular_vel)
|
||||
{
|
||||
log::warn!("Failed to start initial rotation: {}", e);
|
||||
}
|
||||
|
||||
thread::sleep(loop_duration);
|
||||
continue;
|
||||
}
|
||||
None => {
|
||||
// No map yet - need to rotate in place to build initial map
|
||||
match initial_scan_state {
|
||||
InitialScanState::NotStarted => {
|
||||
log::info!("No map data yet, starting initial 360° scan rotation");
|
||||
initial_scan_state = InitialScanState::Rotating {
|
||||
start_time: Instant::now(),
|
||||
};
|
||||
|
||||
// Update status
|
||||
if let Ok(mut state) = shared_state.write() {
|
||||
state.exploration_state.status =
|
||||
"Initial scan: rotating to build map".to_string();
|
||||
}
|
||||
InitialScanState::Rotating { start_time } => {
|
||||
let elapsed = start_time.elapsed();
|
||||
|
||||
// Start rotating
|
||||
if let Err(e) = motion_controller
|
||||
.lock()
|
||||
.unwrap()
|
||||
.set_velocity(0.0, config.motion.angular_vel)
|
||||
{
|
||||
log::warn!("Failed to start initial rotation: {}", e);
|
||||
}
|
||||
if elapsed < initial_scan_duration {
|
||||
let progress =
|
||||
elapsed.as_secs_f32() / initial_scan_duration.as_secs_f32() * 100.0;
|
||||
|
||||
if let Ok(mut state) = shared_state.write() {
|
||||
state.exploration_state.status =
|
||||
format!("Initial scan: {:.0}% complete", progress);
|
||||
}
|
||||
InitialScanState::Rotating { start_time } => {
|
||||
// Check if we've completed the full rotation
|
||||
if start_time.elapsed() >= initial_scan_duration {
|
||||
log::info!("Initial rotation complete, waiting for map data");
|
||||
// Stop rotating
|
||||
if let Err(e) = motion_controller.lock().unwrap().emergency_stop() {
|
||||
log::warn!("Failed to stop after initial rotation: {}", e);
|
||||
}
|
||||
|
||||
// Update status - still waiting for map
|
||||
if let Ok(mut state) = shared_state.write() {
|
||||
state.exploration_state.status =
|
||||
"Waiting for map data after initial scan".to_string();
|
||||
}
|
||||
|
||||
// Give a brief pause then continue checking
|
||||
// Don't reset state - keep waiting for map
|
||||
} else {
|
||||
// Still rotating - update status with progress
|
||||
let progress = start_time.elapsed().as_secs_f32()
|
||||
/ initial_scan_duration.as_secs_f32()
|
||||
* 100.0;
|
||||
if let Ok(mut state) = shared_state.write() {
|
||||
state.exploration_state.status =
|
||||
format!("Initial scan: {:.0}% complete", progress);
|
||||
}
|
||||
}
|
||||
}
|
||||
InitialScanState::Complete => {
|
||||
// Should have map but don't - continue waiting
|
||||
if let Ok(mut state) = shared_state.write() {
|
||||
state.exploration_state.status = "Waiting for map data".to_string();
|
||||
}
|
||||
}
|
||||
thread::sleep(loop_duration);
|
||||
continue;
|
||||
}
|
||||
|
||||
log::info!("Initial 360° scan complete");
|
||||
|
||||
// Stop rotation without disabling the motor controller.
|
||||
if let Err(e) = motion_controller.lock().unwrap().set_velocity(0.0, 0.0) {
|
||||
log::warn!("Failed to stop after initial scan: {}", e);
|
||||
}
|
||||
|
||||
initial_scan_state = InitialScanState::Complete;
|
||||
|
||||
if let Ok(mut state) = shared_state.write() {
|
||||
state.exploration_state.status = "Initial scan complete".to_string();
|
||||
}
|
||||
}
|
||||
|
||||
InitialScanState::Complete => {}
|
||||
}
|
||||
|
||||
// Initial scan is finished: frontier exploration can now use the map.
|
||||
let map = match current_map {
|
||||
Some(m) => m,
|
||||
None => {
|
||||
if let Ok(mut state) = shared_state.write() {
|
||||
state.exploration_state.status =
|
||||
"Waiting for map data after initial scan".to_string();
|
||||
}
|
||||
|
||||
thread::sleep(loop_duration);
|
||||
continue;
|
||||
}
|
||||
@@ -521,26 +521,26 @@ fn run_exploration_loop(
|
||||
ExplorationAction::Stuck { reason } => {
|
||||
log::warn!("Exploration stuck: {}", reason);
|
||||
|
||||
// Stop immediately. Do not try to rotate indefinitely:
|
||||
// if no reachable frontier or breakthrough exists, autonomous
|
||||
// exploration must fail safe and wait for user intervention.
|
||||
{
|
||||
let mut mc = motion_controller.lock().unwrap();
|
||||
|
||||
if let Err(e) = mc.emergency_stop() {
|
||||
log::warn!("Failed to stop: {}", e);
|
||||
log::warn!("Failed to stop robot: {}", e);
|
||||
}
|
||||
}
|
||||
|
||||
// Update shared state
|
||||
motion_enabled = false;
|
||||
|
||||
if let Ok(mut state) = shared_state.write() {
|
||||
state.exploration_state.mode = ExplorationMode::Disabled;
|
||||
state.exploration_state.status = format!("Stuck: {}", reason);
|
||||
state.exploration_state.current_target = None;
|
||||
}
|
||||
|
||||
// Try rotating to find new path
|
||||
if let Err(e) = motion_controller
|
||||
.lock()
|
||||
.unwrap()
|
||||
.set_velocity(0.0, config.motion.angular_vel)
|
||||
{
|
||||
log::warn!("Failed to rotate: {}", e);
|
||||
}
|
||||
log::warn!("Autonomous exploration stopped - user intervention required");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -155,6 +155,10 @@ fn run_navigation_loop(
|
||||
let mut _prev_target_count: usize = 0;
|
||||
let mut _prev_nav_state = NavState::Idle;
|
||||
|
||||
// Generation of the map geometry used by the current path.
|
||||
// A pose-graph optimization invalidates world-frame waypoints.
|
||||
let mut last_slam_optimization_count: u64 = 0;
|
||||
|
||||
// Time since navigation became inactive (for delayed disable)
|
||||
let mut inactive_since: Option<Instant> = None;
|
||||
const IDLE_DISABLE_DELAY: Duration = Duration::from_secs(10);
|
||||
@@ -177,7 +181,17 @@ fn run_navigation_loop(
|
||||
}
|
||||
|
||||
// Read current state
|
||||
let (pose, map, hazard, nav_active, is_charging, lidar_scan, current_target, slam_mapping) = {
|
||||
let (
|
||||
pose,
|
||||
map,
|
||||
hazard,
|
||||
nav_active,
|
||||
is_charging,
|
||||
lidar_scan,
|
||||
current_target,
|
||||
slam_mapping,
|
||||
slam_optimization_count,
|
||||
) = {
|
||||
match shared_state.read() {
|
||||
Ok(state) => {
|
||||
let pose = state.robot_status.pose;
|
||||
@@ -217,6 +231,7 @@ fn run_navigation_loop(
|
||||
lidar_scan,
|
||||
current_target,
|
||||
slam_mapping,
|
||||
state.slam_optimization_count,
|
||||
)
|
||||
}
|
||||
Err(e) => {
|
||||
@@ -367,6 +382,46 @@ fn run_navigation_loop(
|
||||
}
|
||||
}
|
||||
|
||||
// Pose-graph optimization changes the geometry of the map frame.
|
||||
// Any A* path generated from the previous geometry is stale.
|
||||
if slam_optimization_count != last_slam_optimization_count {
|
||||
if slam_optimization_count > last_slam_optimization_count {
|
||||
log::warn!(
|
||||
"SLAM pose graph optimized (generation {} -> {})",
|
||||
last_slam_optimization_count,
|
||||
slam_optimization_count
|
||||
);
|
||||
|
||||
if nav_active {
|
||||
log::warn!("Invalidating active navigation path after SLAM correction");
|
||||
|
||||
if let Ok(mut state) = shared_state.write() {
|
||||
state
|
||||
.navigation_state
|
||||
.request_replan_after_slam_correction();
|
||||
state
|
||||
.navigation_state
|
||||
.set_status("SLAM map corrected - replanning path");
|
||||
}
|
||||
|
||||
// Reset progress/stuck tracking because the pose may have
|
||||
// changed discontinuously in the corrected map frame.
|
||||
navigator.reset();
|
||||
|
||||
// Immediately cancel the velocity command associated with
|
||||
// the old path. The new path is planned below.
|
||||
if motors_enabled {
|
||||
if let Err(e) = motion_controller.lock().unwrap().set_velocity(0.0, 0.0) {
|
||||
log::warn!("Failed to stop before SLAM-triggered replan: {}", e);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Also handles a new mapping session resetting the generation to 0.
|
||||
last_slam_optimization_count = slam_optimization_count;
|
||||
}
|
||||
|
||||
// Only run navigation if there's an active target
|
||||
if nav_active {
|
||||
// Get current target count and nav state before update
|
||||
|
||||
@@ -474,8 +474,14 @@ impl SlamContext {
|
||||
let scan_cloud = self.preprocessor.process(&laser_scan);
|
||||
|
||||
// Skip sparse scans
|
||||
if scan_cloud.len() < 50 {
|
||||
log::debug!("Skipping sparse scan: {} points", scan_cloud.len());
|
||||
let min_scan_points = self.slam.config().min_scan_points;
|
||||
|
||||
if scan_cloud.len() < min_scan_points {
|
||||
log::debug!(
|
||||
"Skipping sparse scan: {} points (minimum: {})",
|
||||
scan_cloud.len(),
|
||||
min_scan_points
|
||||
);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -559,6 +565,12 @@ impl SlamContext {
|
||||
let (w, h) = map.dimensions();
|
||||
log::debug!("Updating map in shared state: {}x{}", w, h);
|
||||
state.update_map(map);
|
||||
|
||||
// If pose-graph optimization changed the geometry, publish
|
||||
// the corrected pose and optimization generation together
|
||||
// with this freshly rebuilt map.
|
||||
state.robot_status.pose = self.slam.current_pose();
|
||||
state.slam_optimization_count = self.slam.total_optimizations() as u64;
|
||||
}
|
||||
self.last_map_update = Instant::now();
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user