From c4bb67c33f51fec59f5795a88e1af5b80b67a2ce Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Thu, 4 Jun 2026 17:58:33 -0700 Subject: [PATCH 01/56] Start at goalside node --- .../mls_planner/rust/src/mls_planner.rs | 1 + .../nav_3d/mls_planner/rust/src/planner.rs | 246 +++++++++++++++--- 2 files changed, 207 insertions(+), 40 deletions(-) diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs index 15321a087a..27b040b191 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs @@ -103,6 +103,7 @@ impl Planner { goal, config.voxel_size, config.robot_height, + config.node_spacing_m, ) } diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs index e3bb8e818a..35c38f43f0 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs @@ -6,11 +6,14 @@ use std::collections::BinaryHeap; use ahash::AHashMap; -use crate::adjacency::{CellId, SurfaceLookup}; +use crate::adjacency::{CellId, SurfaceCells, SurfaceLookup}; use crate::dijkstra::walk_preds; use crate::edges::{NodeEdgeIdx, NodeId, PlannerGraph, NO_NODE}; use crate::voxel::{surface_point_xyz, VoxelKey}; +/// Robot-rooted candidate search radius, in multiples of node spacing. +const CANDIDATE_RADIUS_FACTOR: f32 = 3.0; + /// Snap a pose to the best surface cell. pub fn snap_pose_to_cell( surface_lookup: &SurfaceLookup, @@ -79,6 +82,7 @@ pub fn plan( goal_pose: (f32, f32, f32), voxel_size: f32, z_tolerance_m: f32, + node_spacing_m: f32, ) -> Option> { let start_coord = snap_pose_to_cell(&plg.surface_lookup, start_pose, voxel_size, z_tolerance_m)?; @@ -93,41 +97,125 @@ pub fn plan( .map(|(i, n)| (n.cell_id, i as NodeId)) .collect(); - let start_segment = walk_preds(&plg.cell_state, start_cell); let goal_segment = walk_preds(&plg.cell_state, goal_cell); - let start_node = *node_idx_by_cell.get(start_segment.last()?)?; let goal_node = *node_idx_by_cell.get(goal_segment.last()?)?; - let node_seq = shortest_path_nodes(plg, start_node, goal_node)?; + // Cost-to-go to the goal for every node, with predecessors pointing at the + // goal. Rooting the search at the fixed goal makes this single array the + // whole field we need. It is recomputed each scan, so churn in the node set + // between scans never matters. + let (cost_to_go, pred_to_goal) = node_dijkstra(plg, goal_node); + + // Candidate entry nodes: every node the robot can reach on the surface + // within a local radius, each with its true connect cost. Enter on the node + // that minimizes connect cost plus cost-to-go. This is the first node of the + // optimal robot-to-goal path, so the robot never detours to its nearest node + // when a node closer to the goal is just as reachable. + let radius = (node_spacing_m * CANDIDATE_RADIUS_FACTOR).max(voxel_size); + let (connect_dist, connect_pred) = robot_search(&plg.cells, start_cell, radius); + + let mut entry_node = NO_NODE; + let mut best_score = f32::INFINITY; + for (i, node) in plg.nodes.iter().enumerate() { + let Some(&connect) = connect_dist.get(&node.cell_id) else { + continue; + }; + let score = connect + cost_to_go[i]; + if score < best_score { + best_score = score; + entry_node = i as NodeId; + } + } + + let (lead_in, node_seq) = if best_score.is_finite() { + // Lead in along the actual surface path the search found to the entry + // node, so the connection never leaves the surface or doubles back. + let mut lead = walk_local_preds(&connect_pred, plg.nodes[entry_node as usize].cell_id); + lead.reverse(); + (lead, follow_preds(entry_node, goal_node, &pred_to_goal)?) + } else { + // The local search reached no node with a route to the goal. Fall back + // to the robot's region node and its on-surface lead-in. + let start_segment = walk_preds(&plg.cell_state, start_cell); + let region_node = *node_idx_by_cell.get(start_segment.last()?)?; + if !cost_to_go[region_node as usize].is_finite() { + return None; + } + ( + start_segment, + follow_preds(region_node, goal_node, &pred_to_goal)?, + ) + }; + Some(assemble_waypoints( plg, &node_seq, start_pose, - &start_segment, + &lead_in, goal_pose, &goal_segment, voxel_size, )) } -pub fn shortest_path_nodes(plg: &PlannerGraph, start: NodeId, goal: NodeId) -> Option> { - if start == goal { - return Some(vec![start]); +/// Bounded Dijkstra from the robot's cell over the surface, visiting only cells +/// within `radius_m`. Returns per-cell distance and predecessor maps so the +/// on-surface lead-in to any reached cell can be reconstructed. +fn robot_search( + cells: &SurfaceCells, + source: CellId, + radius_m: f32, +) -> (AHashMap, AHashMap) { + let mut dist: AHashMap = AHashMap::new(); + let mut pred: AHashMap = AHashMap::new(); + let mut heap: BinaryHeap = BinaryHeap::new(); + dist.insert(source, 0.0); + heap.push(Scored(0.0, source)); + + while let Some(Scored(d, u)) = heap.pop() { + if d > radius_m { + break; + } + if d > dist.get(&u).copied().unwrap_or(f32::INFINITY) { + continue; + } + for edge in cells.neighbors(u) { + let nd = d + edge.cost; + if nd < dist.get(&edge.dest).copied().unwrap_or(f32::INFINITY) { + dist.insert(edge.dest, nd); + pred.insert(edge.dest, u); + heap.push(Scored(nd, edge.dest)); + } + } } + (dist, pred) +} + +/// Walk predecessors from `from` back to the search source. +fn walk_local_preds(pred: &AHashMap, from: CellId) -> Vec { + let mut path = vec![from]; + let mut cur = from; + while let Some(&p) = pred.get(&cur) { + cur = p; + path.push(cur); + } + path +} + +/// Cost-to-go to `source` for every node, plus a predecessor pointing one hop +/// toward `source`. Unreachable nodes keep an infinite cost and `NO_NODE` pred. +fn node_dijkstra(plg: &PlannerGraph, source: NodeId) -> (Vec, Vec) { let n = plg.nodes.len(); let mut dist = vec![f32::INFINITY; n]; let mut pred = vec![NO_NODE; n]; - dist[start as usize] = 0.0; + dist[source as usize] = 0.0; let mut heap: BinaryHeap = BinaryHeap::new(); - heap.push(Scored(0.0, start)); + heap.push(Scored(0.0, source)); while let Some(Scored(d, u)) = heap.pop() { if d > dist[u as usize] { continue; } - if u == goal { - break; - } for &edge_idx in &plg.node_adj[u as usize] { let edge = &plg.node_edges[edge_idx as usize]; let neighbor = if edge.a == u { edge.b } else { edge.a }; @@ -139,18 +227,34 @@ pub fn shortest_path_nodes(plg: &PlannerGraph, start: NodeId, goal: NodeId) -> O } } } + (dist, pred) +} - if !dist[goal as usize].is_finite() { - return None; +/// Follow goal-pointing predecessors from `from` to `goal`. +fn follow_preds(from: NodeId, goal: NodeId, pred: &[NodeId]) -> Option> { + let mut seq = vec![from]; + let mut cur = from; + while cur != goal { + let next = pred[cur as usize]; + if next == NO_NODE { + return None; + } + cur = next; + seq.push(cur); } - let mut path = vec![goal]; - let mut cur = goal; - while pred[cur as usize] != NO_NODE { - cur = pred[cur as usize]; - path.push(cur); + Some(seq) +} + +/// Append a cell to the path, collapsing out-and-back spurs. When the next cell +/// equals the second-to-last, the path walked up a Voronoi-tree branch and is +/// now retracing it. Drop the dead-end instead of stitching in a detour. This is +/// what keeps the lead-in from looping out to the robot's region node and back. +fn push_cell(cells: &mut Vec, c: CellId) { + if cells.len() >= 2 && cells[cells.len() - 2] == c { + cells.pop(); + } else if cells.last() != Some(&c) { + cells.push(c); } - path.reverse(); - Some(path) } fn assemble_waypoints( @@ -163,7 +267,9 @@ fn assemble_waypoints( voxel_size: f32, ) -> Vec<(f32, f32, f32)> { let mut cells: Vec = Vec::new(); - cells.extend_from_slice(start_segment); + for &c in start_segment { + push_cell(&mut cells, c); + } for pair in node_seq.windows(2) { let (a, b) = (pair[0], pair[1]); @@ -181,16 +287,12 @@ fn assemble_waypoints( let to_b = walk_preds(&plg.cell_state, end_side); for c in from_a.into_iter().chain(to_b) { - if cells.last() != Some(&c) { - cells.push(c); - } + push_cell(&mut cells, c); } } for &c in goal_segment.iter().rev() { - if cells.last() != Some(&c) { - cells.push(c); - } + push_cell(&mut cells, c); } let mut waypoints: Vec<(f32, f32, f32)> = Vec::with_capacity(cells.len() + 2); @@ -271,6 +373,14 @@ mod tests { (0..n).map(|x| (x, 0, 0)).collect() } + fn plan_simple( + plg: &PlannerGraph, + start: (f32, f32, f32), + goal: (f32, f32, f32), + ) -> Option> { + plan(plg, start, goal, VOXEL, Z_TOL, 1.0) + } + #[test] fn snap_picks_in_column_cell() { let mut lookup = SurfaceLookup::new(); @@ -299,7 +409,7 @@ mod tests { #[test] fn plan_returns_none_if_start_cant_snap() { let plg = graph_with_nodes(&strip(20), &[(10, 0, 0)]); - let result = plan(&plg, (0.5, 0.0, 10.0), (1.0, 0.0, 0.1), VOXEL, Z_TOL); + let result = plan_simple(&plg, (0.5, 0.0, 10.0), (1.0, 0.0, 0.1)); assert!(result.is_none()); } @@ -308,14 +418,14 @@ mod tests { let mut cells: Vec = (0..5).map(|x| (x, 0, 0)).collect(); cells.extend((10..15).map(|x| (x, 0, 0))); let plg = graph_with_nodes(&cells, &[(2, 0, 0), (12, 0, 0)]); - let result = plan(&plg, (0.25, 0.0, 0.1), (1.25, 0.0, 0.1), VOXEL, Z_TOL); + let result = plan_simple(&plg, (0.25, 0.0, 0.1), (1.25, 0.0, 0.1)); assert!(result.is_none()); } #[test] fn plan_same_start_and_goal_passes_through_snap_cell() { let plg = graph_with_nodes(&strip(20), &[(10, 0, 0)]); - let wp = plan(&plg, (1.0, 0.0, 0.05), (1.0, 0.0, 0.05), VOXEL, Z_TOL).unwrap(); + let wp = plan_simple(&plg, (1.0, 0.0, 0.05), (1.0, 0.0, 0.05)).unwrap(); assert_eq!(wp.first(), Some(&(1.0, 0.0, 0.05))); assert_eq!(wp.last(), Some(&(1.0, 0.0, 0.05))); let snap = surface_point_xyz(10, 0, 0, VOXEL); @@ -325,7 +435,10 @@ mod tests { #[test] fn plan_traces_surface_from_pose_to_first_node() { let plg = graph_with_nodes(&strip(20), &[(3, 0, 0), (15, 0, 0)]); - let wp = plan(&plg, (0.2, 0.0, 0.05), (1.7, 0.0, 0.05), VOXEL, Z_TOL).unwrap(); + let wp = plan_simple(&plg, (0.2, 0.0, 0.05), (1.7, 0.0, 0.05)).unwrap(); + // The lead-in follows the surface from the robot's cell through its + // region node, so the first waypoint after the start pose is the robot's + // own snapped cell, not a straight jump ahead. let start_cell_pos = surface_point_xyz(2, 0, 0, VOXEL); let goal_cell_pos = surface_point_xyz(17, 0, 0, VOXEL); assert_eq!(wp[1], start_cell_pos); @@ -333,16 +446,69 @@ mod tests { } #[test] - fn plan_three_nodes_visits_them_all() { + fn plan_lead_in_does_not_backtrack_to_region_node() { + // Robot at cell 5 is in node (3)'s region but sits between that node and + // the goal-side node (15). The lead-in must head straight toward the goal + // along the surface, never looping back to cell 3. + let plg = graph_with_nodes(&strip(20), &[(3, 0, 0), (15, 0, 0)]); + let wp = plan_simple(&plg, (0.55, 0.0, 0.05), (1.7, 0.0, 0.05)).unwrap(); + let xs: Vec = wp[1..wp.len() - 1] + .iter() + .map(|w| (w.0 / VOXEL).floor() as i32) + .collect(); + assert_eq!(xs.first(), Some(&5)); + assert!( + xs.windows(2).all(|p| p[1] >= p[0]), + "lead-in walked backward: {xs:?}" + ); + } + + #[test] + fn plan_path_waypoints_are_all_on_the_surface() { let plg = graph_with_nodes(&strip(20), &[(3, 0, 0), (10, 0, 0), (17, 0, 0)]); - let wp = plan(&plg, (0.2, 0.0, 0.05), (1.9, 0.0, 0.05), VOXEL, Z_TOL).unwrap(); - let node_xy: Vec<(f32, f32)> = plg.nodes.iter().map(|n| (n.pos.0, n.pos.1)).collect(); - for &(nx, ny) in &node_xy { + let wp = plan_simple(&plg, (0.2, 0.0, 0.05), (1.9, 0.0, 0.05)).unwrap(); + // Every waypoint between the raw start and goal poses must land on a + // surface cell. Consecutive waypoints must also be adjacent cells, so the + // path never jumps across a gap. + let on_surface = |w: &(f32, f32, f32)| { + let ix = (w.0 / VOXEL).floor() as i32; + let iy = (w.1 / VOXEL).floor() as i32; + plg.cells.id((ix, iy, 0)).is_some() + }; + for w in &wp[1..wp.len() - 1] { + assert!(on_surface(w), "waypoint {w:?} is off the surface"); + } + for pair in wp[1..wp.len() - 1].windows(2) { + let dx = ((pair[0].0 - pair[1].0) / VOXEL).round().abs() as i32; + let dy = ((pair[0].1 - pair[1].1) / VOXEL).round().abs() as i32; assert!( - wp.iter() - .any(|w| (w.0 - nx).abs() < 1e-5 && (w.1 - ny).abs() < 1e-5), - "node ({nx}, {ny}) should appear among waypoints" + dx + dy <= 1, + "waypoints {:?} and {:?} are not adjacent", + pair[0], + pair[1] ); } } + + #[test] + fn plan_enters_on_goalward_node_not_nearest() { + // The robot sits past node (2) toward the goal. Node (2) is nearest, but + // node (10) is more reachable on a goal-minimizing basis. The entry must + // be the goalward node, so the path never visits node (2) behind it. + let plg = graph_with_nodes(&strip(20), &[(2, 0, 0), (10, 0, 0)]); + let wp = plan_simple(&plg, (0.45, 0.0, 0.05), (1.25, 0.0, 0.05)).unwrap(); + let nearest = surface_point_xyz(2, 0, 0, VOXEL); + assert!( + !wp.iter().any(|w| (w.0 - nearest.0).abs() < 1e-5), + "path doubled back to the nearest node: {wp:?}" + ); + let xs: Vec = wp[1..wp.len() - 1] + .iter() + .map(|w| (w.0 / VOXEL).floor() as i32) + .collect(); + assert!( + xs.windows(2).all(|p| p[1] >= p[0]), + "path stepped backward: {xs:?}" + ); + } } From 188da79052670a8dfd07f39eb1d59327816b3ce7 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Thu, 4 Jun 2026 18:18:01 -0700 Subject: [PATCH 02/56] String pulling --- .../mls_planner/rust/src/mls_planner.rs | 1 + .../nav_3d/mls_planner/rust/src/planner.rs | 172 ++++++++++++++---- 2 files changed, 142 insertions(+), 31 deletions(-) diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs index 27b040b191..8991e99bf1 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs @@ -104,6 +104,7 @@ impl Planner { config.voxel_size, config.robot_height, config.node_spacing_m, + config.node_step_threshold_m, ) } diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs index 35c38f43f0..746b92c4ef 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs @@ -83,6 +83,7 @@ pub fn plan( voxel_size: f32, z_tolerance_m: f32, node_spacing_m: f32, + node_step_threshold_m: f32, ) -> Option> { let start_coord = snap_pose_to_cell(&plg.surface_lookup, start_pose, voxel_size, z_tolerance_m)?; @@ -147,14 +148,15 @@ pub fn plan( ) }; - Some(assemble_waypoints( - plg, - &node_seq, - start_pose, - &lead_in, - goal_pose, - &goal_segment, - voxel_size, + // Straight-line shortcut tolerance: how far the smoothed path may sit from + // the surface in height, in cells. Tied to the traversable step so a shortcut + // can ride over a slope but not float across a gap or cut through a step. + let smooth_tol_cells = ((node_step_threshold_m / voxel_size).round() as i32).max(1); + + let cells = assemble_cells(plg, &node_seq, &lead_in, &goal_segment); + let cells = string_pull(plg, &cells, smooth_tol_cells); + Some(cells_to_waypoints( + plg, &cells, start_pose, goal_pose, voxel_size, )) } @@ -257,17 +259,16 @@ fn push_cell(cells: &mut Vec, c: CellId) { } } -fn assemble_waypoints( +/// Stitch the cell path: the lead-in to the entry node, the cell chains along +/// each node-graph edge, and the goal segment, collapsing out-and-back spurs. +fn assemble_cells( plg: &PlannerGraph, node_seq: &[NodeId], - start_pose: (f32, f32, f32), - start_segment: &[CellId], - goal_pose: (f32, f32, f32), + lead_in: &[CellId], goal_segment: &[CellId], - voxel_size: f32, -) -> Vec<(f32, f32, f32)> { +) -> Vec { let mut cells: Vec = Vec::new(); - for &c in start_segment { + for &c in lead_in { push_cell(&mut cells, c); } @@ -295,9 +296,21 @@ fn assemble_waypoints( push_cell(&mut cells, c); } + cells +} + +/// Turn the cell path into world waypoints, bookended by the raw start and goal +/// poses so the path begins and ends exactly where asked. +fn cells_to_waypoints( + plg: &PlannerGraph, + cells: &[CellId], + start_pose: (f32, f32, f32), + goal_pose: (f32, f32, f32), + voxel_size: f32, +) -> Vec<(f32, f32, f32)> { let mut waypoints: Vec<(f32, f32, f32)> = Vec::with_capacity(cells.len() + 2); waypoints.push(start_pose); - for id in cells { + for &id in cells { let (ix, iy, iz) = plg.cells.coord(id); waypoints.push(surface_point_xyz(ix, iy, iz, voxel_size)); } @@ -305,6 +318,73 @@ fn assemble_waypoints( waypoints } +/// Greedily replace runs of cells with straight shortcuts that stay on the +/// surface. From each anchor, extend to the farthest cell still in line of sight +/// and keep only that one, collapsing the staircase and Voronoi-boundary scallop +/// into straight segments. +fn string_pull(plg: &PlannerGraph, cells: &[CellId], tol_cells: i32) -> Vec { + if cells.len() <= 2 { + return cells.to_vec(); + } + let mut out = vec![cells[0]]; + let mut anchor = 0; + while anchor + 1 < cells.len() { + let anchor_coord = plg.cells.coord(cells[anchor]); + let mut last_ok = anchor + 1; + let mut j = anchor + 1; + while j < cells.len() { + let coord = plg.cells.coord(cells[j]); + if !los_on_surface(&plg.surface_lookup, anchor_coord, coord, tol_cells) { + break; + } + last_ok = j; + j += 1; + } + out.push(cells[last_ok]); + anchor = last_ok; + } + out +} + +/// True if the straight segment between two surface cells stays on the surface: +/// every cell column the segment crosses must hold a surface cell whose height +/// is within `tol_cells` of the segment. Rejects shortcuts that would float over +/// a gap or cut across a step taller than the tolerance. +fn los_on_surface( + surface_lookup: &SurfaceLookup, + a: VoxelKey, + b: VoxelKey, + tol_cells: i32, +) -> bool { + let (dx, dy, dz) = (b.0 - a.0, b.1 - a.1, b.2 - a.2); + let samples = dx.abs().max(dy.abs()) * 2; + if samples == 0 { + return true; + } + let (mut last_ix, mut last_iy) = (i32::MIN, i32::MIN); + for k in 0..=samples { + let t = k as f32 / samples as f32; + let ix = (a.0 as f32 + t * dx as f32).round() as i32; + let iy = (a.1 as f32 + t * dy as f32).round() as i32; + if ix == last_ix && iy == last_iy { + continue; + } + last_ix = ix; + last_iy = iy; + let iz_line = a.2 as f32 + t * dz as f32; + let Some(zs) = surface_lookup.get(&(ix, iy)) else { + return false; + }; + if !zs + .iter() + .any(|&iz| (iz as f32 - iz_line).abs() <= tol_cells as f32) + { + return false; + } + } + true +} + fn edge_between(plg: &PlannerGraph, a: NodeId, b: NodeId) -> Option { for &edge_idx in &plg.node_adj[a as usize] { let edge = &plg.node_edges[edge_idx as usize]; @@ -378,7 +458,7 @@ mod tests { start: (f32, f32, f32), goal: (f32, f32, f32), ) -> Option> { - plan(plg, start, goal, VOXEL, Z_TOL, 1.0) + plan(plg, start, goal, VOXEL, Z_TOL, 1.0, 0.25) } #[test] @@ -463,33 +543,63 @@ mod tests { ); } + fn waypoint_key(w: &(f32, f32, f32)) -> VoxelKey { + ( + (w.0 / VOXEL).floor() as i32, + (w.1 / VOXEL).floor() as i32, + (w.2 / VOXEL).round() as i32 - 1, + ) + } + #[test] - fn plan_path_waypoints_are_all_on_the_surface() { + fn plan_path_segments_stay_on_the_surface() { let plg = graph_with_nodes(&strip(20), &[(3, 0, 0), (10, 0, 0), (17, 0, 0)]); let wp = plan_simple(&plg, (0.2, 0.0, 0.05), (1.9, 0.0, 0.05)).unwrap(); - // Every waypoint between the raw start and goal poses must land on a - // surface cell. Consecutive waypoints must also be adjacent cells, so the - // path never jumps across a gap. - let on_surface = |w: &(f32, f32, f32)| { - let ix = (w.0 / VOXEL).floor() as i32; - let iy = (w.1 / VOXEL).floor() as i32; - plg.cells.id((ix, iy, 0)).is_some() - }; + // After smoothing the waypoints are no longer cell-adjacent, but each + // must land on a surface cell and each straight segment between + // consecutive waypoints must stay on the surface. + let tol = ((0.25f32 / VOXEL).round() as i32).max(1); for w in &wp[1..wp.len() - 1] { - assert!(on_surface(w), "waypoint {w:?} is off the surface"); + assert!( + plg.cells.id(waypoint_key(w)).is_some(), + "waypoint {w:?} is off the surface" + ); } for pair in wp[1..wp.len() - 1].windows(2) { - let dx = ((pair[0].0 - pair[1].0) / VOXEL).round().abs() as i32; - let dy = ((pair[0].1 - pair[1].1) / VOXEL).round().abs() as i32; assert!( - dx + dy <= 1, - "waypoints {:?} and {:?} are not adjacent", + los_on_surface( + &plg.surface_lookup, + waypoint_key(&pair[0]), + waypoint_key(&pair[1]), + tol + ), + "segment {:?} -> {:?} leaves the surface", pair[0], pair[1] ); } } + #[test] + fn string_pull_straightens_open_area() { + // A filled rectangle: every cell is surface, so any straight segment is + // on-surface. The diagonal corner-to-corner path must collapse to a + // near-straight shot instead of the node-graph staircase. + let mut cells: Vec = Vec::new(); + for x in 0..10 { + for y in 0..6 { + cells.push((x, y, 0)); + } + } + let plg = graph_with_nodes(&cells, &[(2, 2, 0), (7, 3, 0)]); + let wp = plan_simple(&plg, (0.05, 0.05, 0.05), (0.85, 0.55, 0.05)).unwrap(); + let interior = wp.len() - 2; + assert!( + interior <= 4, + "path not straightened: {interior} interior points" + ); + } + #[test] fn plan_enters_on_goalward_node_not_nearest() { // The robot sits past node (2) toward the goal. Node (2) is nearest, but From 7d489a2817ee148b78b92ed3e6bae5fa28f2fd54 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Thu, 4 Jun 2026 18:45:31 -0700 Subject: [PATCH 03/56] Trim --- .../nav_3d/mls_planner/rust/src/planner.rs | 141 +++++++++--------- 1 file changed, 67 insertions(+), 74 deletions(-) diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs index 746b92c4ef..85427ea918 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs @@ -101,19 +101,42 @@ pub fn plan( let goal_segment = walk_preds(&plg.cell_state, goal_cell); let goal_node = *node_idx_by_cell.get(goal_segment.last()?)?; - // Cost-to-go to the goal for every node, with predecessors pointing at the - // goal. Rooting the search at the fixed goal makes this single array the - // whole field we need. It is recomputed each scan, so churn in the node set - // between scans never matters. + // Rooted at the goal so one pass covers every node's cost-to-go. let (cost_to_go, pred_to_goal) = node_dijkstra(plg, goal_node); - // Candidate entry nodes: every node the robot can reach on the surface - // within a local radius, each with its true connect cost. Enter on the node - // that minimizes connect cost plus cost-to-go. This is the first node of the - // optimal robot-to-goal path, so the robot never detours to its nearest node - // when a node closer to the goal is just as reachable. let radius = (node_spacing_m * CANDIDATE_RADIUS_FACTOR).max(voxel_size); - let (connect_dist, connect_pred) = robot_search(&plg.cells, start_cell, radius); + let (lead_in, node_seq) = select_entry( + plg, + start_cell, + goal_node, + &cost_to_go, + &pred_to_goal, + &node_idx_by_cell, + radius, + )?; + + // Shortcut height tolerance in cells, tied to the traversable step. + let smooth_tol_cells = ((node_step_threshold_m / voxel_size).round() as i32).max(1); + + let cells = assemble_cells(plg, &node_seq, &lead_in, &goal_segment); + let cells = string_pull(plg, &cells, smooth_tol_cells); + Some(cells_to_waypoints( + plg, &cells, start_pose, goal_pose, voxel_size, + )) +} + +/// Pick the entry node by connect cost plus cost-to-go, with its on-surface +/// lead-in and the node sequence to the goal. +fn select_entry( + plg: &PlannerGraph, + start_cell: CellId, + goal_node: NodeId, + cost_to_go: &[f32], + pred_to_goal: &[NodeId], + node_idx_by_cell: &AHashMap, + radius_m: f32, +) -> Option<(Vec, Vec)> { + let (connect_dist, connect_pred) = robot_search(&plg.cells, start_cell, radius_m); let mut entry_node = NO_NODE; let mut best_score = f32::INFINITY; @@ -128,41 +151,25 @@ pub fn plan( } } - let (lead_in, node_seq) = if best_score.is_finite() { - // Lead in along the actual surface path the search found to the entry - // node, so the connection never leaves the surface or doubles back. + if best_score.is_finite() { let mut lead = walk_local_preds(&connect_pred, plg.nodes[entry_node as usize].cell_id); lead.reverse(); - (lead, follow_preds(entry_node, goal_node, &pred_to_goal)?) - } else { - // The local search reached no node with a route to the goal. Fall back - // to the robot's region node and its on-surface lead-in. - let start_segment = walk_preds(&plg.cell_state, start_cell); - let region_node = *node_idx_by_cell.get(start_segment.last()?)?; - if !cost_to_go[region_node as usize].is_finite() { - return None; - } - ( - start_segment, - follow_preds(region_node, goal_node, &pred_to_goal)?, - ) - }; - - // Straight-line shortcut tolerance: how far the smoothed path may sit from - // the surface in height, in cells. Tied to the traversable step so a shortcut - // can ride over a slope but not float across a gap or cut through a step. - let smooth_tol_cells = ((node_step_threshold_m / voxel_size).round() as i32).max(1); + return Some((lead, follow_preds(entry_node, goal_node, pred_to_goal)?)); + } - let cells = assemble_cells(plg, &node_seq, &lead_in, &goal_segment); - let cells = string_pull(plg, &cells, smooth_tol_cells); - Some(cells_to_waypoints( - plg, &cells, start_pose, goal_pose, voxel_size, + let start_segment = walk_preds(&plg.cell_state, start_cell); + let region_node = *node_idx_by_cell.get(start_segment.last()?)?; + if !cost_to_go[region_node as usize].is_finite() { + return None; + } + Some(( + start_segment, + follow_preds(region_node, goal_node, pred_to_goal)?, )) } -/// Bounded Dijkstra from the robot's cell over the surface, visiting only cells -/// within `radius_m`. Returns per-cell distance and predecessor maps so the -/// on-surface lead-in to any reached cell can be reconstructed. +/// Bounded Dijkstra from the robot cell, visiting cells within the radius. +/// Returns per-cell distance and predecessor maps. fn robot_search( cells: &SurfaceCells, source: CellId, @@ -193,7 +200,7 @@ fn robot_search( (dist, pred) } -/// Walk predecessors from `from` back to the search source. +/// Walk predecessors back to the search source. fn walk_local_preds(pred: &AHashMap, from: CellId) -> Vec { let mut path = vec![from]; let mut cur = from; @@ -204,8 +211,8 @@ fn walk_local_preds(pred: &AHashMap, from: CellId) -> Vec (Vec, Vec) { let n = plg.nodes.len(); let mut dist = vec![f32::INFINITY; n]; @@ -232,7 +239,7 @@ fn node_dijkstra(plg: &PlannerGraph, source: NodeId) -> (Vec, Vec) (dist, pred) } -/// Follow goal-pointing predecessors from `from` to `goal`. +/// Build the node sequence by following goal-pointing predecessors. fn follow_preds(from: NodeId, goal: NodeId, pred: &[NodeId]) -> Option> { let mut seq = vec![from]; let mut cur = from; @@ -247,10 +254,8 @@ fn follow_preds(from: NodeId, goal: NodeId, pred: &[NodeId]) -> Option, c: CellId) { if cells.len() >= 2 && cells[cells.len() - 2] == c { cells.pop(); @@ -259,8 +264,7 @@ fn push_cell(cells: &mut Vec, c: CellId) { } } -/// Stitch the cell path: the lead-in to the entry node, the cell chains along -/// each node-graph edge, and the goal segment, collapsing out-and-back spurs. +/// Build the cell path from the entry lead-in through the node edges to the goal. fn assemble_cells( plg: &PlannerGraph, node_seq: &[NodeId], @@ -299,8 +303,8 @@ fn assemble_cells( cells } -/// Turn the cell path into world waypoints, bookended by the raw start and goal -/// poses so the path begins and ends exactly where asked. +/// Convert the cell path to world waypoints, with the raw start and goal poses +/// as the endpoints. fn cells_to_waypoints( plg: &PlannerGraph, cells: &[CellId], @@ -318,10 +322,8 @@ fn cells_to_waypoints( waypoints } -/// Greedily replace runs of cells with straight shortcuts that stay on the -/// surface. From each anchor, extend to the farthest cell still in line of sight -/// and keep only that one, collapsing the staircase and Voronoi-boundary scallop -/// into straight segments. +/// Shortcut runs of cells with straight on-surface segments, keeping the +/// farthest cell in line of sight from each anchor. fn string_pull(plg: &PlannerGraph, cells: &[CellId], tol_cells: i32) -> Vec { if cells.len() <= 2 { return cells.to_vec(); @@ -346,10 +348,9 @@ fn string_pull(plg: &PlannerGraph, cells: &[CellId], tol_cells: i32) -> Vec = wp[1..wp.len() - 1] @@ -555,9 +552,8 @@ mod tests { fn plan_path_segments_stay_on_the_surface() { let plg = graph_with_nodes(&strip(20), &[(3, 0, 0), (10, 0, 0), (17, 0, 0)]); let wp = plan_simple(&plg, (0.2, 0.0, 0.05), (1.9, 0.0, 0.05)).unwrap(); - // After smoothing the waypoints are no longer cell-adjacent, but each - // must land on a surface cell and each straight segment between - // consecutive waypoints must stay on the surface. + // Smoothed waypoints are no longer cell-adjacent, but each segment + // between them must still stay on the surface. let tol = ((0.25f32 / VOXEL).round() as i32).max(1); for w in &wp[1..wp.len() - 1] { assert!( @@ -582,9 +578,8 @@ mod tests { #[test] fn string_pull_straightens_open_area() { - // A filled rectangle: every cell is surface, so any straight segment is - // on-surface. The diagonal corner-to-corner path must collapse to a - // near-straight shot instead of the node-graph staircase. + // Filled rectangle: every straight segment is on-surface, so the diagonal + // path collapses instead of staircasing through the nodes. let mut cells: Vec = Vec::new(); for x in 0..10 { for y in 0..6 { @@ -602,9 +597,7 @@ mod tests { #[test] fn plan_enters_on_goalward_node_not_nearest() { - // The robot sits past node (2) toward the goal. Node (2) is nearest, but - // node (10) is more reachable on a goal-minimizing basis. The entry must - // be the goalward node, so the path never visits node (2) behind it. + // Robot sits past node (2) toward the goal; entry must skip it for node (10). let plg = graph_with_nodes(&strip(20), &[(2, 0, 0), (10, 0, 0)]); let wp = plan_simple(&plg, (0.45, 0.0, 0.05), (1.25, 0.0, 0.05)).unwrap(); let nearest = surface_point_xyz(2, 0, 0, VOXEL); From 39794f9f4ca735aa74be252b7b44977ae6e1b154 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Fri, 5 Jun 2026 14:47:08 -0700 Subject: [PATCH 04/56] Timing and visualization --- .../nav_3d/mls_planner/mls_planner.pyi | 11 ++ .../mls_planner/rust/src/mls_planner.rs | 31 ++++ .../nav_3d/mls_planner/rust/src/python.rs | 15 ++ .../nav_3d/mls_planner/transformer.py | 8 + .../nav_3d/mls_planner/utils/plan_rrd.py | 153 +++++++++++++++--- 5 files changed, 192 insertions(+), 26 deletions(-) diff --git a/dimos/navigation/nav_3d/mls_planner/mls_planner.pyi b/dimos/navigation/nav_3d/mls_planner/mls_planner.pyi index dfff32d2c8..5ad17c14e6 100644 --- a/dimos/navigation/nav_3d/mls_planner/mls_planner.pyi +++ b/dimos/navigation/nav_3d/mls_planner/mls_planner.pyi @@ -53,6 +53,17 @@ class MLSPlanner: """Plan a path between start and goal. Returns (W, 3) float32, or None if unreachable.""" ... + def voxel_count(self) -> int: + """Number of occupied voxels in the current map.""" + ... + + def last_timings(self) -> dict[str, float]: + """Per-substep wall-clock ms of the last update_global_map. + + Keys are voxelize_ms, surfaces_ms, and graph_ms. + """ + ... + def clear(self) -> None: """Drop the graph and buffered state.""" ... diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs index 8991e99bf1..beb2dcfc4e 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs @@ -3,6 +3,8 @@ //! Config and the owned-state Planner that builds and queries the MLS graph. +use std::time::Instant; + use ahash::AHashSet; use serde::Deserialize; use validator::Validate; @@ -34,12 +36,21 @@ pub struct Config { pub node_step_threshold_m: f32, } +/// Wall-clock cost of each stage of the most recent full rebuild, in milliseconds. +#[derive(Default, Clone, Copy)] +pub struct RebuildTimings { + pub voxelize_ms: f64, + pub surfaces_ms: f64, + pub graph_ms: f64, +} + #[derive(Default)] pub struct Planner { graph: PlannerGraph, voxel_map: AHashSet, by_col: ColumnIz, surface: Vec, + last_timings: RebuildTimings, } impl Planner { @@ -48,11 +59,14 @@ impl Planner { let clearance = (config.robot_height / voxel_size).ceil() as i32; let step = (config.node_step_threshold_m / voxel_size).floor() as i32; + let t_voxelize = Instant::now(); self.voxel_map.clear(); for &p in points { self.voxel_map.insert(voxelize(p, voxel_size)); } + let voxelize_ms = ms(t_voxelize.elapsed()); + let t_surfaces = Instant::now(); extract_surfaces( &self.voxel_map, clearance, @@ -61,7 +75,9 @@ impl Planner { &mut self.by_col, &mut self.surface, ); + let surfaces_ms = ms(t_surfaces.elapsed()); + let t_graph = Instant::now(); build_surface_lookup(&self.surface, &mut self.graph.surface_lookup); build_surface_cells( &mut self.graph.cells, @@ -86,6 +102,13 @@ impl Planner { &mut self.graph.node_edges, &mut self.graph.node_adj, ); + let graph_ms = ms(t_graph.elapsed()); + + self.last_timings = RebuildTimings { + voxelize_ms, + surfaces_ms, + graph_ms, + }; } pub fn plan( @@ -119,4 +142,12 @@ impl Planner { pub fn voxel_count(&self) -> usize { self.voxel_map.len() } + + pub fn last_timings(&self) -> RebuildTimings { + self.last_timings + } +} + +fn ms(d: std::time::Duration) -> f64 { + d.as_secs_f64() * 1000.0 } diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/python.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/python.rs index 58021d5d7f..5e1eca2315 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/python.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/python.rs @@ -5,6 +5,7 @@ use numpy::ndarray::Array2; use numpy::{IntoPyArray, PyArray2, PyReadonlyArray2, PyUntypedArrayMethods}; use pyo3::exceptions::PyValueError; use pyo3::prelude::*; +use pyo3::types::PyDict; use validator::Validate; use crate::edges::edges_to_segments; @@ -164,6 +165,20 @@ impl MLSPlanner { ) } + fn voxel_count(&self) -> usize { + self.planner.voxel_count() + } + + /// Per-substep wall-clock cost (ms) of the most recent ``update_global_map``. + fn last_timings<'py>(&self, py: Python<'py>) -> PyResult> { + let t = self.planner.last_timings(); + let d = PyDict::new(py); + d.set_item("voxelize_ms", t.voxelize_ms)?; + d.set_item("surfaces_ms", t.surfaces_ms)?; + d.set_item("graph_ms", t.graph_ms)?; + Ok(d) + } + fn clear(&mut self) { self.planner = Planner::default(); } diff --git a/dimos/navigation/nav_3d/mls_planner/transformer.py b/dimos/navigation/nav_3d/mls_planner/transformer.py index 3448bebf22..d04532c969 100644 --- a/dimos/navigation/nav_3d/mls_planner/transformer.py +++ b/dimos/navigation/nav_3d/mls_planner/transformer.py @@ -14,6 +14,7 @@ from __future__ import annotations +import time from typing import TYPE_CHECKING, Any from dimos.memory2.transform import Transformer @@ -78,9 +79,14 @@ def __call__( voxel_map = obs.data planner.update_global_map(voxel_map.points_f32()) + t_plan = time.perf_counter() waypoints = planner.plan(start, self.goal) + plan_ms = (time.perf_counter() - t_plan) * 1000 path = self._path_from_waypoints(waypoints, obs.ts) + timings = {**planner.last_timings(), "plan_ms": plan_ms} + timings["total_ms"] = sum(timings.values()) + yield obs.derive( data=path, tags={ @@ -91,5 +97,7 @@ def __call__( "node_edges": planner.node_edges(), "start": start, "planned": waypoints is not None, + "timings": timings, + "voxels": planner.voxel_count(), }, ) diff --git a/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py b/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py index fea6c89aa6..18857f9ef8 100644 --- a/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py +++ b/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py @@ -17,15 +17,19 @@ from __future__ import annotations from pathlib import Path as FsPath +import re from typing import TYPE_CHECKING import rerun as rr import typer from dimos.mapping.ray_tracing.transformer import RayTraceMap +from dimos.memory2.store.memory import MemoryStore from dimos.memory2.store.sqlite import SqliteStore +from dimos.memory2.stream import Stream from dimos.memory2.transform import FnTransformer from dimos.memory2.type.observation import Observation +from dimos.memory2.vis.plot.plot import Plot from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.nav_msgs.Path import Path from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2, register_colormap_annotation @@ -40,6 +44,72 @@ PairObs = Observation[tuple[Observation[PointCloud2], Observation[Odometry]]] +TIMING_KEYS = ["voxelize_ms", "surfaces_ms", "graph_ms", "plan_ms", "total_ms"] +SIZE_KEYS = ["voxels", "surface_cells", "nodes", "edges"] + + +def _print_summary(streams: dict[str, dict[str, Stream[float]]]) -> None: + print("\nper-frame summary (mean / p50 / p95 / max):") + for kind, by_key in streams.items(): + for key, stream in by_key.items(): + values = [obs.data for obs in stream] + if not values: + continue + arr = np.asarray(values, dtype=np.float64) + mean, p50, p95, peak = ( + arr.mean(), + np.percentile(arr, 50), + np.percentile(arr, 95), + arr.max(), + ) + print(f" {kind}/{key:<14} {mean:9.2f} {p50:9.2f} {p95:9.2f} {peak:9.2f}") + + +def _stitch_svgs(svgs: list[str]) -> str: + """Stack standalone SVGs vertically into one. Namespaces each panel's ids + so matplotlib's reused ids (axes_1, clip paths, glyphs) don't collide.""" + panels: list[str] = [] + widths: list[float] = [] + offset = 0.0 + for i, svg in enumerate(svgs): + body = svg[svg.index("px conversion + # overflows the parent viewport and clips each panel on the right. + body = body.replace(m.group(0), f'width="{width}" height="{height}"', 1) + body = body.replace("\n' + '\n' + "\n".join(panels) + "\n\n" + ) + + +def _save_plot(timing_streams: dict[str, Stream[float]], voxels: Stream[float], path: str) -> None: + panels: list[str] = [] + for key in TIMING_KEYS: + plot = Plot() + plot.add(timing_streams[key], label=key, connect=None) + panels.append(plot.to_svg()) + voxel_plot = Plot() + voxel_plot.add(voxels, label="voxels", connect=None) + panels.append(voxel_plot.to_svg()) + with open(path, "w") as f: + f.write(_stitch_svgs(panels)) + print(f"wrote {path}") + def _attach_pose_from_odom(pair_obs: PairObs) -> Observation[PointCloud2]: lidar_obs = pair_obs.data[0] @@ -103,6 +173,9 @@ def main( False, "--live", help="Also spawn the rerun viewer when --out is set" ), render_voxel: float = typer.Option(0.05, "--render-voxel", help="Rerun voxel render size (m)"), + plot_out: FsPath | None = typer.Option( + None, "--plot-out", help="Write an SVG timing/size plot here when the run ends" + ), ) -> None: db_path = resolve_named_path(dataset, ".db") @@ -119,7 +192,7 @@ def main( store = SqliteStore(path=str(db_path)) with store: - lidar = store.stream(lidar_stream, PointCloud2).order_by("ts") + lidar = store.stream(lidar_stream, PointCloud2).order_by("ts").from_time(110).to_time(120) odom = store.stream(odom_stream, Odometry).order_by("ts") pose_tagged = lidar.align(odom, tolerance=align_tol).transform( @@ -143,37 +216,65 @@ def main( rr.log("world/goal", rr.Points3D([goal], colors=[[255, 0, 0]], radii=0.1), static=True) - for obs in pipeline: - rr.set_time(TIMELINE, timestamp=obs.ts) + metrics = MemoryStore() + timing_streams = {k: metrics.stream(f"timing_{k}", float) for k in TIMING_KEYS} + size_streams = {k: metrics.stream(f"size_{k}", float) for k in SIZE_KEYS} - start = obs.tags["start"] - rr.log("world/start", rr.Points3D([start], colors=[[0, 255, 0]], radii=0.1)) + try: + for obs in pipeline: + rr.set_time(TIMELINE, timestamp=obs.ts) - voxel_map = obs.tags["voxel_map"] - rr.log("world/voxel_map", voxel_map.to_rerun(voxel_size=render_voxel)) + start = obs.tags["start"] + rr.log("world/start", rr.Points3D([start], colors=[[0, 255, 0]], radii=0.1)) - surface = obs.tags["surface_map"] - if surface.size: - rr.log( - "world/surface_map", - rr.Points3D(surface, colors=[[120, 120, 200]], radii=render_voxel / 2), - ) + voxel_map = obs.tags["voxel_map"] + rr.log("world/voxel_map", voxel_map.to_rerun(voxel_size=render_voxel)) - nodes = obs.tags["nodes"] - if nodes.size: - rr.log("world/nodes", rr.Points3D(nodes, colors=[[255, 200, 0]], radii=0.05)) + surface = obs.tags["surface_map"] + if surface.size: + rr.log( + "world/surface_map", + rr.Points3D(surface, colors=[[120, 120, 200]], radii=render_voxel / 2), + ) - _log_edges(obs.tags["node_edges"], "world/node_edges") - _log_path(obs.data, "world/path") + nodes = obs.tags["nodes"] + if nodes.size: + rr.log("world/nodes", rr.Points3D(nodes, colors=[[255, 200, 0]], radii=0.05)) - count = obs.tags.get("frame_count", "?") - planned = obs.tags.get("planned", False) - print( - f"frame_count={count} planned={planned} waypoints={len(obs.data.poses)}", - end="\r", - flush=True, - ) - print() + edges = obs.tags["node_edges"] + _log_edges(edges, "world/node_edges") + _log_path(obs.data, "world/path") + + timings = obs.tags["timings"] + sizes = { + "voxels": obs.tags["voxels"], + "surface_cells": len(surface), + "nodes": len(nodes), + "edges": len(edges), + } + for key, value in timings.items(): + timing_streams[key].append(float(value), ts=obs.ts) + rr.log(f"metrics/timing/{key}", rr.Scalars(value)) + for key, value in sizes.items(): + size_streams[key].append(float(value), ts=obs.ts) + rr.log(f"metrics/size/{key}", rr.Scalars(value)) + + count = obs.tags.get("frame_count", "?") + planned = obs.tags.get("planned", False) + print( + f"frame_count={count} planned={planned} " + f"waypoints={len(obs.data.poses)} " + f"rebuild={timings['total_ms'] - timings['plan_ms']:.1f}ms " + f"plan={timings['plan_ms']:.1f}ms", + end="\r", + flush=True, + ) + except KeyboardInterrupt: + print("\ninterrupted; reporting metrics for completed frames") + finally: + _print_summary({"timing": timing_streams, "size": size_streams}) + if plot_out is not None: + _save_plot(timing_streams, size_streams["voxels"], str(plot_out)) if out is not None: print(f"wrote {out}") From 2fbfa6de4fe1f51f721c8895bd042a6dccb3b872 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Fri, 5 Jun 2026 18:12:44 -0700 Subject: [PATCH 05/56] Incremental maps --- dimos/mapping/ray_tracing/transformer.py | 50 +- .../nav_3d/mls_planner/mls_planner.pyi | 19 + .../nav_3d/mls_planner/rust/src/adjacency.rs | 83 +- .../nav_3d/mls_planner/rust/src/dijkstra.rs | 229 +++++- .../nav_3d/mls_planner/rust/src/edges.rs | 160 +++- .../mls_planner/rust/src/mls_planner.rs | 742 +++++++++++++++++- .../nav_3d/mls_planner/rust/src/nodes.rs | 157 +++- .../nav_3d/mls_planner/rust/src/planner.rs | 82 +- .../nav_3d/mls_planner/rust/src/python.rs | 70 +- .../nav_3d/mls_planner/rust/src/surfaces.rs | 86 ++ .../nav_3d/mls_planner/transformer.py | 11 +- .../nav_3d/mls_planner/utils/plan_rrd.py | 7 +- 12 files changed, 1557 insertions(+), 139 deletions(-) diff --git a/dimos/mapping/ray_tracing/transformer.py b/dimos/mapping/ray_tracing/transformer.py index 847d83a5d3..ddb16f0d99 100644 --- a/dimos/mapping/ray_tracing/transformer.py +++ b/dimos/mapping/ray_tracing/transformer.py @@ -16,6 +16,7 @@ from typing import TYPE_CHECKING, Any +import numpy as np import open3d as o3d # type: ignore[import-untyped] import open3d.core as o3c # type: ignore[import-untyped] @@ -38,6 +39,8 @@ def __init__( voxel_size: float = 0.1, max_range: float = 30.0, emit_every: int = 1, + emit_local: bool = False, + region_percentile: float = 95.0, **mapper_kwargs: Any, ) -> None: if emit_every < 0: @@ -45,16 +48,47 @@ def __init__( self.voxel_size = voxel_size self.max_range = max_range self.emit_every = emit_every + self.emit_local = emit_local + self.region_percentile = region_percentile self._mapper_kwargs = mapper_kwargs + def _robust_bounds( + self, points: np.ndarray, origins: np.ndarray + ) -> tuple[float, float, float, float, float]: + """Robot-centered cylinder sized to a percentile of the observed points, + so a sparse far tail of returns does not inflate the local region. + + The radius covers `region_percentile` of points by xy distance, and the + z band is trimmed to the same percentile to drop ceiling/tall returns. + """ + margin = self._mapper_kwargs.get("shadow_depth", 0.2) + self.voxel_size + cx = float(origins[:, 0].mean()) + cy = float(origins[:, 1].mean()) + dist = np.hypot(points[:, 0] - cx, points[:, 1] - cy) + radius = float(np.percentile(dist, self.region_percentile)) + margin + + lo_pct = 100.0 - self.region_percentile + z_min = float(np.percentile(points[:, 2], lo_pct)) - margin + z_max = float(np.percentile(points[:, 2], self.region_percentile)) + margin + return cx, cy, radius, z_min, z_max + def _make_obs( self, mapper: VoxelRayMapper, last_obs: Observation[PointCloud2], count: int, + batch_points: list[np.ndarray], + batch_origins: list[tuple[float, float, float]], ) -> Observation[PointCloud2]: tags = {**last_obs.tags, "frame_count": count} - positions = mapper.global_map() + if self.emit_local and batch_points: + points = np.concatenate(batch_points, axis=0) + origins = np.asarray(batch_origins, dtype=np.float64) + cx, cy, radius, z_min, z_max = self._robust_bounds(points, origins) + positions = mapper.local_map((cx, cy, 0.0), radius, z_min, z_max) + tags["region_bounds"] = (cx, cy, radius, z_min, z_max) + else: + positions = mapper.global_map() pcd = o3d.t.geometry.PointCloud() pcd.point["positions"] = o3c.Tensor.from_numpy(positions) cloud = PointCloud2(pointcloud=pcd, frame_id="world", ts=last_obs.ts) @@ -69,17 +103,25 @@ def __call__( ) last_obs: Observation[PointCloud2] | None = None count = 0 + batch_points: list[np.ndarray] = [] + batch_origins: list[tuple[float, float, float]] = [] for obs in upstream: if obs.pose_tuple is None: continue x, y, z, *_ = obs.pose_tuple - mapper.add_frame(obs.data.points_f32(), (x, y, z)) + pts = obs.data.points_f32() + mapper.add_frame(pts, (x, y, z)) + if self.emit_local and pts.size: + batch_points.append(pts) + batch_origins.append((x, y, z)) last_obs = obs count += 1 if self.emit_every > 0 and count % self.emit_every == 0: - yield self._make_obs(mapper, last_obs, count) + yield self._make_obs(mapper, last_obs, count, batch_points, batch_origins) + batch_points = [] + batch_origins = [] if last_obs is not None and (self.emit_every == 0 or count % self.emit_every != 0): - yield self._make_obs(mapper, last_obs, count) + yield self._make_obs(mapper, last_obs, count, batch_points, batch_origins) diff --git a/dimos/navigation/nav_3d/mls_planner/mls_planner.pyi b/dimos/navigation/nav_3d/mls_planner/mls_planner.pyi index 5ad17c14e6..6061a522ce 100644 --- a/dimos/navigation/nav_3d/mls_planner/mls_planner.pyi +++ b/dimos/navigation/nav_3d/mls_planner/mls_planner.pyi @@ -33,6 +33,21 @@ class MLSPlanner: """Voxelize the map and rebuild surfaces, nodes, and edges. Shape (N, 3) float32.""" ... + def update_region( + self, + points: NDArray[np.float32], + origin: tuple[float, float], + radius: float, + z_min: float, + z_max: float, + ) -> None: + """Replace the cylindrical region with a local map slice and rebuild. + + Removes cached voxels inside the cylinder, inserts the local map points, + and leaves everything outside untouched. Points are (N, 3) float32. + """ + ... + def surface_map(self) -> NDArray[np.float32]: """Standable surface cells as (M, 3) float32 centers.""" ... @@ -57,6 +72,10 @@ class MLSPlanner: """Number of occupied voxels in the current map.""" ... + def voxel_map(self) -> NDArray[np.float32]: + """Accumulated occupied voxel centers as (N, 3) float32, for visualization.""" + ... + def last_timings(self) -> dict[str, float]: """Per-substep wall-clock ms of the last update_global_map. diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/adjacency.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/adjacency.rs index a4408acae7..f75f1b0179 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/adjacency.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/adjacency.rs @@ -6,7 +6,7 @@ //! Uses a "slot map" to store cells. When inserting, either expand the map //! or reuse a freed location marked with a tombstone. -use ahash::AHashMap; +use ahash::{AHashMap, AHashSet}; use rayon::prelude::*; use crate::voxel::VoxelKey; @@ -24,6 +24,10 @@ const NEIGHBORS_4: [(i32, i32); 4] = [(-1, 0), (1, 0), (0, -1), (0, 1)]; #[derive(Clone, Copy, Debug)] pub struct Edge { pub dest: CellId, + /// Geometric cost, set at build time and never mutated. + pub base_cost: f32, + /// Effective cost = base_cost scaled by the wall-safe penalty. Derived + /// from base_cost each frame so the penalty stays idempotent. pub cost: f32, } @@ -114,9 +118,18 @@ impl SurfaceCells { &self.edges[id as usize] } + #[inline] + pub fn edges_mut(&mut self, id: CellId) -> &mut [Edge] { + &mut self.edges[id as usize] + } + #[cfg(test)] pub fn add_edge(&mut self, src: CellId, dest: CellId, cost: f32) { - self.edges[src as usize].push(Edge { dest, cost }); + self.edges[src as usize].push(Edge { + dest, + base_cost: cost, + cost, + }); } /// Iterate live cells: (id, outgoing edges). @@ -209,12 +222,76 @@ pub fn build_surface_cells( .get(&(ix + dx, iy + dy, nz)) .expect("neighbor cell exists in lookup"); let cost = ((dx * dx + dy * dy + dz * dz) as f32).sqrt() * voxel_size; - local.push(Edge { dest, cost }); + local.push(Edge { + dest, + base_cost: cost, + cost, + }); } } }); } +/// Recompute outgoing edges for every live cell within one step of a changed +/// cell, matching `build_surface_cells` edge logic. Call after surgical +/// insert/remove so the affected region's adjacency matches a full rebuild. +/// +/// `seeds` are the changed (added and removed) cell coordinates. The recompute +/// set is the live cells at those coordinates plus their surface neighbors. +pub fn rebuild_edges_around( + cells: &mut SurfaceCells, + surface_lookup: &SurfaceLookup, + seeds: &[VoxelKey], + voxel_size: f32, + step_threshold_cells: i32, +) { + let mut affected: AHashSet = AHashSet::new(); + for &(ix, iy, iz) in seeds { + if let Some(id) = cells.id((ix, iy, iz)) { + affected.insert(id); + } + for (dx, dy) in NEIGHBORS_4 { + let Some(nzs) = surface_lookup.get(&(ix + dx, iy + dy)) else { + continue; + }; + for &nz in nzs { + if (nz - iz).abs() > step_threshold_cells { + continue; + } + if let Some(id) = cells.id((ix + dx, iy + dy, nz)) { + affected.insert(id); + } + } + } + } + + for id in affected { + let (ix, iy, iz) = cells.coord(id); + let mut edges: Vec = Vec::new(); + for (dx, dy) in NEIGHBORS_4 { + let Some(nzs) = surface_lookup.get(&(ix + dx, iy + dy)) else { + continue; + }; + for &nz in nzs { + let dz = nz - iz; + if dz.abs() > step_threshold_cells { + continue; + } + let dest = cells + .id((ix + dx, iy + dy, nz)) + .expect("neighbor cell exists in lookup"); + let cost = ((dx * dx + dy * dy + dz * dz) as f32).sqrt() * voxel_size; + edges.push(Edge { + dest, + base_cost: cost, + cost, + }); + } + } + cells.edges[id as usize] = edges; + } +} + #[cfg(test)] mod tests { use super::*; diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/dijkstra.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/dijkstra.rs index 59a2890c89..7ecf9b7f7f 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/dijkstra.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/dijkstra.rs @@ -7,7 +7,10 @@ use std::cmp::Ordering; use std::collections::BinaryHeap; +use ahash::AHashSet; + use crate::adjacency::{CellId, SurfaceCells, NO_CELL}; +use crate::voxel::VoxelKey; #[derive(Default)] pub struct DijkstraState { @@ -28,37 +31,126 @@ impl DijkstraState { self.source.resize(n, 0); self.heap.clear(); } + + /// Grow the vecs to hold `n` slots without disturbing existing labels. + /// New slots default to unreached. + fn ensure_capacity(&mut self, n: usize) { + if self.dist.len() < n { + self.dist.resize(n, f32::INFINITY); + self.pred.resize(n, NO_CELL); + self.source.resize(n, 0); + } + } } /// Multi-source dijkstra. /// /// Labels each node with distance to nearest source, the source id, and the path. -pub fn dijkstra(cells: &SurfaceCells, sources: &[CellId], state: &mut DijkstraState) { +/// When `use_base` is set, edges are weighted by their geometric `base_cost` +/// instead of the penalized `cost` (used for the wall-distance field). +pub fn dijkstra( + cells: &SurfaceCells, + sources: &[CellId], + state: &mut DijkstraState, + use_base: bool, +) { state.reset(cells.slot_capacity()); - for (label, &s) in sources.iter().enumerate() { + for &s in sources { if !cells.is_live(s) { continue; } state.dist[s as usize] = 0.0; - state.source[s as usize] = label as u32; - state.heap.push(Scored(0.0, s)); + state.source[s as usize] = s; + state.heap.push(Scored(0.0, cells.coord(s), s)); } - while let Some(Scored(d, u)) = state.heap.pop() { + while let Some(Scored(d, _, u)) = state.heap.pop() { let cur = state.dist[u as usize]; if d > cur { continue; } let su = state.source[u as usize]; for edge in cells.neighbors(u) { - let nd = d + edge.cost; + let nd = d + if use_base { edge.base_cost } else { edge.cost }; let v = edge.dest as usize; if nd < state.dist[v] { state.dist[v] = nd; state.pred[v] = u; state.source[v] = su; - state.heap.push(Scored(nd, edge.dest)); + state + .heap + .push(Scored(nd, cells.coord(edge.dest), edge.dest)); + } + } + } +} + +/// Bounded multi-source Dijkstra that only re-labels cells in `window`, +/// keeping every cached label outside it. +/// +/// Window cells are cleared, then the wavefront is re-seeded two ways: from +/// any sources that lie inside the window, and from the frontier of already +/// labeled cells just outside it. Relaxation never writes a cell outside the +/// window, so the result inside is consistent with the cached labels around it +/// as long as the window margin exceeds the reach of the change. +pub fn dijkstra_region( + cells: &SurfaceCells, + sources: &[CellId], + window: &AHashSet, + state: &mut DijkstraState, + use_base: bool, +) { + state.ensure_capacity(cells.slot_capacity()); + state.heap.clear(); + + for &w in window { + let i = w as usize; + state.dist[i] = f32::INFINITY; + state.pred[i] = NO_CELL; + state.source[i] = 0; + } + + for &s in sources { + if !cells.is_live(s) || !window.contains(&s) { + continue; + } + state.dist[s as usize] = 0.0; + state.source[s as usize] = s; + state.heap.push(Scored(0.0, cells.coord(s), s)); + } + + let mut frontier: AHashSet = AHashSet::new(); + for &w in window { + for edge in cells.neighbors(w) { + let n = edge.dest; + if !window.contains(&n) && state.dist[n as usize].is_finite() { + frontier.insert(n); + } + } + } + for &n in &frontier { + state + .heap + .push(Scored(state.dist[n as usize], cells.coord(n), n)); + } + + while let Some(Scored(d, _, u)) = state.heap.pop() { + if d > state.dist[u as usize] { + continue; + } + let su = state.source[u as usize]; + for edge in cells.neighbors(u) { + let v = edge.dest; + if !window.contains(&v) { + continue; + } + let nd = d + if use_base { edge.base_cost } else { edge.cost }; + if nd < state.dist[v as usize] { + state.dist[v as usize] = nd; + state.pred[v as usize] = u; + state.source[v as usize] = su; + state.heap.push(Scored(nd, cells.coord(v), v)); } } } @@ -70,9 +162,11 @@ pub fn dijkstra(cells: &SurfaceCells, sources: &[CellId], state: &mut DijkstraSt pub fn walk_preds(state: &DijkstraState, start: CellId) -> Vec { let mut cells = vec![start]; let mut cur = start; + let mut seen: AHashSet = AHashSet::new(); + seen.insert(start); loop { let p = state.pred[cur as usize]; - if p == NO_CELL { + if p == NO_CELL || !seen.insert(p) { break; } cur = p; @@ -81,7 +175,7 @@ pub fn walk_preds(state: &DijkstraState, start: CellId) -> Vec { cells } -struct Scored(f32, CellId); +struct Scored(f32, VoxelKey, CellId); impl PartialEq for Scored { fn eq(&self, other: &Self) -> bool { @@ -96,15 +190,104 @@ impl PartialOrd for Scored { } impl Ord for Scored { fn cmp(&self, other: &Self) -> Ordering { - // Order on score, and use cell id for tie-breaker for repeatability - other.0.total_cmp(&self.0).then(self.1.cmp(&other.1)) + // Min-distance pops first; tie-break by coordinate so the result is + // independent of CellId assignment (incremental == full rebuild). + other.0.total_cmp(&self.0).then(other.1.cmp(&self.1)) } } #[cfg(test)] mod tests { use super::*; - use crate::adjacency::SurfaceCells; + use crate::adjacency::{ + build_surface_cells, build_surface_lookup, SurfaceCells, SurfaceLookup, + }; + use crate::voxel::VoxelKey; + + fn grid(n: i32) -> SurfaceCells { + let cells: Vec = (0..n) + .flat_map(|x| (0..n).map(move |y| (x, y, 0))) + .collect(); + let mut lookup = SurfaceLookup::new(); + build_surface_lookup(&cells, &mut lookup); + let mut sc = SurfaceCells::default(); + build_surface_cells(&mut sc, &lookup, 0.1, 2); + sc + } + + fn root_of(state: &DijkstraState, start: CellId) -> CellId { + let mut cur = start; + while state.pred[cur as usize] != NO_CELL { + cur = state.pred[cur as usize]; + } + cur + } + + #[test] + fn region_window_all_equals_full() { + let sc = grid(10); + let sources = [sc.id((0, 0, 0)).unwrap(), sc.id((9, 9, 0)).unwrap()]; + + let mut full = DijkstraState::default(); + dijkstra(&sc, &sources, &mut full, false); + + let window: AHashSet = sc.ids().collect(); + let mut region = DijkstraState::default(); + dijkstra_region(&sc, &sources, &window, &mut region, false); + + for id in sc.ids() { + assert_eq!( + region.dist[id as usize], + full.dist[id as usize], + "dist mismatch at {:?}", + sc.coord(id) + ); + } + } + + #[test] + fn region_partial_window_reproduces_cached_distances() { + let sc = grid(12); + let sources = [sc.id((0, 0, 0)).unwrap(), sc.id((11, 11, 0)).unwrap()]; + + let mut full = DijkstraState::default(); + dijkstra(&sc, &sources, &mut full, false); + + // Seed the regional state with the full result as the cache, then + // recompute an interior block. Nothing changed, so the block must come + // back identical and every cell must still trace to a real source. + let mut region = DijkstraState { + dist: full.dist.clone(), + pred: full.pred.clone(), + source: full.source.clone(), + ..Default::default() + }; + + let window: AHashSet = sc + .ids() + .filter(|&id| { + let (x, y, _) = sc.coord(id); + (3..=8).contains(&x) && (3..=8).contains(&y) + }) + .collect(); + dijkstra_region(&sc, &sources, &window, &mut region, false); + + for &id in &window { + assert_eq!( + region.dist[id as usize], + full.dist[id as usize], + "dist mismatch at {:?}", + sc.coord(id) + ); + let root = root_of(®ion, id); + assert!( + sources.contains(&root), + "cell {:?} traces to non-source {:?}", + sc.coord(id), + sc.coord(root) + ); + } + } fn chain(n: i32) -> (SurfaceCells, Vec) { let mut sc = SurfaceCells::default(); @@ -120,7 +303,7 @@ mod tests { fn single_source_dist_and_pred() { let (sc, ids) = chain(5); let mut st = DijkstraState::default(); - dijkstra(&sc, &[ids[0]], &mut st); + dijkstra(&sc, &[ids[0]], &mut st, false); for (i, &id) in ids.iter().enumerate().take(5) { assert_eq!(st.dist[id as usize], i as f32); assert_eq!(st.source[id as usize], 0); @@ -140,13 +323,13 @@ mod tests { fn multi_source_labels_by_nearest() { let (sc, ids) = chain(5); let mut st = DijkstraState::default(); - dijkstra(&sc, &[ids[0], ids[4]], &mut st); - assert_eq!(st.source[ids[0] as usize], 0); - assert_eq!(st.source[ids[1] as usize], 0); - assert_eq!(st.source[ids[3] as usize], 1); - assert_eq!(st.source[ids[4] as usize], 1); + dijkstra(&sc, &[ids[0], ids[4]], &mut st, false); + assert_eq!(st.source[ids[0] as usize], ids[0]); + assert_eq!(st.source[ids[1] as usize], ids[0]); + assert_eq!(st.source[ids[3] as usize], ids[4]); + assert_eq!(st.source[ids[4] as usize], ids[4]); let s2 = st.source[ids[2] as usize]; - assert!(s2 == 0 || s2 == 1); + assert!(s2 == ids[0] || s2 == ids[4]); assert_eq!(st.dist[ids[0] as usize], 0.0); assert_eq!(st.dist[ids[1] as usize], 1.0); assert_eq!(st.dist[ids[2] as usize], 2.0); @@ -166,7 +349,7 @@ mod tests { sc.add_edge(c, d, 1.0); sc.add_edge(d, c, 1.0); let mut st = DijkstraState::default(); - dijkstra(&sc, &[a], &mut st); + dijkstra(&sc, &[a], &mut st, false); assert_eq!(st.dist[a as usize], 0.0); assert_eq!(st.dist[b as usize], 1.0); assert!(!st.dist[c as usize].is_finite()); @@ -186,7 +369,7 @@ mod tests { sc.add_edge(c, b, 1.0); sc.add_edge(b, c, 1.0); let mut st = DijkstraState::default(); - dijkstra(&sc, &[a], &mut st); + dijkstra(&sc, &[a], &mut st, false); assert_eq!(st.dist[b as usize], 2.0); assert_eq!(st.pred[b as usize], c); } @@ -195,9 +378,9 @@ mod tests { fn buffer_reuse_does_not_leak_prior_state() { let (sc1, ids1) = chain(5); let mut st = DijkstraState::default(); - dijkstra(&sc1, &[ids1[0]], &mut st); + dijkstra(&sc1, &[ids1[0]], &mut st, false); let (sc2, ids2) = chain(3); - dijkstra(&sc2, &[ids2[0]], &mut st); + dijkstra(&sc2, &[ids2[0]], &mut st, false); for (i, &id) in ids2.iter().enumerate().take(3) { assert_eq!(st.dist[id as usize], i as f32); } diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/edges.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/edges.rs index 8884b96af0..260868b8ec 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/edges.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/edges.rs @@ -8,17 +8,19 @@ //! the Voronoi region. We use the boundaries of these regions to build the //! edges between start nodes. -use ahash::AHashMap; +use ahash::{AHashMap, AHashSet}; use rayon::prelude::*; use crate::adjacency::{CellId, Edge, SurfaceCells, SurfaceLookup, NO_CELL}; -use crate::dijkstra::{dijkstra, walk_preds, DijkstraState}; +use crate::dijkstra::{dijkstra, dijkstra_region, walk_preds, DijkstraState}; use crate::nodes::NodeData; use crate::voxel::VoxelKey; -/// Index into planner graph nodes -pub type NodeId = u32; -pub const NO_NODE: NodeId = u32::MAX; +/// A node is identified by the CellId it sits on. This stays stable across +/// incremental updates as long as that cell is untouched, so cached node +/// edges and the Voronoi forest survive a regional rebuild. +pub type NodeId = CellId; +pub const NO_NODE: NodeId = NO_CELL; /// Index into planner graph node edges pub type NodeEdgeIdx = u32; @@ -40,8 +42,13 @@ pub struct PlannerGraph { pub surface_lookup: SurfaceLookup, pub nodes: Vec, pub node_edges: Vec, - pub node_adj: Vec>, + pub node_adj: AHashMap>, + /// Voronoi forest: each cell's nearest node, distance, and predecessor. + /// Read by the planner to expand node edges into surface-cell paths. pub cell_state: DijkstraState, + /// Persistent wall-distance field, kept so a regional node rebuild can + /// reseed its wavefront from cached values outside the dirty window. + pub wall_state: DijkstraState, } impl PlannerGraph { @@ -59,7 +66,7 @@ pub fn build_node_edges( nodes: &[NodeData], state: &mut DijkstraState, out_edges: &mut Vec, - out_adj: &mut Vec>, + out_adj: &mut AHashMap>, ) { out_edges.clear(); out_adj.clear(); @@ -70,18 +77,126 @@ pub fn build_node_edges( } let source_cells: Vec = nodes.iter().map(|n| n.cell_id).collect(); - dijkstra(cells, &source_cells, state); + dijkstra(cells, &source_cells, state, false); best_boundary_edges(cells, state, out_edges); - out_adj.resize_with(nodes.len(), Vec::new); - for v in out_adj.iter_mut() { - v.clear(); + rebuild_node_adj(out_edges, out_adj); +} + +/// Rebuild the per-node edge index from the edge list. +fn rebuild_node_adj(edges: &[NodeEdge], out_adj: &mut AHashMap>) { + out_adj.clear(); + for (edge_idx, edge) in edges.iter().enumerate() { + out_adj + .entry(edge.a) + .or_default() + .push(edge_idx as NodeEdgeIdx); + out_adj + .entry(edge.b) + .or_default() + .push(edge_idx as NodeEdgeIdx); + } +} + +/// Regional counterpart to `build_node_edges`. +/// +/// Recomputes the Voronoi forest only inside `window`, then rebuilds the node +/// edges by keeping cached edges whose boundary lies entirely outside the +/// window and rescanning window cells for fresh boundary crossings. +pub fn build_node_edges_region( + cells: &SurfaceCells, + nodes: &[NodeData], + window: &AHashSet, + state: &mut DijkstraState, + out_edges: &mut Vec, + out_adj: &mut AHashMap>, +) { + let source_cells: Vec = nodes.iter().map(|n| n.cell_id).collect(); + if source_cells.is_empty() { + state.reset(cells.slot_capacity()); + out_edges.clear(); + out_adj.clear(); + return; } - for (edge_idx, edge) in out_edges.iter().enumerate() { - out_adj[edge.a as usize].push(edge_idx as NodeEdgeIdx); - out_adj[edge.b as usize].push(edge_idx as NodeEdgeIdx); + dijkstra_region(cells, &source_cells, window, state, false); + + let live_node: AHashSet = source_cells.iter().copied().collect(); + + let mut merged: AHashMap<(NodeId, NodeId), NodeEdge> = AHashMap::new(); + for e in out_edges.iter() { + if window.contains(&e.boundary_u) || window.contains(&e.boundary_v) { + continue; + } + if !live_node.contains(&e.a) || !live_node.contains(&e.b) { + continue; + } + merged.insert((e.a, e.b), *e); + } + + let win_cells: Vec = window.iter().copied().collect(); + let fresh = win_cells + .par_iter() + .fold( + AHashMap::<(NodeId, NodeId), NodeEdge>::new, + |mut local, &u| { + let du = state.dist[u as usize]; + if du.is_finite() { + let sa = state.source[u as usize]; + for edge in cells.neighbors(u) { + let v = edge.dest; + let dv = state.dist[v as usize]; + if !dv.is_finite() { + continue; + } + let sb = state.source[v as usize]; + if sa == sb { + continue; + } + let cost = du + edge.cost + dv; + let (key_a, key_b, bu, bv) = if sa < sb { + (sa, sb, u, v) + } else { + (sb, sa, v, u) + }; + let entry = local.entry((key_a, key_b)).or_insert(NodeEdge { + a: key_a, + b: key_b, + cost: f32::INFINITY, + boundary_u: NO_CELL, + boundary_v: NO_CELL, + }); + if cost < entry.cost { + entry.cost = cost; + entry.boundary_u = bu; + entry.boundary_v = bv; + } + } + } + local + }, + ) + .reduce(AHashMap::<(NodeId, NodeId), NodeEdge>::new, |mut a, b| { + for (k, v_edge) in b { + let entry = a.entry(k).or_insert(v_edge); + if v_edge.cost < entry.cost { + *entry = v_edge; + } + } + a + }); + + for (k, fe) in fresh { + let entry = merged.entry(k).or_insert(fe); + if fe.cost < entry.cost { + *entry = fe; + } } + + out_edges.clear(); + out_edges.extend(merged.into_values()); + out_edges.par_sort_unstable_by_key(|e| (e.a, e.b)); + rebuild_node_adj(out_edges, out_adj); } fn best_boundary_edges(cells: &SurfaceCells, state: &DijkstraState, out: &mut Vec) { @@ -211,16 +326,25 @@ mod tests { let pg = setup(&strip_cells(), &[(3, 0, 0), (15, 0, 0)]); assert_eq!(pg.node_edges.len(), 1); let e = &pg.node_edges[0]; - assert_eq!((e.a, e.b), (0, 1)); - assert_eq!(pg.node_adj[0], vec![0]); - assert_eq!(pg.node_adj[1], vec![0]); + let a = pg.cells.id((3, 0, 0)).unwrap(); + let b = pg.cells.id((15, 0, 0)).unwrap(); + assert_eq!((e.a.min(e.b), e.a.max(e.b)), (a.min(b), a.max(b))); + assert_eq!(pg.node_adj[&a], vec![0]); + assert_eq!(pg.node_adj[&b], vec![0]); } #[test] fn three_nodes_in_line_form_a_chain() { let pg = setup(&strip_cells(), &[(3, 0, 0), (10, 0, 0), (17, 0, 0)]); + let c = |k| pg.cells.id(k).unwrap(); let pairs: Vec<(NodeId, NodeId)> = pg.node_edges.iter().map(|e| (e.a, e.b)).collect(); - assert_eq!(pairs, vec![(0, 1), (1, 2)]); + assert_eq!( + pairs, + vec![ + (c((3, 0, 0)), c((10, 0, 0))), + (c((10, 0, 0)), c((17, 0, 0))) + ] + ); } #[test] diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs index beb2dcfc4e..74422cfc90 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs @@ -6,14 +6,17 @@ use std::time::Instant; use ahash::AHashSet; +use rayon::prelude::*; use serde::Deserialize; use validator::Validate; -use crate::adjacency::{build_surface_cells, build_surface_lookup}; -use crate::edges::{build_node_edges, PlannerGraph}; -use crate::nodes::place_nodes; +use crate::adjacency::{build_surface_cells, build_surface_lookup, rebuild_edges_around, CellId}; +use crate::edges::{build_node_edges, build_node_edges_region, PlannerGraph}; +use crate::nodes::{place_nodes, place_nodes_region}; use crate::planner; -use crate::surfaces::{extract_surfaces, ColumnIz}; +use crate::surfaces::{ + add_to_by_col, extract_surfaces, extract_surfaces_region, remove_from_by_col, ColumnIz, +}; use crate::voxel::{voxelize, VoxelKey}; #[derive(Debug, Deserialize, Validate)] @@ -44,12 +47,46 @@ pub struct RebuildTimings { pub graph_ms: f64, } +/// Cylindrical dirty region the planner re-derives from a local map slice. +/// +/// The xy test matches the ray tracer's LocalBounds so that removing the +/// region and re-inserting the local map points is an exact replacement. +pub struct RegionBounds { + pub origin_x: f32, + pub origin_y: f32, + pub radius: f32, + pub z_min: f32, + pub z_max: f32, +} + +impl RegionBounds { + fn contains_voxel(&self, (kx, ky, kz): VoxelKey, voxel_size: f32) -> bool { + let half = voxel_size * 0.5; + let z = kz as f32 * voxel_size + half; + if z < self.z_min || z > self.z_max { + return false; + } + let dx = kx as f32 * voxel_size + half - self.origin_x; + let dy = ky as f32 * voxel_size + half - self.origin_y; + dx * dx + dy * dy <= self.radius * self.radius + } + + /// Inclusive voxel-column bounding box of the cylinder in the xy plane. + fn column_bbox(&self, voxel_size: f32) -> (i32, i32, i32, i32) { + let inv = 1.0 / voxel_size; + let x0 = ((self.origin_x - self.radius) * inv).floor() as i32; + let x1 = ((self.origin_x + self.radius) * inv).floor() as i32; + let y0 = ((self.origin_y - self.radius) * inv).floor() as i32; + let y1 = ((self.origin_y + self.radius) * inv).floor() as i32; + (x0, x1, y0, y1) + } +} + #[derive(Default)] pub struct Planner { graph: PlannerGraph, voxel_map: AHashSet, by_col: ColumnIz, - surface: Vec, last_timings: RebuildTimings, } @@ -57,7 +94,6 @@ impl Planner { pub fn update_global_map(&mut self, points: &[(f32, f32, f32)], config: &Config) { let voxel_size = config.voxel_size; let clearance = (config.robot_height / voxel_size).ceil() as i32; - let step = (config.node_step_threshold_m / voxel_size).floor() as i32; let t_voxelize = Instant::now(); self.voxel_map.clear(); @@ -67,31 +103,289 @@ impl Planner { let voxelize_ms = ms(t_voxelize.elapsed()); let t_surfaces = Instant::now(); + let mut surface: Vec = Vec::new(); extract_surfaces( &self.voxel_map, clearance, config.surface_dilation_passes, config.surface_erosion_passes, &mut self.by_col, - &mut self.surface, + &mut surface, ); + build_surface_lookup(&surface, &mut self.graph.surface_lookup); let surfaces_ms = ms(t_surfaces.elapsed()); + let graph_ms = self.rebuild_graph(config); + + self.last_timings = RebuildTimings { + voxelize_ms, + surfaces_ms, + graph_ms, + }; + } + + /// Re-derive the planner from a cylindrical slice of the map. + /// + /// Replaces the cached voxels inside the cylinder with the local map slice, + /// then recomputes surfaces and the graph only where voxels actually + /// changed (plus a correctness margin), leaving everything else cached. The + /// cylinder bounds what can change. The realized change is usually far + /// smaller, so the work each frame scales with the change, not the map. + pub fn update_region( + &mut self, + local_points: &[(f32, f32, f32)], + bounds: &RegionBounds, + config: &Config, + ) { + let voxel_size = config.voxel_size; + let clearance = (config.robot_height / voxel_size).ceil() as i32; + let pad = (config.surface_dilation_passes + config.surface_erosion_passes) as i32; + + let t_voxelize = Instant::now(); + let changed = self.replace_region_voxels(local_points, bounds, voxel_size); + let voxelize_ms = ms(t_voxelize.elapsed()); + + // No voxel changed, so surfaces and the graph are untouched. + let Some((bx0, bx1, by0, by1)) = changed else { + self.last_timings = RebuildTimings { + voxelize_ms, + surfaces_ms: 0.0, + graph_ms: 0.0, + }; + return; + }; + + let t_surfaces = Instant::now(); + // A changed voxel column only shifts surfaces within `pad` (morphology + // reach), so the write-back box is the changed-column bbox grown by pad. + let write = (bx0 - pad, bx1 + pad, by0 - pad, by1 + pad); + let new_cells = extract_surfaces_region( + &self.by_col, + clearance, + config.surface_dilation_passes, + config.surface_erosion_passes, + write, + ); + let (added, removed) = self.replace_surface_region(write, &new_cells); + let surfaces_ms = ms(t_surfaces.elapsed()); + + let t_graph = Instant::now(); + let step = (config.node_step_threshold_m / voxel_size).floor() as i32; + for &c in &removed { + self.graph.cells.remove(c); + } + for &c in &added { + self.graph.cells.insert(c); + } + let mut seeds = added; + seeds.extend_from_slice(&removed); + + // No surface cell changed, so nodes and edges are untouched. + if !seeds.is_empty() { + rebuild_edges_around( + &mut self.graph.cells, + &self.graph.surface_lookup, + &seeds, + voxel_size, + step, + ); + + let window = self.node_window(&seeds, config); + place_nodes_region( + &mut self.graph.cells, + &window, + config.voxel_size, + config.node_spacing_m, + config.node_wall_buffer_m, + &mut self.graph.wall_state, + &mut self.graph.nodes, + ); + build_node_edges_region( + &self.graph.cells, + &self.graph.nodes, + &window, + &mut self.graph.cell_state, + &mut self.graph.node_edges, + &mut self.graph.node_adj, + ); + } + let graph_ms = ms(t_graph.elapsed()); + + self.last_timings = RebuildTimings { + voxelize_ms, + surfaces_ms, + graph_ms, + }; + } + + /// Replace the voxels inside the cylinder with the local map points, + /// keeping the per-column index in sync. Returns the inclusive column bbox + /// of voxels that actually changed (added or removed), or None if nothing + /// changed, so downstream work can be scoped to the realized change. + /// + /// Work is bounded by the cylinder region: the cached voxels inside it are + /// enumerated through the per-column index over the cylinder's column bbox, + /// never by scanning the whole map. + fn replace_region_voxels( + &mut self, + local_points: &[(f32, f32, f32)], + bounds: &RegionBounds, + voxel_size: f32, + ) -> Option<(i32, i32, i32, i32)> { + let new_set: AHashSet = local_points + .iter() + .map(|&p| voxelize(p, voxel_size)) + .collect(); + + let (x0, x1, y0, y1) = bounds.column_bbox(voxel_size); + let by_col = &self.by_col; + let stale: Vec = (x0..(x1 + 1)) + .into_par_iter() + .flat_map_iter(|ix| { + let mut local: Vec = Vec::new(); + for iy in y0..=y1 { + let Some(zs) = by_col.get(&(ix, iy)) else { + continue; + }; + for &iz in zs { + let k = (ix, iy, iz); + if bounds.contains_voxel(k, voxel_size) && !new_set.contains(&k) { + local.push(k); + } + } + } + local + }) + .collect(); + + let mut bb = ChangeBounds::new(); + for &k in &stale { + bb.add(k.0, k.1); + self.voxel_map.remove(&k); + remove_from_by_col(&mut self.by_col, k); + } + for &k in &new_set { + if self.voxel_map.insert(k) { + bb.add(k.0, k.1); + add_to_by_col(&mut self.by_col, k); + } + } + bb.bounds() + } + + /// Replace the surface_lookup entries for columns in the `write` box with + /// freshly extracted cells (which all lie within the box). Returns the + /// (added, removed) surface cells so the graph can be patched surgically. + fn replace_surface_region( + &mut self, + write: (i32, i32, i32, i32), + new_cells: &[VoxelKey], + ) -> (Vec, Vec) { + let (x0, x1, y0, y1) = write; + let mut old: AHashSet = AHashSet::new(); + for ix in x0..=x1 { + for iy in y0..=y1 { + if let Some(zs) = self.graph.surface_lookup.remove(&(ix, iy)) { + for iz in zs { + old.insert((ix, iy, iz)); + } + } + } + } + let new: AHashSet = new_cells.iter().copied().collect(); + + for &(ix, iy, iz) in new_cells { + self.graph + .surface_lookup + .entry((ix, iy)) + .or_default() + .push(iz); + } + for &(ix, iy, _) in new_cells { + if let Some(zs) = self.graph.surface_lookup.get_mut(&(ix, iy)) { + zs.sort_unstable(); + zs.dedup(); + } + } + + let added: Vec = new.iter().filter(|c| !old.contains(c)).copied().collect(); + let removed: Vec = old.iter().filter(|c| !new.contains(c)).copied().collect(); + (added, removed) + } + + /// Rebuild all cells from surface_lookup, then nodes and edges. + /// Returns graph_ms. + fn rebuild_graph(&mut self, config: &Config) -> f64 { + let voxel_size = config.voxel_size; + let step = (config.node_step_threshold_m / voxel_size).floor() as i32; + let t_graph = Instant::now(); - build_surface_lookup(&self.surface, &mut self.graph.surface_lookup); build_surface_cells( &mut self.graph.cells, &self.graph.surface_lookup, voxel_size, step, ); + self.rebuild_nodes(config); + ms(t_graph.elapsed()) + } + + /// Live cells within the bbox of the changed surface cells, grown by the + /// node-graph margin. The margin must exceed the reach of any label change + /// (surface morphology pad, the wall-distance buffer, and the node spacing) + /// so cached nodes, edges, and the Voronoi forest outside the window stay + /// valid and identical to a full rebuild. + fn node_window(&self, changed: &[VoxelKey], config: &Config) -> AHashSet { + let voxel_size = config.voxel_size; + let pad = (config.surface_dilation_passes + config.surface_erosion_passes) as i32; + let buffer_cells = (config.node_wall_buffer_m / voxel_size).ceil() as i32; + let spacing_cells = (config.node_spacing_m / voxel_size).ceil() as i32; + let margin = pad + buffer_cells + spacing_cells + 2; + let mut bb = ChangeBounds::new(); + for &(ix, iy, _) in changed { + bb.add(ix, iy); + } + let Some((min_x, max_x, min_y, max_y)) = bb.bounds() else { + return AHashSet::new(); + }; + let (x0, x1, y0, y1) = ( + min_x - margin, + max_x + margin, + min_y - margin, + max_y + margin, + ); + + let lookup = &self.graph.surface_lookup; + let cells = &self.graph.cells; + let ids: Vec = (x0..(x1 + 1)) + .into_par_iter() + .flat_map_iter(|ix| { + let mut local: Vec = Vec::new(); + for iy in y0..=y1 { + let Some(zs) = lookup.get(&(ix, iy)) else { + continue; + }; + for &iz in zs { + if let Some(id) = cells.id((ix, iy, iz)) { + local.push(id); + } + } + } + local + }) + .collect(); + ids.into_iter().collect() + } + + /// Place nodes and build node edges from the current cells (full rebuild). + fn rebuild_nodes(&mut self, config: &Config) { place_nodes( &mut self.graph.cells, - voxel_size, + config.voxel_size, config.node_spacing_m, config.node_wall_buffer_m, - &mut self.graph.cell_state, + &mut self.graph.wall_state, &mut self.graph.nodes, ); @@ -102,13 +396,6 @@ impl Planner { &mut self.graph.node_edges, &mut self.graph.node_adj, ); - let graph_ms = ms(t_graph.elapsed()); - - self.last_timings = RebuildTimings { - voxelize_ms, - surfaces_ms, - graph_ms, - }; } pub fn plan( @@ -135,14 +422,24 @@ impl Planner { &self.graph } - pub fn surface(&self) -> &[VoxelKey] { - &self.surface + pub fn surface(&self) -> Vec { + let mut out: Vec = Vec::new(); + for (&(ix, iy), zs) in &self.graph.surface_lookup { + for &iz in zs { + out.push((ix, iy, iz)); + } + } + out } pub fn voxel_count(&self) -> usize { self.voxel_map.len() } + pub fn voxel_keys(&self) -> impl Iterator + '_ { + self.voxel_map.iter().copied() + } + pub fn last_timings(&self) -> RebuildTimings { self.last_timings } @@ -151,3 +448,410 @@ impl Planner { fn ms(d: std::time::Duration) -> f64 { d.as_secs_f64() * 1000.0 } + +/// Running inclusive xy bounding box of changed columns. +struct ChangeBounds { + min_x: i32, + max_x: i32, + min_y: i32, + max_y: i32, + any: bool, +} + +impl ChangeBounds { + fn new() -> Self { + Self { + min_x: i32::MAX, + max_x: i32::MIN, + min_y: i32::MAX, + max_y: i32::MIN, + any: false, + } + } + + fn add(&mut self, ix: i32, iy: i32) { + self.any = true; + self.min_x = self.min_x.min(ix); + self.max_x = self.max_x.max(ix); + self.min_y = self.min_y.min(iy); + self.max_y = self.max_y.max(iy); + } + + fn bounds(&self) -> Option<(i32, i32, i32, i32)> { + self.any + .then_some((self.min_x, self.max_x, self.min_y, self.max_y)) + } +} + +#[cfg(test)] +mod region_tests { + use super::*; + use std::collections::{BTreeMap, BTreeSet}; + + fn test_config() -> Config { + Config { + world_frame: String::new(), + voxel_size: 0.1, + robot_height: 0.5, + surface_dilation_passes: 3, + surface_erosion_passes: 3, + node_spacing_m: 1.0, + node_wall_buffer_m: 0.3, + node_step_threshold_m: 0.25, + } + } + + /// Floor slab with a wall down the middle, as world-frame point centers. + fn world_points() -> Vec<(f32, f32, f32)> { + let vs = 0.1_f32; + let half = vs * 0.5; + let mut pts = Vec::new(); + for ix in 0..40 { + for iy in 0..40 { + pts.push((ix as f32 * vs + half, iy as f32 * vs + half, half)); + } + } + // a wall column from z=0 up, to create wall-adjacency for nodes + for iy in 0..40 { + for iz in 0..15 { + pts.push(( + 20.0 * vs + half, + iy as f32 * vs + half, + iz as f32 * vs + half, + )); + } + } + pts + } + + fn surface_set(p: &Planner) -> BTreeSet { + p.surface().into_iter().collect() + } + + fn voxel_set(p: &Planner) -> BTreeSet { + p.voxel_map.iter().copied().collect() + } + + /// Cell adjacency keyed by coordinate (CellId-independent). + fn cell_edges(p: &Planner) -> BTreeMap> { + let cells = &p.graph.cells; + let mut out: BTreeMap> = BTreeMap::new(); + for (id, edges) in cells.iter() { + let src = cells.coord(id); + let set = out.entry(src).or_default(); + for e in edges { + set.insert((cells.coord(e.dest), e.cost.to_bits())); + } + } + out + } + + fn node_coords(p: &Planner) -> BTreeSet { + p.graph + .nodes + .iter() + .map(|n| p.graph.cells.coord(n.cell_id)) + .collect() + } + + fn node_edge_pairs(p: &Planner) -> BTreeSet<(VoxelKey, VoxelKey, u32)> { + let cells = &p.graph.cells; + p.graph + .node_edges + .iter() + .map(|e| { + let a = cells.coord(e.a); + let b = cells.coord(e.b); + let (lo, hi) = if a <= b { (a, b) } else { (b, a) }; + (lo, hi, e.cost.to_bits()) + }) + .collect() + } + + #[test] + fn region_update_matches_full_rebuild() { + let cfg = test_config(); + let bounds = RegionBounds { + origin_x: 2.0, + origin_y: 2.0, + radius: 1.0, + z_min: -1.0, + z_max: 2.0, + }; + let all = world_points(); + + // Full rebuild over the whole map. + let mut full = Planner::default(); + full.update_global_map(&all, &cfg); + + // Region path: build everything outside the cylinder, then patch the + // cylinder in from a local slice. + let inside: Vec<_> = all + .iter() + .copied() + .filter(|&p| bounds.contains_voxel(voxelize(p, cfg.voxel_size), cfg.voxel_size)) + .collect(); + let outside: Vec<_> = all + .iter() + .copied() + .filter(|&p| !bounds.contains_voxel(voxelize(p, cfg.voxel_size), cfg.voxel_size)) + .collect(); + assert!(!inside.is_empty(), "cylinder should cover some voxels"); + + let mut region = Planner::default(); + region.update_global_map(&outside, &cfg); + region.update_region(&inside, &bounds, &cfg); + + assert_eq!(voxel_set(®ion), voxel_set(&full), "voxel map mismatch"); + assert_eq!(surface_set(®ion), surface_set(&full), "surface mismatch"); + assert_eq!( + cell_edges(®ion), + cell_edges(&full), + "cell edges mismatch" + ); + assert_eq!( + node_coords(®ion), + node_coords(&full), + "node set mismatch" + ); + assert_eq!( + node_edge_pairs(®ion), + node_edge_pairs(&full), + "node edge mismatch" + ); + } + + #[test] + fn region_update_removes_stale_voxels() { + let cfg = test_config(); + let bounds = RegionBounds { + origin_x: 2.0, + origin_y: 2.0, + radius: 1.0, + z_min: -1.0, + z_max: 2.0, + }; + let all = world_points(); + + let mut full = Planner::default(); + full.update_global_map(&all, &cfg); + + let inside: Vec<_> = all + .iter() + .copied() + .filter(|&p| bounds.contains_voxel(voxelize(p, cfg.voxel_size), cfg.voxel_size)) + .collect(); + let outside: Vec<_> = all + .iter() + .copied() + .filter(|&p| !bounds.contains_voxel(voxelize(p, cfg.voxel_size), cfg.voxel_size)) + .collect(); + + // Seed the cylinder with a stack of junk voxels not present in `all`, + // so update_region must clear them (and the surface they induce). + let mut seeded = outside.clone(); + for iz in 3..8 { + seeded.push((2.05, 2.05, iz as f32 * cfg.voxel_size + 0.05)); + } + let mut region = Planner::default(); + region.update_global_map(&seeded, &cfg); + region.update_region(&inside, &bounds, &cfg); + + assert_eq!(voxel_set(®ion), voxel_set(&full), "voxel mismatch"); + assert_eq!(surface_set(®ion), surface_set(&full), "surface mismatch"); + assert_eq!( + cell_edges(®ion), + cell_edges(&full), + "cell edges mismatch" + ); + assert_eq!(node_coords(®ion), node_coords(&full), "node mismatch"); + assert_eq!( + node_edge_pairs(®ion), + node_edge_pairs(&full), + "node edge mismatch" + ); + } + + /// Floor 8m x 8m with a wall at x=4m that only a gap at y in [3.5, 4.5] + /// passes through, so crossing the wall is a non-trivial route. + fn big_world() -> Vec<(f32, f32, f32)> { + let vs = 0.1_f32; + let half = vs * 0.5; + let mut pts = Vec::new(); + for ix in 0..80 { + for iy in 0..80 { + pts.push((ix as f32 * vs + half, iy as f32 * vs + half, half)); + } + } + for iy in 0..80 { + if (35..45).contains(&iy) { + continue; + } + for iz in 0..15 { + pts.push(( + 40.0 * vs + half, + iy as f32 * vs + half, + iz as f32 * vs + half, + )); + } + } + pts + } + + fn slice(all: &[(f32, f32, f32)], b: &RegionBounds, vs: f32) -> Vec<(f32, f32, f32)> { + all.iter() + .copied() + .filter(|&p| b.contains_voxel(voxelize(p, vs), vs)) + .collect() + } + + fn path_len(w: &[(f32, f32, f32)]) -> f32 { + w.windows(2) + .map(|p| { + let dx = p[1].0 - p[0].0; + let dy = p[1].1 - p[0].1; + let dz = p[1].2 - p[0].2; + (dx * dx + dy * dy + dz * dz).sqrt() + }) + .sum() + } + + type Pose = (f32, f32, f32); + const PLAN_PAIRS: [(Pose, Pose); 4] = [ + ((0.5, 0.5, 0.05), (7.5, 7.5, 0.05)), + ((0.5, 7.5, 0.05), (7.5, 0.5, 0.05)), + ((0.5, 0.5, 0.05), (0.5, 7.5, 0.05)), + ((7.5, 0.5, 0.05), (7.5, 7.5, 0.05)), + ]; + + fn assert_plans_equivalent(full: &Planner, region: &Planner, cfg: &Config) { + for (s, g) in PLAN_PAIRS { + let pf = full.plan(s, g, cfg); + let pr = region.plan(s, g, cfg); + assert_eq!( + pf.is_some(), + pr.is_some(), + "path existence differs for {s:?} -> {g:?}" + ); + if let (Some(pf), Some(pr)) = (pf, pr) { + let (lf, lr) = (path_len(&pf), path_len(&pr)); + assert!(lr <= lf * 1.6 + 0.5, "region path too long: {lr} vs {lf}"); + assert!(lf <= lr * 1.6 + 0.5, "full path too long: {lf} vs {lr}"); + } + } + } + + /// Re-observing local cylinders on a fully built map must not change the + /// geometry and must keep planning equivalent. The window here is far + /// smaller than the map, so this exercises the cached frontier. + #[test] + fn region_reobserve_preserves_planning() { + let cfg = test_config(); + let all = big_world(); + let vs = cfg.voxel_size; + + let mut full = Planner::default(); + full.update_global_map(&all, &cfg); + + let mut region = Planner::default(); + region.update_global_map(&all, &cfg); + + for &(cx, cy) in &[(2.0, 2.0), (4.0, 4.0), (4.0, 6.0), (6.0, 3.0), (1.5, 7.0)] { + let b = RegionBounds { + origin_x: cx, + origin_y: cy, + radius: 1.2, + z_min: -1.0, + z_max: 2.0, + }; + region.update_region(&slice(&all, &b, vs), &b, &cfg); + } + + assert_eq!(voxel_set(®ion), voxel_set(&full), "voxel set changed"); + assert_eq!(surface_set(®ion), surface_set(&full), "surface changed"); + assert_plans_equivalent(&full, ®ion, &cfg); + } + + /// Re-observing the same geometry must change nothing: no voxel, surface, + /// cell, node, or edge moves. This is the anti-jitter guarantee, far nodes + /// stay put when their region is re-seen, matching a full rebuild. + #[test] + fn region_reobserve_leaves_graph_bit_identical() { + let cfg = test_config(); + let all = big_world(); + let vs = cfg.voxel_size; + + let mut p = Planner::default(); + p.update_global_map(&all, &cfg); + let before_cells = cell_edges(&p); + let before_nodes = node_coords(&p); + let before_edges = node_edge_pairs(&p); + + for &(cx, cy) in &[(2.0, 2.0), (4.0, 4.0), (6.0, 3.0), (1.5, 7.0), (7.0, 7.0)] { + let b = RegionBounds { + origin_x: cx, + origin_y: cy, + radius: 1.2, + z_min: -1.0, + z_max: 2.0, + }; + p.update_region(&slice(&all, &b, vs), &b, &cfg); + } + + assert_eq!( + cell_edges(&p), + before_cells, + "cells changed on re-observation" + ); + assert_eq!( + node_coords(&p), + before_nodes, + "nodes moved on re-observation" + ); + assert_eq!( + node_edge_pairs(&p), + before_edges, + "edges changed on re-observation" + ); + } + + /// Build the planner purely from streamed local cylinders, as the live + /// pipeline does, and require equivalent planning to a one-shot full build. + #[test] + fn region_stream_only_plans_like_full() { + let cfg = test_config(); + let all = big_world(); + let vs = cfg.voxel_size; + + let mut full = Planner::default(); + full.update_global_map(&all, &cfg); + + let mut region = Planner::default(); + let mut cx = 0.5; + while cx <= 7.5 { + let mut cy = 0.5; + while cy <= 7.5 { + let b = RegionBounds { + origin_x: cx, + origin_y: cy, + radius: 1.5, + z_min: -1.0, + z_max: 2.0, + }; + let s = slice(&all, &b, vs); + if !s.is_empty() { + region.update_region(&s, &b, &cfg); + } + cy += 1.0; + } + cx += 1.0; + } + + assert_eq!( + voxel_set(®ion), + voxel_set(&full), + "stream did not reconstruct the map" + ); + assert_plans_equivalent(&full, ®ion, &cfg); + } +} diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/nodes.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/nodes.rs index 50c0463763..51b6df8476 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/nodes.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/nodes.rs @@ -5,11 +5,11 @@ //! nodes at local maxima via NMS, and rescale cell-edge costs to push paths //! toward corridor centers. -use ahash::AHashMap; +use ahash::{AHashMap, AHashSet}; use rayon::prelude::*; use crate::adjacency::{CellId, Edge, SurfaceCells}; -use crate::dijkstra::{dijkstra, DijkstraState}; +use crate::dijkstra::{dijkstra, dijkstra_region, DijkstraState}; use crate::voxel::{surface_point_xyz, VoxelKey}; #[derive(Clone, Copy, Debug)] @@ -37,7 +37,7 @@ pub fn place_nodes( let mut wall_seeds: Vec = Vec::new(); collect_wall_adjacent_cells(cells, &mut wall_seeds); - dijkstra(cells, &wall_seeds, state); + dijkstra(cells, &wall_seeds, state, true); let mut candidates: Vec = cells .ids() @@ -46,10 +46,10 @@ pub fn place_nodes( candidates.par_sort_unstable_by(|&a, &b| { state.dist[b as usize] .total_cmp(&state.dist[a as usize]) - .then(a.cmp(&b)) + .then(cells.coord(a).cmp(&cells.coord(b))) }); - let survivors = nms_grid(cells, &candidates, voxel_size, node_spacing_m); + let survivors = nms_grid(cells, &candidates, &[], voxel_size, node_spacing_m); out_nodes.reserve(survivors.len()); for &id in &survivors { @@ -63,27 +63,111 @@ pub fn place_nodes( apply_wall_safe_penalty(cells, &state.dist, node_wall_buffer_m); } -/// Cells missing any of their 4 xy-direction neighbors are treated as -/// boundaries. Direction membership is tracked with a 4-bit mask so the -/// 349k-cell case avoids per-cell hashset allocation. -fn collect_wall_adjacent_cells(cells: &SurfaceCells, out: &mut Vec) { +/// Regional counterpart to `place_nodes`. +/// +/// Recomputes the wall-distance field and node placement only inside the dirty +/// `window`, keeping cached nodes outside it. Cached nodes are fed to NMS as +/// seeds so freshly placed nodes respect spacing across the window seam. +pub fn place_nodes_region( + cells: &mut SurfaceCells, + window: &AHashSet, + voxel_size: f32, + node_spacing_m: f32, + node_wall_buffer_m: f32, + wall_state: &mut DijkstraState, + nodes: &mut Vec, +) { + let mut wall_seeds: Vec = Vec::new(); + collect_wall_adjacent_in_window(cells, window, &mut wall_seeds); + dijkstra_region(cells, &wall_seeds, window, wall_state, true); + + nodes.retain(|n| cells.is_live(n.cell_id) && !window.contains(&n.cell_id)); + let kept: Vec = nodes.iter().map(|n| n.cell_id).collect(); + + let mut candidates: Vec = window + .iter() + .copied() + .filter(|&id| cells.is_live(id) && wall_state.dist[id as usize] >= node_wall_buffer_m) + .collect(); + candidates.par_sort_unstable_by(|&a, &b| { + wall_state.dist[b as usize] + .total_cmp(&wall_state.dist[a as usize]) + .then(cells.coord(a).cmp(&cells.coord(b))) + }); + + let survivors = nms_grid(cells, &candidates, &kept, voxel_size, node_spacing_m); + nodes.reserve(survivors.len()); + for &id in &survivors { + let (ix, iy, iz) = cells.coord(id); + nodes.push(NodeData { + cell_id: id, + pos: surface_point_xyz(ix, iy, iz, voxel_size), + }); + } + + apply_wall_safe_penalty_region(cells, &wall_state.dist, node_wall_buffer_m, window); +} + +/// Wall-adjacency over a cell subset, matching `collect_wall_adjacent_cells`. +fn collect_wall_adjacent_in_window( + cells: &SurfaceCells, + window: &AHashSet, + out: &mut Vec, +) { out.clear(); - for (id, edges) in cells.iter() { - let (cx, cy, _) = cells.coord(id); - - // Check if all 4 neighbors are present - let mut mask: u8 = 0; - for e in edges { - let (nx, ny, _) = cells.coord(e.dest); - mask |= match (nx - cx, ny - cy) { - (-1, 0) => 1, - (1, 0) => 2, - (0, -1) => 4, - (0, 1) => 8, - _ => 0, - }; + for &id in window { + if cells.is_live(id) && is_wall_adjacent(cells, id) { + out.push(id); + } + } +} + +/// A cell is wall-adjacent when it is missing at least one of its 4 xy-direction +/// neighbors. Membership is tracked with a 4-bit mask to avoid per-cell +/// allocation on the 349k-cell case. +fn is_wall_adjacent(cells: &SurfaceCells, id: CellId) -> bool { + let (cx, cy, _) = cells.coord(id); + let mut mask: u8 = 0; + for e in cells.neighbors(id) { + let (nx, ny, _) = cells.coord(e.dest); + mask |= match (nx - cx, ny - cy) { + (-1, 0) => 1, + (1, 0) => 2, + (0, -1) => 4, + (0, 1) => 8, + _ => 0, + }; + } + mask != 0b1111 +} + +/// Re-scale edge costs for cells whose wall distance changed. The changed set +/// is the window plus its immediate neighbors, since an edge cost depends on +/// the wall distance at both endpoints. Idempotent via `base_cost`. +fn apply_wall_safe_penalty_region( + cells: &mut SurfaceCells, + dist: &[f32], + buffer_m: f32, + window: &AHashSet, +) { + let mut affected: AHashSet = AHashSet::with_capacity(window.len() * 2); + for &w in window { + affected.insert(w); + for e in cells.neighbors(w) { + affected.insert(e.dest); } - if mask != 0b1111 { + } + for id in affected { + scale_edges(cells.edges_mut(id), id, dist, buffer_m); + } +} + +/// Wall-adjacent cells over the whole graph. Falls back to a single cell so a +/// fully-enclosed map still seeds the wall-distance field. +fn collect_wall_adjacent_cells(cells: &SurfaceCells, out: &mut Vec) { + out.clear(); + for id in cells.ids() { + if is_wall_adjacent(cells, id) { out.push(id); } } @@ -95,9 +179,14 @@ fn collect_wall_adjacent_cells(cells: &SurfaceCells, out: &mut Vec) { } /// Space out nodes based on minimum distance. +/// +/// `seeds` are pre-existing nodes that suppress nearby candidates without +/// themselves being emitted, used to keep a regional re-placement consistent +/// with cached nodes just outside the window. fn nms_grid( cells: &SurfaceCells, candidates_sorted: &[CellId], + seeds: &[CellId], voxel_size: f32, node_spacing_m: f32, ) -> Vec { @@ -113,6 +202,9 @@ fn nms_grid( }; let mut bins: AHashMap<(i32, i32, i32), Vec> = AHashMap::new(); + for &s in seeds { + bins.entry(bin_of(cells.coord(s))).or_default().push(s); + } let mut survivors: Vec = Vec::new(); for &id in candidates_sorted { let coord = cells.coord(id); @@ -150,14 +242,21 @@ fn nms_grid( fn apply_wall_safe_penalty(cells: &mut SurfaceCells, dist: &[f32], buffer_m: f32) { let mut edge_lists: Vec<(CellId, &mut Vec)> = cells.iter_edges_mut().collect(); edge_lists.par_iter_mut().for_each(|(src, edges)| { - let pu = penalty_of(dist[*src as usize], buffer_m); - for edge in edges.iter_mut() { - let pv = penalty_of(dist[edge.dest as usize], buffer_m); - edge.cost *= (pu + pv) / 2.0; - } + scale_edges(edges, *src, dist, buffer_m); }); } +/// Rescale one cell's outgoing edges from their geometric `base_cost`. Reading +/// from `base_cost` keeps this idempotent, so a regional repass cannot compound. +#[inline] +fn scale_edges(edges: &mut [Edge], src: CellId, dist: &[f32], buffer_m: f32) { + let pu = penalty_of(dist[src as usize], buffer_m); + for edge in edges.iter_mut() { + let pv = penalty_of(dist[edge.dest as usize], buffer_m); + edge.cost = edge.base_cost * (pu + pv) / 2.0; + } +} + #[inline] fn penalty_of(d: f32, buffer_m: f32) -> f32 { (1.0 + (buffer_m - d) / buffer_m).max(1.0) diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs index 85427ea918..7258366e56 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs @@ -4,7 +4,7 @@ use std::cmp::Ordering; use std::collections::BinaryHeap; -use ahash::AHashMap; +use ahash::{AHashMap, AHashSet}; use crate::adjacency::{CellId, SurfaceCells, SurfaceLookup}; use crate::dijkstra::walk_preds; @@ -91,15 +91,13 @@ pub fn plan( let start_cell = plg.cells.id(start_coord)?; let goal_cell = plg.cells.id(goal_coord)?; - let node_idx_by_cell: AHashMap = plg - .nodes - .iter() - .enumerate() - .map(|(i, n)| (n.cell_id, i as NodeId)) - .collect(); + let node_cells: AHashSet = plg.nodes.iter().map(|n| n.cell_id).collect(); let goal_segment = walk_preds(&plg.cell_state, goal_cell); - let goal_node = *node_idx_by_cell.get(goal_segment.last()?)?; + let goal_node = *goal_segment.last()?; + if !node_cells.contains(&goal_node) { + return None; + } // Rooted at the goal so one pass covers every node's cost-to-go. let (cost_to_go, pred_to_goal) = node_dijkstra(plg, goal_node); @@ -111,7 +109,7 @@ pub fn plan( goal_node, &cost_to_go, &pred_to_goal, - &node_idx_by_cell, + &node_cells, radius, )?; @@ -131,35 +129,40 @@ fn select_entry( plg: &PlannerGraph, start_cell: CellId, goal_node: NodeId, - cost_to_go: &[f32], - pred_to_goal: &[NodeId], - node_idx_by_cell: &AHashMap, + cost_to_go: &AHashMap, + pred_to_goal: &AHashMap, + node_cells: &AHashSet, radius_m: f32, ) -> Option<(Vec, Vec)> { let (connect_dist, connect_pred) = robot_search(&plg.cells, start_cell, radius_m); let mut entry_node = NO_NODE; let mut best_score = f32::INFINITY; - for (i, node) in plg.nodes.iter().enumerate() { + for node in &plg.nodes { let Some(&connect) = connect_dist.get(&node.cell_id) else { continue; }; - let score = connect + cost_to_go[i]; + let Some(&ctg) = cost_to_go.get(&node.cell_id) else { + continue; + }; + let score = connect + ctg; if score < best_score { best_score = score; - entry_node = i as NodeId; + entry_node = node.cell_id; } } if best_score.is_finite() { - let mut lead = walk_local_preds(&connect_pred, plg.nodes[entry_node as usize].cell_id); + let mut lead = walk_local_preds(&connect_pred, entry_node); lead.reverse(); return Some((lead, follow_preds(entry_node, goal_node, pred_to_goal)?)); } let start_segment = walk_preds(&plg.cell_state, start_cell); - let region_node = *node_idx_by_cell.get(start_segment.last()?)?; - if !cost_to_go[region_node as usize].is_finite() { + let region_node = *start_segment.last()?; + if !node_cells.contains(®ion_node) + || !cost_to_go.get(®ion_node).is_some_and(|c| c.is_finite()) + { return None; } Some(( @@ -211,27 +214,33 @@ fn walk_local_preds(pred: &AHashMap, from: CellId) -> Vec (Vec, Vec) { - let n = plg.nodes.len(); - let mut dist = vec![f32::INFINITY; n]; - let mut pred = vec![NO_NODE; n]; - dist[source as usize] = 0.0; +/// Cost-to-go to source for every reachable node, with a predecessor pointing +/// one hop toward it. Nodes are keyed by their CellId. Unreachable nodes are +/// simply absent from the maps. +fn node_dijkstra( + plg: &PlannerGraph, + source: NodeId, +) -> (AHashMap, AHashMap) { + let mut dist: AHashMap = AHashMap::new(); + let mut pred: AHashMap = AHashMap::new(); + dist.insert(source, 0.0); let mut heap: BinaryHeap = BinaryHeap::new(); heap.push(Scored(0.0, source)); while let Some(Scored(d, u)) = heap.pop() { - if d > dist[u as usize] { + if d > dist.get(&u).copied().unwrap_or(f32::INFINITY) { continue; } - for &edge_idx in &plg.node_adj[u as usize] { + let Some(adj) = plg.node_adj.get(&u) else { + continue; + }; + for &edge_idx in adj { let edge = &plg.node_edges[edge_idx as usize]; let neighbor = if edge.a == u { edge.b } else { edge.a }; let nd = d + edge.cost; - if nd < dist[neighbor as usize] { - dist[neighbor as usize] = nd; - pred[neighbor as usize] = u; + if nd < dist.get(&neighbor).copied().unwrap_or(f32::INFINITY) { + dist.insert(neighbor, nd); + pred.insert(neighbor, u); heap.push(Scored(nd, neighbor)); } } @@ -240,14 +249,15 @@ fn node_dijkstra(plg: &PlannerGraph, source: NodeId) -> (Vec, Vec) } /// Build the node sequence by following goal-pointing predecessors. -fn follow_preds(from: NodeId, goal: NodeId, pred: &[NodeId]) -> Option> { +fn follow_preds( + from: NodeId, + goal: NodeId, + pred: &AHashMap, +) -> Option> { let mut seq = vec![from]; let mut cur = from; while cur != goal { - let next = pred[cur as usize]; - if next == NO_NODE { - return None; - } + let &next = pred.get(&cur)?; cur = next; seq.push(cur); } @@ -387,7 +397,7 @@ fn los_on_surface( } fn edge_between(plg: &PlannerGraph, a: NodeId, b: NodeId) -> Option { - for &edge_idx in &plg.node_adj[a as usize] { + for &edge_idx in plg.node_adj.get(&a)? { let edge = &plg.node_edges[edge_idx as usize]; let other = if edge.a == a { edge.b } else { edge.a }; if other == b { diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/python.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/python.rs index 5e1eca2315..13fdac5f57 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/python.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/python.rs @@ -9,7 +9,7 @@ use pyo3::types::PyDict; use validator::Validate; use crate::edges::edges_to_segments; -use crate::mls_planner::{Config, Planner}; +use crate::mls_planner::{Config, Planner, RegionBounds}; use crate::voxel::surface_point_xyz; #[pyclass] @@ -87,12 +87,56 @@ impl MLSPlanner { Ok(()) } + #[pyo3(signature = (points, origin, radius, z_min, z_max))] + fn update_region( + &mut self, + py: Python<'_>, + points: &Bound<'_, PyAny>, + origin: (f32, f32), + radius: f32, + z_min: f32, + z_max: f32, + ) -> PyResult<()> { + let points: PyReadonlyArray2<'_, f32> = points + .extract() + .map_err(|_| PyValueError::new_err("points must be a (N, 3) float32 numpy array"))?; + let shape = points.shape(); + if shape[1] != 3 { + return Err(PyValueError::new_err(format!( + "points must be (N, 3) float32, got shape {:?}", + shape + ))); + } + let arr = points.as_array(); + let n = shape[0]; + let pts: Vec<(f32, f32, f32)> = (0..n) + .filter_map(|i| { + let x = arr[[i, 0]]; + let y = arr[[i, 1]]; + let z = arr[[i, 2]]; + (x.is_finite() && y.is_finite() && z.is_finite()).then_some((x, y, z)) + }) + .collect(); + + let bounds = RegionBounds { + origin_x: origin.0, + origin_y: origin.1, + radius, + z_min, + z_max, + }; + let config = &self.config; + let planner = &mut self.planner; + py.allow_threads(move || planner.update_region(&pts, &bounds, config)); + Ok(()) + } + fn surface_map<'py>(&self, py: Python<'py>) -> Bound<'py, PyArray2> { let voxel_size = self.config.voxel_size; let surface = self.planner.surface(); let positions: Vec = py.allow_threads(|| { let mut out: Vec = Vec::with_capacity(surface.len() * 3); - for &(ix, iy, iz) in surface { + for (ix, iy, iz) in surface { let (x, y, z) = surface_point_xyz(ix, iy, iz, voxel_size); out.push(x); out.push(y); @@ -169,7 +213,27 @@ impl MLSPlanner { self.planner.voxel_count() } - /// Per-substep wall-clock cost (ms) of the most recent ``update_global_map``. + /// Accumulated occupied voxel centers as (N, 3) float32, for visualization. + fn voxel_map<'py>(&self, py: Python<'py>) -> Bound<'py, PyArray2> { + let vs = self.config.voxel_size; + let half = vs * 0.5; + let keys: Vec<(i32, i32, i32)> = self.planner.voxel_keys().collect(); + let positions: Vec = py.allow_threads(|| { + let mut out: Vec = Vec::with_capacity(keys.len() * 3); + for (kx, ky, kz) in keys { + out.push(kx as f32 * vs + half); + out.push(ky as f32 * vs + half); + out.push(kz as f32 * vs + half); + } + out + }); + let n = positions.len() / 3; + Array2::from_shape_vec((n, 3), positions) + .expect("3 elements pushed per voxel") + .into_pyarray(py) + } + + /// Per-substep wall-clock cost (ms) of the most recent map update. fn last_timings<'py>(&self, py: Python<'py>) -> PyResult> { let t = self.planner.last_timings(); let d = PyDict::new(py); diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/surfaces.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/surfaces.rs index 7451b959c4..f34a776a29 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/surfaces.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/surfaces.rs @@ -84,6 +84,92 @@ pub fn extract_surfaces( ); } +/// Insert a voxel into the per-column index, keeping each column sorted. +pub fn add_to_by_col(by_col: &mut ColumnIz, (ix, iy, iz): VoxelKey) { + let zs = by_col.entry((ix, iy)).or_default(); + if let Err(pos) = zs.binary_search(&iz) { + zs.insert(pos, iz); + } +} + +/// Remove a voxel from the per-column index, dropping emptied columns. +pub fn remove_from_by_col(by_col: &mut ColumnIz, (ix, iy, iz): VoxelKey) { + if let Some(zs) = by_col.get_mut(&(ix, iy)) { + if let Ok(pos) = zs.binary_search(&iz) { + zs.remove(pos); + } + if zs.is_empty() { + by_col.remove(&(ix, iy)); + } + } +} + +/// Re-extract surface cells whose columns fall in the inclusive `write` box +/// `(x0, x1, y0, y1)`. Reads `by_col` over `write` expanded by the morphology +/// halo so closing at the box boundary matches a full rebuild, then filters +/// the result back to `write`. `by_col` must already be current. +pub fn extract_surfaces_region( + by_col: &ColumnIz, + clearance_cells: i32, + dilation_passes: u32, + erosion_passes: u32, + write: (i32, i32, i32, i32), +) -> Vec { + let (wx0, wx1, wy0, wy1) = write; + let pad = (dilation_passes + erosion_passes) as i32; + + let standable: Vec = ((wx0 - pad)..(wx1 + pad + 1)) + .into_par_iter() + .flat_map_iter(|ix| { + let mut local: Vec = Vec::new(); + for iy in (wy0 - pad)..=(wy1 + pad) { + let Some(zs) = by_col.get(&(ix, iy)) else { + continue; + }; + for w in zs.windows(2) { + if w[1] - w[0] > clearance_cells { + local.push((ix, iy, w[0])); + } + } + if let Some(&last_iz) = zs.last() { + local.push((ix, iy, last_iz)); + } + } + local + }) + .collect(); + + let in_write = |ix: i32, iy: i32| ix >= wx0 && ix <= wx1 && iy >= wy0 && iy <= wy1; + + if dilation_passes == 0 && erosion_passes == 0 { + return standable + .into_iter() + .filter(|&(ix, iy, _)| in_write(ix, iy)) + .collect(); + } + + let mut by_z: AHashMap> = AHashMap::new(); + for &(ix, iy, iz) in &standable { + by_z.entry(iz).or_default().push((ix, iy)); + } + let slices: Vec<(i32, Vec<(i32, i32)>)> = by_z.into_iter().collect(); + slices + .par_iter() + .flat_map_iter(|(iz, xys)| { + close_at_z( + xys, + *iz, + by_col, + dilation_passes, + erosion_passes, + clearance_cells, + ) + .into_iter() + .filter(|c| in_write(c.0, c.1)) + }) + .collect() +} + /// Dilation and erosion on all xy slices of the extracted surfaces /// to fill in small holes. fn close_surface_holes( diff --git a/dimos/navigation/nav_3d/mls_planner/transformer.py b/dimos/navigation/nav_3d/mls_planner/transformer.py index d04532c969..ab7ef5a10c 100644 --- a/dimos/navigation/nav_3d/mls_planner/transformer.py +++ b/dimos/navigation/nav_3d/mls_planner/transformer.py @@ -77,8 +77,13 @@ def __call__( x, y, z, *_ = obs.pose_tuple start = (float(x), float(y), float(z) - self.robot_height) - voxel_map = obs.data - planner.update_global_map(voxel_map.points_f32()) + bounds = obs.tags.get("region_bounds") + if bounds is None: + raise ValueError( + "MLSPlan consumes local map slices; construct RayTraceMap(emit_local=True)" + ) + ox, oy, radius, z_min, z_max = bounds + planner.update_region(obs.data.points_f32(), (ox, oy), radius, z_min, z_max) t_plan = time.perf_counter() waypoints = planner.plan(start, self.goal) plan_ms = (time.perf_counter() - t_plan) * 1000 @@ -91,7 +96,7 @@ def __call__( data=path, tags={ **obs.tags, - "voxel_map": voxel_map, + "voxel_map": planner.voxel_map(), "surface_map": planner.surface_map(), "nodes": planner.nodes(), "node_edges": planner.node_edges(), diff --git a/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py b/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py index 18857f9ef8..a01a14a846 100644 --- a/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py +++ b/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py @@ -204,6 +204,7 @@ def main( max_range=max_range, ray_subsample=ray_subsample, emit_every=emit_every, + emit_local=True, ) ).transform( MLSPlan( @@ -228,7 +229,11 @@ def main( rr.log("world/start", rr.Points3D([start], colors=[[0, 255, 0]], radii=0.1)) voxel_map = obs.tags["voxel_map"] - rr.log("world/voxel_map", voxel_map.to_rerun(voxel_size=render_voxel)) + if voxel_map.size: + rr.log( + "world/voxel_map", + rr.Points3D(voxel_map, colors=[[80, 80, 80]], radii=render_voxel / 2), + ) surface = obs.tags["surface_map"] if surface.size: From b3da827d9f9492aab741e7b1e4968eb4d9999ed0 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Fri, 5 Jun 2026 18:27:53 -0700 Subject: [PATCH 06/56] Clean up --- dimos/mapping/ray_tracing/transformer.py | 38 +++-- .../nav_3d/mls_planner/rust/src/adjacency.rs | 3 +- .../nav_3d/mls_planner/rust/src/dijkstra.rs | 64 ++++---- .../nav_3d/mls_planner/rust/src/edges.rs | 141 ++++++------------ .../mls_planner/rust/src/mls_planner.rs | 122 ++++++++------- .../nav_3d/mls_planner/rust/src/nodes.rs | 23 ++- .../nav_3d/mls_planner/test_transformer.py | 60 ++++---- .../nav_3d/mls_planner/utils/plan_rrd.py | 16 +- 8 files changed, 218 insertions(+), 249 deletions(-) diff --git a/dimos/mapping/ray_tracing/transformer.py b/dimos/mapping/ray_tracing/transformer.py index ddb16f0d99..40a9b55f38 100644 --- a/dimos/mapping/ray_tracing/transformer.py +++ b/dimos/mapping/ray_tracing/transformer.py @@ -27,6 +27,8 @@ if TYPE_CHECKING: from collections.abc import Iterator + from numpy.typing import NDArray + from dimos.memory2.type.observation import Observation @@ -52,21 +54,33 @@ def __init__( self.region_percentile = region_percentile self._mapper_kwargs = mapper_kwargs - def _robust_bounds( - self, points: np.ndarray, origins: np.ndarray + def _local_bounds( + self, + batch_points: list[NDArray[np.float32]], + batch_origins: list[tuple[float, float, float]], + last_obs: Observation[PointCloud2], ) -> tuple[float, float, float, float, float]: - """Robot-centered cylinder sized to a percentile of the observed points, - so a sparse far tail of returns does not inflate the local region. + """Robot-centered cylinder sized to a percentile of the observed points. - The radius covers `region_percentile` of points by xy distance, and the - z band is trimmed to the same percentile to drop ceiling/tall returns. + Falls back to a zero-radius region at the robot when no finite points + were seen, so an empty lidar frame is a no-op update rather than a crash. """ margin = self._mapper_kwargs.get("shadow_depth", 0.2) + self.voxel_size + points = ( + np.concatenate(batch_points, axis=0) + if batch_points + else np.empty((0, 3), dtype=np.float32) + ) + points = points[np.isfinite(points).all(axis=1)] + if points.size == 0: + rx, ry, rz, *_ = last_obs.pose_tuple + return rx, ry, 0.0, rz, rz + + origins = np.asarray(batch_origins, dtype=np.float64) cx = float(origins[:, 0].mean()) cy = float(origins[:, 1].mean()) dist = np.hypot(points[:, 0] - cx, points[:, 1] - cy) radius = float(np.percentile(dist, self.region_percentile)) + margin - lo_pct = 100.0 - self.region_percentile z_min = float(np.percentile(points[:, 2], lo_pct)) - margin z_max = float(np.percentile(points[:, 2], self.region_percentile)) + margin @@ -77,14 +91,12 @@ def _make_obs( mapper: VoxelRayMapper, last_obs: Observation[PointCloud2], count: int, - batch_points: list[np.ndarray], + batch_points: list[NDArray[np.float32]], batch_origins: list[tuple[float, float, float]], ) -> Observation[PointCloud2]: tags = {**last_obs.tags, "frame_count": count} - if self.emit_local and batch_points: - points = np.concatenate(batch_points, axis=0) - origins = np.asarray(batch_origins, dtype=np.float64) - cx, cy, radius, z_min, z_max = self._robust_bounds(points, origins) + if self.emit_local: + cx, cy, radius, z_min, z_max = self._local_bounds(batch_points, batch_origins, last_obs) positions = mapper.local_map((cx, cy, 0.0), radius, z_min, z_max) tags["region_bounds"] = (cx, cy, radius, z_min, z_max) else: @@ -103,7 +115,7 @@ def __call__( ) last_obs: Observation[PointCloud2] | None = None count = 0 - batch_points: list[np.ndarray] = [] + batch_points: list[NDArray[np.float32]] = [] batch_origins: list[tuple[float, float, float]] = [] for obs in upstream: diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/adjacency.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/adjacency.rs index f75f1b0179..fc5a497019 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/adjacency.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/adjacency.rs @@ -26,8 +26,7 @@ pub struct Edge { pub dest: CellId, /// Geometric cost, set at build time and never mutated. pub base_cost: f32, - /// Effective cost = base_cost scaled by the wall-safe penalty. Derived - /// from base_cost each frame so the penalty stays idempotent. + /// base_cost scaled by the wall-safe penalty, recomputed each update. pub cost: f32, } diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/dijkstra.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/dijkstra.rs index 7ecf9b7f7f..7265e6269d 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/dijkstra.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/dijkstra.rs @@ -43,16 +43,31 @@ impl DijkstraState { } } -/// Multi-source dijkstra. -/// -/// Labels each node with distance to nearest source, the source id, and the path. -/// When `use_base` is set, edges are weighted by their geometric `base_cost` -/// instead of the penalized `cost` (used for the wall-distance field). +/// Which edge weight a search uses. +#[derive(Clone, Copy)] +pub enum Weight { + /// Geometric distance, for the wall-distance field. + Base, + /// Wall-safe penalized cost, for the node Voronoi. + Penalized, +} + +impl Weight { + #[inline] + fn of(self, edge: &crate::adjacency::Edge) -> f32 { + match self { + Weight::Base => edge.base_cost, + Weight::Penalized => edge.cost, + } + } +} + +/// Multi-source dijkstra labeling each cell with its nearest source and path. pub fn dijkstra( cells: &SurfaceCells, sources: &[CellId], state: &mut DijkstraState, - use_base: bool, + weight: Weight, ) { state.reset(cells.slot_capacity()); @@ -72,7 +87,7 @@ pub fn dijkstra( } let su = state.source[u as usize]; for edge in cells.neighbors(u) { - let nd = d + if use_base { edge.base_cost } else { edge.cost }; + let nd = d + weight.of(edge); let v = edge.dest as usize; if nd < state.dist[v] { state.dist[v] = nd; @@ -86,20 +101,15 @@ pub fn dijkstra( } } -/// Bounded multi-source Dijkstra that only re-labels cells in `window`, -/// keeping every cached label outside it. -/// -/// Window cells are cleared, then the wavefront is re-seeded two ways: from -/// any sources that lie inside the window, and from the frontier of already -/// labeled cells just outside it. Relaxation never writes a cell outside the -/// window, so the result inside is consistent with the cached labels around it -/// as long as the window margin exceeds the reach of the change. +/// Bounded multi-source dijkstra that re-labels only cells in `window`, seeding +/// the wavefront from in-window sources and the cached frontier just outside it. +/// Correct while the window margin exceeds the reach of the change. pub fn dijkstra_region( cells: &SurfaceCells, sources: &[CellId], window: &AHashSet, state: &mut DijkstraState, - use_base: bool, + weight: Weight, ) { state.ensure_capacity(cells.slot_capacity()); state.heap.clear(); @@ -145,7 +155,7 @@ pub fn dijkstra_region( if !window.contains(&v) { continue; } - let nd = d + if use_base { edge.base_cost } else { edge.cost }; + let nd = d + weight.of(edge); if nd < state.dist[v as usize] { state.dist[v as usize] = nd; state.pred[v as usize] = u; @@ -229,11 +239,11 @@ mod tests { let sources = [sc.id((0, 0, 0)).unwrap(), sc.id((9, 9, 0)).unwrap()]; let mut full = DijkstraState::default(); - dijkstra(&sc, &sources, &mut full, false); + dijkstra(&sc, &sources, &mut full, Weight::Penalized); let window: AHashSet = sc.ids().collect(); let mut region = DijkstraState::default(); - dijkstra_region(&sc, &sources, &window, &mut region, false); + dijkstra_region(&sc, &sources, &window, &mut region, Weight::Penalized); for id in sc.ids() { assert_eq!( @@ -251,7 +261,7 @@ mod tests { let sources = [sc.id((0, 0, 0)).unwrap(), sc.id((11, 11, 0)).unwrap()]; let mut full = DijkstraState::default(); - dijkstra(&sc, &sources, &mut full, false); + dijkstra(&sc, &sources, &mut full, Weight::Penalized); // Seed the regional state with the full result as the cache, then // recompute an interior block. Nothing changed, so the block must come @@ -270,7 +280,7 @@ mod tests { (3..=8).contains(&x) && (3..=8).contains(&y) }) .collect(); - dijkstra_region(&sc, &sources, &window, &mut region, false); + dijkstra_region(&sc, &sources, &window, &mut region, Weight::Penalized); for &id in &window { assert_eq!( @@ -303,7 +313,7 @@ mod tests { fn single_source_dist_and_pred() { let (sc, ids) = chain(5); let mut st = DijkstraState::default(); - dijkstra(&sc, &[ids[0]], &mut st, false); + dijkstra(&sc, &[ids[0]], &mut st, Weight::Penalized); for (i, &id) in ids.iter().enumerate().take(5) { assert_eq!(st.dist[id as usize], i as f32); assert_eq!(st.source[id as usize], 0); @@ -323,7 +333,7 @@ mod tests { fn multi_source_labels_by_nearest() { let (sc, ids) = chain(5); let mut st = DijkstraState::default(); - dijkstra(&sc, &[ids[0], ids[4]], &mut st, false); + dijkstra(&sc, &[ids[0], ids[4]], &mut st, Weight::Penalized); assert_eq!(st.source[ids[0] as usize], ids[0]); assert_eq!(st.source[ids[1] as usize], ids[0]); assert_eq!(st.source[ids[3] as usize], ids[4]); @@ -349,7 +359,7 @@ mod tests { sc.add_edge(c, d, 1.0); sc.add_edge(d, c, 1.0); let mut st = DijkstraState::default(); - dijkstra(&sc, &[a], &mut st, false); + dijkstra(&sc, &[a], &mut st, Weight::Penalized); assert_eq!(st.dist[a as usize], 0.0); assert_eq!(st.dist[b as usize], 1.0); assert!(!st.dist[c as usize].is_finite()); @@ -369,7 +379,7 @@ mod tests { sc.add_edge(c, b, 1.0); sc.add_edge(b, c, 1.0); let mut st = DijkstraState::default(); - dijkstra(&sc, &[a], &mut st, false); + dijkstra(&sc, &[a], &mut st, Weight::Penalized); assert_eq!(st.dist[b as usize], 2.0); assert_eq!(st.pred[b as usize], c); } @@ -378,9 +388,9 @@ mod tests { fn buffer_reuse_does_not_leak_prior_state() { let (sc1, ids1) = chain(5); let mut st = DijkstraState::default(); - dijkstra(&sc1, &[ids1[0]], &mut st, false); + dijkstra(&sc1, &[ids1[0]], &mut st, Weight::Penalized); let (sc2, ids2) = chain(3); - dijkstra(&sc2, &[ids2[0]], &mut st, false); + dijkstra(&sc2, &[ids2[0]], &mut st, Weight::Penalized); for (i, &id) in ids2.iter().enumerate().take(3) { assert_eq!(st.dist[id as usize], i as f32); } diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/edges.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/edges.rs index 260868b8ec..c42e4f5117 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/edges.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/edges.rs @@ -11,14 +11,13 @@ use ahash::{AHashMap, AHashSet}; use rayon::prelude::*; -use crate::adjacency::{CellId, Edge, SurfaceCells, SurfaceLookup, NO_CELL}; -use crate::dijkstra::{dijkstra, dijkstra_region, walk_preds, DijkstraState}; +use crate::adjacency::{CellId, SurfaceCells, SurfaceLookup, NO_CELL}; +use crate::dijkstra::{dijkstra, dijkstra_region, walk_preds, DijkstraState, Weight}; use crate::nodes::NodeData; use crate::voxel::VoxelKey; -/// A node is identified by the CellId it sits on. This stays stable across -/// incremental updates as long as that cell is untouched, so cached node -/// edges and the Voronoi forest survive a regional rebuild. +/// A node is identified by the CellId it sits on, stable across incremental +/// updates so cached edges and the Voronoi forest survive a regional rebuild. pub type NodeId = CellId; pub const NO_NODE: NodeId = NO_CELL; @@ -43,11 +42,9 @@ pub struct PlannerGraph { pub nodes: Vec, pub node_edges: Vec, pub node_adj: AHashMap>, - /// Voronoi forest: each cell's nearest node, distance, and predecessor. - /// Read by the planner to expand node edges into surface-cell paths. + /// Voronoi forest read by the planner to expand node edges into cell paths. pub cell_state: DijkstraState, - /// Persistent wall-distance field, kept so a regional node rebuild can - /// reseed its wavefront from cached values outside the dirty window. + /// Persistent wall-distance field, reseeded regionally from cached values. pub wall_state: DijkstraState, } @@ -77,7 +74,7 @@ pub fn build_node_edges( } let source_cells: Vec = nodes.iter().map(|n| n.cell_id).collect(); - dijkstra(cells, &source_cells, state, false); + dijkstra(cells, &source_cells, state, Weight::Penalized); best_boundary_edges(cells, state, out_edges); @@ -99,11 +96,9 @@ fn rebuild_node_adj(edges: &[NodeEdge], out_adj: &mut AHashMap = source_cells.iter().copied().collect(); @@ -135,63 +130,7 @@ pub fn build_node_edges_region( } let win_cells: Vec = window.iter().copied().collect(); - let fresh = win_cells - .par_iter() - .fold( - AHashMap::<(NodeId, NodeId), NodeEdge>::new, - |mut local, &u| { - let du = state.dist[u as usize]; - if du.is_finite() { - let sa = state.source[u as usize]; - for edge in cells.neighbors(u) { - let v = edge.dest; - let dv = state.dist[v as usize]; - if !dv.is_finite() { - continue; - } - let sb = state.source[v as usize]; - if sa == sb { - continue; - } - let cost = du + edge.cost + dv; - let (key_a, key_b, bu, bv) = if sa < sb { - (sa, sb, u, v) - } else { - (sb, sa, v, u) - }; - let entry = local.entry((key_a, key_b)).or_insert(NodeEdge { - a: key_a, - b: key_b, - cost: f32::INFINITY, - boundary_u: NO_CELL, - boundary_v: NO_CELL, - }); - if cost < entry.cost { - entry.cost = cost; - entry.boundary_u = bu; - entry.boundary_v = bv; - } - } - } - local - }, - ) - .reduce(AHashMap::<(NodeId, NodeId), NodeEdge>::new, |mut a, b| { - for (k, v_edge) in b { - let entry = a.entry(k).or_insert(v_edge); - if v_edge.cost < entry.cost { - *entry = v_edge; - } - } - a - }); - - for (k, fe) in fresh { - let entry = merged.entry(k).or_insert(fe); - if fe.cost < entry.cost { - *entry = fe; - } - } + merge_min(&mut merged, boundary_edge_map(cells, state, &win_cells)); out_edges.clear(); out_edges.extend(merged.into_values()); @@ -200,19 +139,29 @@ pub fn build_node_edges_region( } fn best_boundary_edges(cells: &SurfaceCells, state: &DijkstraState, out: &mut Vec) { - let cell_entries: Vec<(CellId, &[Edge])> = cells.iter().collect(); + let scan: Vec = cells.ids().collect(); + let merged = boundary_edge_map(cells, state, &scan); + out.clear(); + out.extend(merged.into_values()); + out.par_sort_unstable_by_key(|e| (e.a, e.b)); +} - let merged: AHashMap<(NodeId, NodeId), NodeEdge> = cell_entries - .par_iter() +/// Cheapest Voronoi-boundary crossing per adjacent node pair, scanning `scan`. +fn boundary_edge_map( + cells: &SurfaceCells, + state: &DijkstraState, + scan: &[CellId], +) -> AHashMap<(NodeId, NodeId), NodeEdge> { + scan.par_iter() .fold( AHashMap::<(NodeId, NodeId), NodeEdge>::new, - |mut local, (u, edges)| { - let du = state.dist[*u as usize]; + |mut local, &u| { + let du = state.dist[u as usize]; if !du.is_finite() { return local; } - let sa = state.source[*u as usize]; - for edge in *edges { + let sa = state.source[u as usize]; + for edge in cells.neighbors(u) { let v = edge.dest; let dv = state.dist[v as usize]; if !dv.is_finite() { @@ -223,13 +172,11 @@ fn best_boundary_edges(cells: &SurfaceCells, state: &DijkstraState, out: &mut Ve continue; } let cost = du + edge.cost + dv; - let (key_a, key_b, bu, bv) = if sa < sb { - (sa, sb, *u, v) + (sa, sb, u, v) } else { - (sb, sa, v, *u) + (sb, sa, v, u) }; - let entry = local.entry((key_a, key_b)).or_insert(NodeEdge { a: key_a, b: key_b, @@ -246,19 +193,23 @@ fn best_boundary_edges(cells: &SurfaceCells, state: &DijkstraState, out: &mut Ve local }, ) - .reduce(AHashMap::<(NodeId, NodeId), NodeEdge>::new, |mut a, b| { - for (k, v_edge) in b { - let entry = a.entry(k).or_insert(v_edge); - if v_edge.cost < entry.cost { - *entry = v_edge; - } - } + .reduce(AHashMap::new, |mut a, b| { + merge_min(&mut a, b); a - }); + }) +} - out.clear(); - out.extend(merged.into_values()); - out.par_sort_unstable_by_key(|e| (e.a, e.b)); +/// Keep the lower-cost edge for each node pair when merging two maps. +fn merge_min( + into: &mut AHashMap<(NodeId, NodeId), NodeEdge>, + from: AHashMap<(NodeId, NodeId), NodeEdge>, +) { + for (k, edge) in from { + let entry = into.entry(k).or_insert(edge); + if edge.cost < entry.cost { + *entry = edge; + } + } } /// Walk every node-graph edge and emit one segment per consecutive cell diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs index 74422cfc90..32155daf35 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs @@ -39,7 +39,7 @@ pub struct Config { pub node_step_threshold_m: f32, } -/// Wall-clock cost of each stage of the most recent full rebuild, in milliseconds. +/// Wall-clock cost in ms of each stage of the most recent map update. #[derive(Default, Clone, Copy)] pub struct RebuildTimings { pub voxelize_ms: f64, @@ -47,10 +47,8 @@ pub struct RebuildTimings { pub graph_ms: f64, } -/// Cylindrical dirty region the planner re-derives from a local map slice. -/// -/// The xy test matches the ray tracer's LocalBounds so that removing the -/// region and re-inserting the local map points is an exact replacement. +/// Cylindrical region the planner re-derives from a local map slice. The xy +/// test mirrors the ray tracer so the slice is an exact replacement. pub struct RegionBounds { pub origin_x: f32, pub origin_y: f32, @@ -124,13 +122,8 @@ impl Planner { }; } - /// Re-derive the planner from a cylindrical slice of the map. - /// - /// Replaces the cached voxels inside the cylinder with the local map slice, - /// then recomputes surfaces and the graph only where voxels actually - /// changed (plus a correctness margin), leaving everything else cached. The - /// cylinder bounds what can change. The realized change is usually far - /// smaller, so the work each frame scales with the change, not the map. + /// Re-derive the planner from a local map slice, recomputing only where + /// voxels changed and reusing everything else. pub fn update_region( &mut self, local_points: &[(f32, f32, f32)], @@ -170,7 +163,25 @@ impl Planner { let surfaces_ms = ms(t_surfaces.elapsed()); let t_graph = Instant::now(); - let step = (config.node_step_threshold_m / voxel_size).floor() as i32; + self.rebuild_region_graph(added, removed, config); + let graph_ms = ms(t_graph.elapsed()); + + self.last_timings = RebuildTimings { + voxelize_ms, + surfaces_ms, + graph_ms, + }; + } + + /// Patch cells for the changed surface, then re-place nodes and edges over + /// the change window. A no-op when no surface cell changed. + fn rebuild_region_graph( + &mut self, + added: Vec, + removed: Vec, + config: &Config, + ) { + let step = (config.node_step_threshold_m / config.voxel_size).floor() as i32; for &c in &removed { self.graph.cells.remove(c); } @@ -179,53 +190,40 @@ impl Planner { } let mut seeds = added; seeds.extend_from_slice(&removed); - - // No surface cell changed, so nodes and edges are untouched. - if !seeds.is_empty() { - rebuild_edges_around( - &mut self.graph.cells, - &self.graph.surface_lookup, - &seeds, - voxel_size, - step, - ); - - let window = self.node_window(&seeds, config); - place_nodes_region( - &mut self.graph.cells, - &window, - config.voxel_size, - config.node_spacing_m, - config.node_wall_buffer_m, - &mut self.graph.wall_state, - &mut self.graph.nodes, - ); - build_node_edges_region( - &self.graph.cells, - &self.graph.nodes, - &window, - &mut self.graph.cell_state, - &mut self.graph.node_edges, - &mut self.graph.node_adj, - ); + if seeds.is_empty() { + return; } - let graph_ms = ms(t_graph.elapsed()); - self.last_timings = RebuildTimings { - voxelize_ms, - surfaces_ms, - graph_ms, - }; + rebuild_edges_around( + &mut self.graph.cells, + &self.graph.surface_lookup, + &seeds, + config.voxel_size, + step, + ); + let window = self.node_window(&seeds, config); + place_nodes_region( + &mut self.graph.cells, + &window, + config.voxel_size, + config.node_spacing_m, + config.node_wall_buffer_m, + &mut self.graph.wall_state, + &mut self.graph.nodes, + ); + build_node_edges_region( + &self.graph.cells, + &self.graph.nodes, + &window, + &mut self.graph.cell_state, + &mut self.graph.node_edges, + &mut self.graph.node_adj, + ); } - /// Replace the voxels inside the cylinder with the local map points, - /// keeping the per-column index in sync. Returns the inclusive column bbox - /// of voxels that actually changed (added or removed), or None if nothing - /// changed, so downstream work can be scoped to the realized change. - /// - /// Work is bounded by the cylinder region: the cached voxels inside it are - /// enumerated through the per-column index over the cylinder's column bbox, - /// never by scanning the whole map. + /// Replace the cylinder's voxels with the local map points and keep the + /// per-column index in sync. Returns the column bbox of changed voxels, or + /// None if nothing changed. Bounded by the cylinder, never the whole map. fn replace_region_voxels( &mut self, local_points: &[(f32, f32, f32)], @@ -330,17 +328,17 @@ impl Planner { ms(t_graph.elapsed()) } - /// Live cells within the bbox of the changed surface cells, grown by the - /// node-graph margin. The margin must exceed the reach of any label change - /// (surface morphology pad, the wall-distance buffer, and the node spacing) - /// so cached nodes, edges, and the Voronoi forest outside the window stay - /// valid and identical to a full rebuild. + /// Live cells within the changed-cell bbox grown by the node-graph margin. + /// The margin covers the reach of any node, edge, or Voronoi change so the + /// graph outside the window stays valid. fn node_window(&self, changed: &[VoxelKey], config: &Config) -> AHashSet { + // A few extra cells beyond the morphology, wall-buffer, and spacing reach. + const SLACK_CELLS: i32 = 2; let voxel_size = config.voxel_size; let pad = (config.surface_dilation_passes + config.surface_erosion_passes) as i32; let buffer_cells = (config.node_wall_buffer_m / voxel_size).ceil() as i32; let spacing_cells = (config.node_spacing_m / voxel_size).ceil() as i32; - let margin = pad + buffer_cells + spacing_cells + 2; + let margin = pad + buffer_cells + spacing_cells + SLACK_CELLS; let mut bb = ChangeBounds::new(); for &(ix, iy, _) in changed { diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/nodes.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/nodes.rs index 51b6df8476..11950d5d4c 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/nodes.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/nodes.rs @@ -9,7 +9,7 @@ use ahash::{AHashMap, AHashSet}; use rayon::prelude::*; use crate::adjacency::{CellId, Edge, SurfaceCells}; -use crate::dijkstra::{dijkstra, dijkstra_region, DijkstraState}; +use crate::dijkstra::{dijkstra, dijkstra_region, DijkstraState, Weight}; use crate::voxel::{surface_point_xyz, VoxelKey}; #[derive(Clone, Copy, Debug)] @@ -37,7 +37,7 @@ pub fn place_nodes( let mut wall_seeds: Vec = Vec::new(); collect_wall_adjacent_cells(cells, &mut wall_seeds); - dijkstra(cells, &wall_seeds, state, true); + dijkstra(cells, &wall_seeds, state, Weight::Base); let mut candidates: Vec = cells .ids() @@ -63,11 +63,9 @@ pub fn place_nodes( apply_wall_safe_penalty(cells, &state.dist, node_wall_buffer_m); } -/// Regional counterpart to `place_nodes`. -/// -/// Recomputes the wall-distance field and node placement only inside the dirty -/// `window`, keeping cached nodes outside it. Cached nodes are fed to NMS as -/// seeds so freshly placed nodes respect spacing across the window seam. +/// Regional counterpart to place_nodes: recompute the wall-distance field and +/// node placement inside the window, keeping cached nodes outside it as NMS +/// seeds so spacing holds across the seam. pub fn place_nodes_region( cells: &mut SurfaceCells, window: &AHashSet, @@ -79,7 +77,7 @@ pub fn place_nodes_region( ) { let mut wall_seeds: Vec = Vec::new(); collect_wall_adjacent_in_window(cells, window, &mut wall_seeds); - dijkstra_region(cells, &wall_seeds, window, wall_state, true); + dijkstra_region(cells, &wall_seeds, window, wall_state, Weight::Base); nodes.retain(|n| cells.is_live(n.cell_id) && !window.contains(&n.cell_id)); let kept: Vec = nodes.iter().map(|n| n.cell_id).collect(); @@ -141,9 +139,8 @@ fn is_wall_adjacent(cells: &SurfaceCells, id: CellId) -> bool { mask != 0b1111 } -/// Re-scale edge costs for cells whose wall distance changed. The changed set -/// is the window plus its immediate neighbors, since an edge cost depends on -/// the wall distance at both endpoints. Idempotent via `base_cost`. +/// Rescale edge costs for the window and its neighbors, whose wall distance may +/// have changed. Idempotent via base_cost. fn apply_wall_safe_penalty_region( cells: &mut SurfaceCells, dist: &[f32], @@ -246,8 +243,8 @@ fn apply_wall_safe_penalty(cells: &mut SurfaceCells, dist: &[f32], buffer_m: f32 }); } -/// Rescale one cell's outgoing edges from their geometric `base_cost`. Reading -/// from `base_cost` keeps this idempotent, so a regional repass cannot compound. +/// Rescale one cell's outgoing edges from base_cost. Idempotent, so a regional +/// repass cannot compound the penalty. #[inline] fn scale_edges(edges: &mut [Edge], src: CellId, dist: &[f32], buffer_m: f32) { let pu = penalty_of(dist[src as usize], buffer_m); diff --git a/dimos/navigation/nav_3d/mls_planner/test_transformer.py b/dimos/navigation/nav_3d/mls_planner/test_transformer.py index a38d63b9d6..4b92f5cbef 100644 --- a/dimos/navigation/nav_3d/mls_planner/test_transformer.py +++ b/dimos/navigation/nav_3d/mls_planner/test_transformer.py @@ -15,56 +15,48 @@ from __future__ import annotations import numpy as np -from numpy.typing import NDArray -import pytest - -pytest.importorskip("dimos_mls_planner") from dimos.memory2.type.observation import Observation from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.navigation.nav_3d.mls_planner.transformer import MLSPlan -def _obs(points: NDArray[np.float32], pose: tuple[float, float, float]) -> Observation[PointCloud2]: - return Observation(id=0, ts=0.0, pose=pose, _data=PointCloud2.from_numpy(points)) - - -def _flat_floor(half_extent: float = 3.0, spacing: float = 0.1) -> NDArray[np.float32]: - coords = np.arange(-half_extent, half_extent, spacing, dtype=np.float32) - xs, ys = np.meshgrid(coords, coords) - zs = np.zeros_like(xs) - return np.stack([xs.ravel(), ys.ravel(), zs.ravel()], axis=1) +def _obs( + points: np.ndarray, + pose: tuple[float, float, float], + region_bounds: tuple[float, float, float, float, float], +) -> Observation[PointCloud2]: + return Observation( + id=0, + ts=0.0, + pose=pose, + tags={"region_bounds": region_bounds}, + _data=PointCloud2.from_numpy(points), + ) -def test_flat_floor_yields_populated_path_and_planned_true() -> None: - obs = _obs(_flat_floor(), pose=(-2.0, -2.0, 1.0)) +def test_start_z_is_dropped_by_robot_height() -> None: + obs = _obs( + np.zeros((1, 3), dtype=np.float32), + pose=(1.0, 2.0, 3.0), + region_bounds=(1.0, 2.0, 5.0, -1.0, 5.0), + ) - [out] = list(MLSPlan(goal=(2.0, 2.0, 0.0), voxel_size=0.2, robot_height=1.0)(iter([obs]))) + [out] = list(MLSPlan(goal=(10.0, 10.0, 0.0), robot_height=0.4)(iter([obs]))) - assert out.tags["planned"] is True - assert len(out.data.poses) >= 2 - assert out.tags["voxel_map"] is obs.data - assert out.tags["nodes"].shape[1] == 3 - assert out.tags["surface_map"].shape[1] == 3 + assert out.tags["start"] == (1.0, 2.0, 3.0 - 0.4) def test_no_route_yields_empty_path_with_planned_false() -> None: + # Random points form no traversable surface, so plan() returns None. rng = np.random.default_rng(0) - obs = _obs(rng.random((50, 3)).astype(np.float32), pose=(0.0, 0.0, 0.0)) + obs = _obs( + rng.random((50, 3)).astype(np.float32), + pose=(0.0, 0.0, 0.0), + region_bounds=(0.5, 0.5, 2.0, -1.0, 2.0), + ) [out] = list(MLSPlan(goal=(100.0, 100.0, 100.0))(iter([obs]))) assert out.tags["planned"] is False assert out.data.poses == [] - - -def test_poseless_obs_is_skipped_and_following_posed_obs_plans() -> None: - poseless = Observation(id=1, ts=0.0, pose=None, _data=PointCloud2.from_numpy(_flat_floor())) - posed = _obs(_flat_floor(), pose=(-2.0, -2.0, 1.0)) - - results = list( - MLSPlan(goal=(2.0, 2.0, 0.0), voxel_size=0.2, robot_height=1.0)(iter([poseless, posed])) - ) - - assert len(results) == 1 - assert results[0].tags["planned"] is True diff --git a/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py b/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py index a01a14a846..0b0338c206 100644 --- a/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py +++ b/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py @@ -66,8 +66,8 @@ def _print_summary(streams: dict[str, dict[str, Stream[float]]]) -> None: def _stitch_svgs(svgs: list[str]) -> str: - """Stack standalone SVGs vertically into one. Namespaces each panel's ids - so matplotlib's reused ids (axes_1, clip paths, glyphs) don't collide.""" + """Stack standalone SVGs vertically into one, namespacing each panel's ids + so matplotlib's reused ids do not collide.""" panels: list[str] = [] widths: list[float] = [] offset = 0.0 @@ -176,6 +176,12 @@ def main( plot_out: FsPath | None = typer.Option( None, "--plot-out", help="Write an SVG timing/size plot here when the run ends" ), + from_time: float | None = typer.Option( + None, "--from-time", help="Start timestamp (s); default is the stream start" + ), + to_time: float | None = typer.Option( + None, "--to-time", help="End timestamp (s); default is the stream end" + ), ) -> None: db_path = resolve_named_path(dataset, ".db") @@ -192,7 +198,11 @@ def main( store = SqliteStore(path=str(db_path)) with store: - lidar = store.stream(lidar_stream, PointCloud2).order_by("ts").from_time(110).to_time(120) + lidar = store.stream(lidar_stream, PointCloud2).order_by("ts") + if from_time is not None: + lidar = lidar.from_time(from_time) + if to_time is not None: + lidar = lidar.to_time(to_time) odom = store.stream(odom_stream, Odometry).order_by("ts") pose_tagged = lidar.align(odom, tolerance=align_tol).transform( From 9b3198a423f0f5c26b6cdca912776d2d34851706 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Sat, 6 Jun 2026 12:38:58 -0700 Subject: [PATCH 07/56] Clean up --- dimos/mapping/ray_tracing/transformer.py | 3 +- .../nav_3d/mls_planner/mls_planner.pyi | 3 +- .../nav_3d/mls_planner/rust/src/adjacency.rs | 8 +- .../nav_3d/mls_planner/rust/src/dijkstra.rs | 4 +- .../nav_3d/mls_planner/rust/src/edges.rs | 2 +- .../mls_planner/rust/src/mls_planner.rs | 101 ++---------------- .../nav_3d/mls_planner/rust/src/nodes.rs | 69 +++++++----- .../nav_3d/mls_planner/rust/src/planner.rs | 4 +- .../nav_3d/mls_planner/rust/src/surfaces.rs | 88 +++++++-------- .../nav_3d/mls_planner/utils/plan_rrd.py | 5 +- 10 files changed, 99 insertions(+), 188 deletions(-) diff --git a/dimos/mapping/ray_tracing/transformer.py b/dimos/mapping/ray_tracing/transformer.py index 40a9b55f38..1ec566e728 100644 --- a/dimos/mapping/ray_tracing/transformer.py +++ b/dimos/mapping/ray_tracing/transformer.py @@ -62,8 +62,7 @@ def _local_bounds( ) -> tuple[float, float, float, float, float]: """Robot-centered cylinder sized to a percentile of the observed points. - Falls back to a zero-radius region at the robot when no finite points - were seen, so an empty lidar frame is a no-op update rather than a crash. + An empty or all-nonfinite frame yields a zero-radius region at the robot. """ margin = self._mapper_kwargs.get("shadow_depth", 0.2) + self.voxel_size points = ( diff --git a/dimos/navigation/nav_3d/mls_planner/mls_planner.pyi b/dimos/navigation/nav_3d/mls_planner/mls_planner.pyi index 6061a522ce..260a96832c 100644 --- a/dimos/navigation/nav_3d/mls_planner/mls_planner.pyi +++ b/dimos/navigation/nav_3d/mls_planner/mls_planner.pyi @@ -43,8 +43,7 @@ class MLSPlanner: ) -> None: """Replace the cylindrical region with a local map slice and rebuild. - Removes cached voxels inside the cylinder, inserts the local map points, - and leaves everything outside untouched. Points are (N, 3) float32. + Points are (N, 3) float32. """ ... diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/adjacency.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/adjacency.rs index fc5a497019..2a6d1c84a8 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/adjacency.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/adjacency.rs @@ -232,11 +232,9 @@ pub fn build_surface_cells( } /// Recompute outgoing edges for every live cell within one step of a changed -/// cell, matching `build_surface_cells` edge logic. Call after surgical -/// insert/remove so the affected region's adjacency matches a full rebuild. -/// -/// `seeds` are the changed (added and removed) cell coordinates. The recompute -/// set is the live cells at those coordinates plus their surface neighbors. +/// cell, matching build_surface_cells edge logic. Call after surgical +/// insert or remove so the affected region matches a full rebuild. The seed +/// coordinates are the changed cells, and their surface neighbors are included. pub fn rebuild_edges_around( cells: &mut SurfaceCells, surface_lookup: &SurfaceLookup, diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/dijkstra.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/dijkstra.rs index 7265e6269d..eb0859a5d3 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/dijkstra.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/dijkstra.rs @@ -200,8 +200,8 @@ impl PartialOrd for Scored { } impl Ord for Scored { fn cmp(&self, other: &Self) -> Ordering { - // Min-distance pops first; tie-break by coordinate so the result is - // independent of CellId assignment (incremental == full rebuild). + // Min-distance pops first. Coordinate tie-break keeps the result + // independent of CellId assignment, so incremental matches full rebuild. other.0.total_cmp(&self.0).then(other.1.cmp(&self.1)) } } diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/edges.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/edges.rs index c42e4f5117..f294c241bb 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/edges.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/edges.rs @@ -146,7 +146,7 @@ fn best_boundary_edges(cells: &SurfaceCells, state: &DijkstraState, out: &mut Ve out.par_sort_unstable_by_key(|e| (e.a, e.b)); } -/// Cheapest Voronoi-boundary crossing per adjacent node pair, scanning `scan`. +/// Cheapest Voronoi-boundary crossing per adjacent node pair over the scanned cells. fn boundary_edge_map( cells: &SurfaceCells, state: &DijkstraState, diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs index 32155daf35..4f3da56392 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs @@ -149,8 +149,8 @@ impl Planner { }; let t_surfaces = Instant::now(); - // A changed voxel column only shifts surfaces within `pad` (morphology - // reach), so the write-back box is the changed-column bbox grown by pad. + // A changed voxel column shifts surfaces only within pad of it, so the + // write-back box is the changed-column bbox grown by pad. let write = (bx0 - pad, bx1 + pad, by0 - pad, by1 + pad); let new_cells = extract_surfaces_region( &self.by_col, @@ -271,9 +271,9 @@ impl Planner { bb.bounds() } - /// Replace the surface_lookup entries for columns in the `write` box with - /// freshly extracted cells (which all lie within the box). Returns the - /// (added, removed) surface cells so the graph can be patched surgically. + /// Replace the surface_lookup entries for columns in the write box with the + /// freshly extracted cells. Returns the added and removed surface cells so + /// the graph can be patched surgically. fn replace_surface_region( &mut self, write: (i32, i32, i32, i32), @@ -328,9 +328,8 @@ impl Planner { ms(t_graph.elapsed()) } - /// Live cells within the changed-cell bbox grown by the node-graph margin. - /// The margin covers the reach of any node, edge, or Voronoi change so the - /// graph outside the window stays valid. + /// Live cells within the changed-cell bbox grown by the node-graph margin, + /// which covers the reach of any node, edge, or Voronoi change. fn node_window(&self, changed: &[VoxelKey], config: &Config) -> AHashSet { // A few extra cells beyond the morphology, wall-buffer, and spacing reach. const SLACK_CELLS: i32 = 2; @@ -530,7 +529,7 @@ mod region_tests { p.voxel_map.iter().copied().collect() } - /// Cell adjacency keyed by coordinate (CellId-independent). + /// Cell adjacency keyed by coordinate, independent of CellId. fn cell_edges(p: &Planner) -> BTreeMap> { let cells = &p.graph.cells; let mut out: BTreeMap> = BTreeMap::new(); @@ -566,59 +565,6 @@ mod region_tests { .collect() } - #[test] - fn region_update_matches_full_rebuild() { - let cfg = test_config(); - let bounds = RegionBounds { - origin_x: 2.0, - origin_y: 2.0, - radius: 1.0, - z_min: -1.0, - z_max: 2.0, - }; - let all = world_points(); - - // Full rebuild over the whole map. - let mut full = Planner::default(); - full.update_global_map(&all, &cfg); - - // Region path: build everything outside the cylinder, then patch the - // cylinder in from a local slice. - let inside: Vec<_> = all - .iter() - .copied() - .filter(|&p| bounds.contains_voxel(voxelize(p, cfg.voxel_size), cfg.voxel_size)) - .collect(); - let outside: Vec<_> = all - .iter() - .copied() - .filter(|&p| !bounds.contains_voxel(voxelize(p, cfg.voxel_size), cfg.voxel_size)) - .collect(); - assert!(!inside.is_empty(), "cylinder should cover some voxels"); - - let mut region = Planner::default(); - region.update_global_map(&outside, &cfg); - region.update_region(&inside, &bounds, &cfg); - - assert_eq!(voxel_set(®ion), voxel_set(&full), "voxel map mismatch"); - assert_eq!(surface_set(®ion), surface_set(&full), "surface mismatch"); - assert_eq!( - cell_edges(®ion), - cell_edges(&full), - "cell edges mismatch" - ); - assert_eq!( - node_coords(®ion), - node_coords(&full), - "node set mismatch" - ); - assert_eq!( - node_edge_pairs(®ion), - node_edge_pairs(&full), - "node edge mismatch" - ); - } - #[test] fn region_update_removes_stale_voxels() { let cfg = test_config(); @@ -739,37 +685,6 @@ mod region_tests { } } - /// Re-observing local cylinders on a fully built map must not change the - /// geometry and must keep planning equivalent. The window here is far - /// smaller than the map, so this exercises the cached frontier. - #[test] - fn region_reobserve_preserves_planning() { - let cfg = test_config(); - let all = big_world(); - let vs = cfg.voxel_size; - - let mut full = Planner::default(); - full.update_global_map(&all, &cfg); - - let mut region = Planner::default(); - region.update_global_map(&all, &cfg); - - for &(cx, cy) in &[(2.0, 2.0), (4.0, 4.0), (4.0, 6.0), (6.0, 3.0), (1.5, 7.0)] { - let b = RegionBounds { - origin_x: cx, - origin_y: cy, - radius: 1.2, - z_min: -1.0, - z_max: 2.0, - }; - region.update_region(&slice(&all, &b, vs), &b, &cfg); - } - - assert_eq!(voxel_set(®ion), voxel_set(&full), "voxel set changed"); - assert_eq!(surface_set(®ion), surface_set(&full), "surface changed"); - assert_plans_equivalent(&full, ®ion, &cfg); - } - /// Re-observing the same geometry must change nothing: no voxel, surface, /// cell, node, or edge moves. This is the anti-jitter guarantee, far nodes /// stay put when their region is re-seen, matching a full rebuild. diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/nodes.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/nodes.rs index 11950d5d4c..0357d7460d 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/nodes.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/nodes.rs @@ -39,18 +39,40 @@ pub fn place_nodes( collect_wall_adjacent_cells(cells, &mut wall_seeds); dijkstra(cells, &wall_seeds, state, Weight::Base); - let mut candidates: Vec = cells + let candidates: Vec = cells .ids() .filter(|&id| state.dist[id as usize] >= node_wall_buffer_m) .collect(); + place_from_candidates( + cells, + candidates, + &state.dist, + &[], + voxel_size, + node_spacing_m, + out_nodes, + ); + + apply_wall_safe_penalty(cells, &state.dist, node_wall_buffer_m); +} + +/// Sort candidates by descending wall distance, thin them with NMS against the +/// seed nodes, and append the survivors as nodes. +fn place_from_candidates( + cells: &SurfaceCells, + mut candidates: Vec, + dist: &[f32], + seeds: &[CellId], + voxel_size: f32, + node_spacing_m: f32, + out_nodes: &mut Vec, +) { candidates.par_sort_unstable_by(|&a, &b| { - state.dist[b as usize] - .total_cmp(&state.dist[a as usize]) + dist[b as usize] + .total_cmp(&dist[a as usize]) .then(cells.coord(a).cmp(&cells.coord(b))) }); - - let survivors = nms_grid(cells, &candidates, &[], voxel_size, node_spacing_m); - + let survivors = nms_grid(cells, &candidates, seeds, voxel_size, node_spacing_m); out_nodes.reserve(survivors.len()); for &id in &survivors { let (ix, iy, iz) = cells.coord(id); @@ -59,8 +81,6 @@ pub fn place_nodes( pos: surface_point_xyz(ix, iy, iz, voxel_size), }); } - - apply_wall_safe_penalty(cells, &state.dist, node_wall_buffer_m); } /// Regional counterpart to place_nodes: recompute the wall-distance field and @@ -82,31 +102,25 @@ pub fn place_nodes_region( nodes.retain(|n| cells.is_live(n.cell_id) && !window.contains(&n.cell_id)); let kept: Vec = nodes.iter().map(|n| n.cell_id).collect(); - let mut candidates: Vec = window + let candidates: Vec = window .iter() .copied() .filter(|&id| cells.is_live(id) && wall_state.dist[id as usize] >= node_wall_buffer_m) .collect(); - candidates.par_sort_unstable_by(|&a, &b| { - wall_state.dist[b as usize] - .total_cmp(&wall_state.dist[a as usize]) - .then(cells.coord(a).cmp(&cells.coord(b))) - }); - - let survivors = nms_grid(cells, &candidates, &kept, voxel_size, node_spacing_m); - nodes.reserve(survivors.len()); - for &id in &survivors { - let (ix, iy, iz) = cells.coord(id); - nodes.push(NodeData { - cell_id: id, - pos: surface_point_xyz(ix, iy, iz, voxel_size), - }); - } + place_from_candidates( + cells, + candidates, + &wall_state.dist, + &kept, + voxel_size, + node_spacing_m, + nodes, + ); apply_wall_safe_penalty_region(cells, &wall_state.dist, node_wall_buffer_m, window); } -/// Wall-adjacency over a cell subset, matching `collect_wall_adjacent_cells`. +/// Wall-adjacency over a cell subset, matching collect_wall_adjacent_cells. fn collect_wall_adjacent_in_window( cells: &SurfaceCells, window: &AHashSet, @@ -177,9 +191,8 @@ fn collect_wall_adjacent_cells(cells: &SurfaceCells, out: &mut Vec) { /// Space out nodes based on minimum distance. /// -/// `seeds` are pre-existing nodes that suppress nearby candidates without -/// themselves being emitted, used to keep a regional re-placement consistent -/// with cached nodes just outside the window. +/// The seed nodes suppress nearby candidates without being emitted, keeping a +/// regional re-placement consistent with cached nodes outside the window. fn nms_grid( cells: &SurfaceCells, candidates_sorted: &[CellId], diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs index 7258366e56..e60eb50eff 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs @@ -536,7 +536,7 @@ mod tests { #[test] fn plan_lead_in_does_not_backtrack_to_region_node() { - // Robot at cell 5 is in node (3)'s region but sits between it and node (15). + // Robot at cell 5 is in node 3's region but sits between it and node 15. let plg = graph_with_nodes(&strip(20), &[(3, 0, 0), (15, 0, 0)]); let wp = plan_simple(&plg, (0.55, 0.0, 0.05), (1.7, 0.0, 0.05)).unwrap(); let xs: Vec = wp[1..wp.len() - 1] @@ -607,7 +607,7 @@ mod tests { #[test] fn plan_enters_on_goalward_node_not_nearest() { - // Robot sits past node (2) toward the goal; entry must skip it for node (10). + // Robot sits past node 2 toward the goal. Entry must skip it for node 10. let plg = graph_with_nodes(&strip(20), &[(2, 0, 0), (10, 0, 0)]); let wp = plan_simple(&plg, (0.45, 0.0, 0.05), (1.25, 0.0, 0.05)).unwrap(); let nearest = surface_point_xyz(2, 0, 0, VOXEL); diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/surfaces.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/surfaces.rs index f34a776a29..2455c668d3 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/surfaces.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/surfaces.rs @@ -61,14 +61,7 @@ pub fn extract_surfaces( .par_iter() .flat_map_iter(|((ix, iy), zs)| { let mut local: Vec = Vec::new(); - for w in zs.windows(2) { - if w[1] - w[0] > clearance_cells { - local.push((*ix, *iy, w[0])); - } - } - if let Some(&last_iz) = zs.last() { - local.push((*ix, *iy, last_iz)); - } + standable_in_column(*ix, *iy, zs, clearance_cells, &mut local); local }) .collect(); @@ -84,6 +77,25 @@ pub fn extract_surfaces( ); } +/// Standable cells in one column: any cell with robot clearance above, plus +/// the topmost cell. +fn standable_in_column( + ix: i32, + iy: i32, + zs: &[i32], + clearance_cells: i32, + out: &mut Vec, +) { + for w in zs.windows(2) { + if w[1] - w[0] > clearance_cells { + out.push((ix, iy, w[0])); + } + } + if let Some(&last_iz) = zs.last() { + out.push((ix, iy, last_iz)); + } +} + /// Insert a voxel into the per-column index, keeping each column sorted. pub fn add_to_by_col(by_col: &mut ColumnIz, (ix, iy, iz): VoxelKey) { let zs = by_col.entry((ix, iy)).or_default(); @@ -104,10 +116,10 @@ pub fn remove_from_by_col(by_col: &mut ColumnIz, (ix, iy, iz): VoxelKey) { } } -/// Re-extract surface cells whose columns fall in the inclusive `write` box -/// `(x0, x1, y0, y1)`. Reads `by_col` over `write` expanded by the morphology -/// halo so closing at the box boundary matches a full rebuild, then filters -/// the result back to `write`. `by_col` must already be current. +/// Re-extract surface cells whose columns fall in the inclusive write box. +/// Reads by_col over the box plus the morphology halo so closing at the +/// boundary matches a full rebuild, then filters back to the box. by_col must +/// already be current. pub fn extract_surfaces_region( by_col: &ColumnIz, clearance_cells: i32, @@ -123,50 +135,26 @@ pub fn extract_surfaces_region( .flat_map_iter(|ix| { let mut local: Vec = Vec::new(); for iy in (wy0 - pad)..=(wy1 + pad) { - let Some(zs) = by_col.get(&(ix, iy)) else { - continue; - }; - for w in zs.windows(2) { - if w[1] - w[0] > clearance_cells { - local.push((ix, iy, w[0])); - } - } - if let Some(&last_iz) = zs.last() { - local.push((ix, iy, last_iz)); + if let Some(zs) = by_col.get(&(ix, iy)) { + standable_in_column(ix, iy, zs, clearance_cells, &mut local); } } local }) .collect(); - let in_write = |ix: i32, iy: i32| ix >= wx0 && ix <= wx1 && iy >= wy0 && iy <= wy1; - - if dilation_passes == 0 && erosion_passes == 0 { - return standable - .into_iter() - .filter(|&(ix, iy, _)| in_write(ix, iy)) - .collect(); - } - - let mut by_z: AHashMap> = AHashMap::new(); - for &(ix, iy, iz) in &standable { - by_z.entry(iz).or_default().push((ix, iy)); - } - let slices: Vec<(i32, Vec<(i32, i32)>)> = by_z.into_iter().collect(); - slices - .par_iter() - .flat_map_iter(|(iz, xys)| { - close_at_z( - xys, - *iz, - by_col, - dilation_passes, - erosion_passes, - clearance_cells, - ) - .into_iter() - .filter(|c| in_write(c.0, c.1)) - }) + let mut closed: Vec = Vec::new(); + close_surface_holes( + standable, + by_col, + dilation_passes, + erosion_passes, + clearance_cells, + &mut closed, + ); + closed + .into_iter() + .filter(|&(ix, iy, _)| ix >= wx0 && ix <= wx1 && iy >= wy0 && iy <= wy1) .collect() } diff --git a/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py b/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py index 0b0338c206..274ec83ee0 100644 --- a/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py +++ b/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py @@ -81,9 +81,8 @@ def _stitch_svgs(svgs: list[str]) -> str: body = re.sub(r'id="([^"]+)"', rf'id="{prefix}\1"', body) body = re.sub(r"url\(#([^)]+)\)", rf"url(#{prefix}\1)", body) body = re.sub(r'xlink:href="#([^"]+)"', rf'xlink:href="#{prefix}\1"', body) - # Drop the "pt" unit so the nested width/height are read as parent user - # units (the outer viewBox is unitless); otherwise pt->px conversion - # overflows the parent viewport and clips each panel on the right. + # Drop the pt unit so nested width/height read as parent user units. + # Otherwise pt to px conversion overflows the viewport and clips panels. body = body.replace(m.group(0), f'width="{width}" height="{height}"', 1) body = body.replace(" Date: Tue, 9 Jun 2026 11:49:44 -0700 Subject: [PATCH 08/56] Safety boundaries --- .../nav_3d/mls_planner/mls_planner.pyi | 10 ++ .../nav_3d/mls_planner/mls_planner_native.py | 2 + .../mls_planner/rust/src/mls_planner.rs | 100 ++++++++++++++++++ .../nav_3d/mls_planner/rust/src/nodes.rs | 87 +++++++++++---- .../nav_3d/mls_planner/rust/src/planner.rs | 83 ++++++++++++--- .../nav_3d/mls_planner/rust/src/python.rs | 29 +++++ .../nav_3d/mls_planner/transformer.py | 2 +- .../nav_3d/mls_planner/utils/plan_rrd.py | 41 ++++++- 8 files changed, 316 insertions(+), 38 deletions(-) diff --git a/dimos/navigation/nav_3d/mls_planner/mls_planner.pyi b/dimos/navigation/nav_3d/mls_planner/mls_planner.pyi index 260a96832c..68cc5dea93 100644 --- a/dimos/navigation/nav_3d/mls_planner/mls_planner.pyi +++ b/dimos/navigation/nav_3d/mls_planner/mls_planner.pyi @@ -28,6 +28,8 @@ class MLSPlanner: node_spacing_m: float = 1.0, node_wall_buffer_m: float = 0.3, node_step_threshold_m: float = 0.25, + robot_radius_m: float = 0.2, + wall_penalty_weight: float = 4.0, ) -> None: ... def update_global_map(self, points: NDArray[np.float32]) -> None: """Voxelize the map and rebuild surfaces, nodes, and edges. Shape (N, 3) float32.""" @@ -51,6 +53,14 @@ class MLSPlanner: """Standable surface cells as (M, 3) float32 centers.""" ... + def surface_clearance_map(self) -> NDArray[np.float32]: + """Surface cells as (M, 4) float32 rows of [x, y, z, clearance]. + + Clearance is the horizontal distance to the nearest untraversable edge. + Unreached cells report +inf. + """ + ... + def nodes(self) -> NDArray[np.float32]: """Graph node positions as (K, 3) float32.""" ... diff --git a/dimos/navigation/nav_3d/mls_planner/mls_planner_native.py b/dimos/navigation/nav_3d/mls_planner/mls_planner_native.py index 1a435fa0cb..b90ccba370 100644 --- a/dimos/navigation/nav_3d/mls_planner/mls_planner_native.py +++ b/dimos/navigation/nav_3d/mls_planner/mls_planner_native.py @@ -39,6 +39,8 @@ class MLSPlannerNativeConfig(NativeModuleConfig): node_spacing_m: float = 1.0 node_wall_buffer_m: float = 0.3 node_step_threshold_m: float = 0.25 + robot_radius_m: float = 0.2 + wall_penalty_weight: float = 4.0 class MLSPlannerNative(NativeModule): diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs index 4f3da56392..cb926b514f 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs @@ -37,6 +37,22 @@ pub struct Config { pub node_wall_buffer_m: f32, #[validate(range(min = 0.0))] pub node_step_threshold_m: f32, + /// Hard clearance floor: cells closer than this to a wall are impassable. + #[serde(default = "default_robot_radius_m")] + #[validate(range(min = 0.0))] + pub robot_radius_m: f32, + /// Strength of the soft wall penalty at the radius, decaying with distance. + #[serde(default = "default_wall_penalty_weight")] + #[validate(range(min = 0.0))] + pub wall_penalty_weight: f32, +} + +fn default_robot_radius_m() -> f32 { + 0.2 +} + +fn default_wall_penalty_weight() -> f32 { + 4.0 } /// Wall-clock cost in ms of each stage of the most recent map update. @@ -208,6 +224,8 @@ impl Planner { config.voxel_size, config.node_spacing_m, config.node_wall_buffer_m, + config.robot_radius_m, + config.wall_penalty_weight, &mut self.graph.wall_state, &mut self.graph.nodes, ); @@ -382,6 +400,8 @@ impl Planner { config.voxel_size, config.node_spacing_m, config.node_wall_buffer_m, + config.robot_radius_m, + config.wall_penalty_weight, &mut self.graph.wall_state, &mut self.graph.nodes, ); @@ -412,6 +432,7 @@ impl Planner { config.robot_height, config.node_spacing_m, config.node_step_threshold_m, + config.node_wall_buffer_m, ) } @@ -429,6 +450,20 @@ impl Planner { out } + /// Surface cells paired with their wall clearance, the distance to the + /// nearest untraversable edge. Unreached cells report +inf. + pub fn surface_clearance(&self) -> Vec<(VoxelKey, f32)> { + let dist = &self.graph.wall_state.dist; + self.graph + .cells + .ids() + .map(|id| { + let d = dist.get(id as usize).copied().unwrap_or(f32::INFINITY); + (self.graph.cells.coord(id), d) + }) + .collect() + } + pub fn voxel_count(&self) -> usize { self.voxel_map.len() } @@ -495,6 +530,8 @@ mod region_tests { node_spacing_m: 1.0, node_wall_buffer_m: 0.3, node_step_threshold_m: 0.25, + robot_radius_m: 0.0, + wall_penalty_weight: 1.0, } } @@ -767,4 +804,67 @@ mod region_tests { ); assert_plans_equivalent(&full, ®ion, &cfg); } + + /// Floor split by a wall with a narrow 1-cell gap near x=1.0 and a wide gap + /// near x=4.5. Start and goal straddle the narrow gap. + fn two_gap_world() -> Vec<(f32, f32, f32)> { + let vs = 0.1_f32; + let half = vs * 0.5; + let mut pts = Vec::new(); + for ix in 0..60 { + for iy in 0..40 { + pts.push((ix as f32 * vs + half, iy as f32 * vs + half, half)); + } + } + for ix in 0..60 { + if ix == 10 || (40..50).contains(&ix) { + continue; + } + for iz in 0..7 { + pts.push(( + ix as f32 * vs + half, + 20.0 * vs + half, + iz as f32 * vs + half, + )); + } + } + pts + } + + /// The hard clearance floor must make the narrow gap impassable, forcing + /// the longer detour through the wide gap. + #[test] + fn hard_clearance_floor_avoids_narrow_gap() { + let mut cfg = test_config(); + cfg.node_spacing_m = 0.8; + let pts = two_gap_world(); + let start = (1.0, 1.0, 0.05); + let goal = (1.0, 3.5, 0.05); + let max_x = |w: &[(f32, f32, f32)]| w.iter().map(|p| p.0).fold(f32::MIN, f32::max); + + // No floor: the shortest route slips straight through the narrow gap. + cfg.robot_radius_m = 0.0; + let mut open = Planner::default(); + open.update_global_map(&pts, &cfg); + let wp_open = open.plan(start, goal, &cfg).expect("open plan exists"); + + // Floor wider than the narrow gap: it is impassable, so detour wide. + cfg.robot_radius_m = 0.2; + let mut safe = Planner::default(); + safe.update_global_map(&pts, &cfg); + let wp_safe = safe.plan(start, goal, &cfg).expect("safe plan exists"); + + assert!(max_x(&wp_open) < 2.0, "open path should use the near gap"); + assert!( + max_x(&wp_safe) > 3.5, + "safe path should detour to the wide gap: max_x={}", + max_x(&wp_safe) + ); + assert!( + path_len(&wp_safe) > path_len(&wp_open) * 1.5, + "safe route should be substantially longer: {} vs {}", + path_len(&wp_safe), + path_len(&wp_open) + ); + } } diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/nodes.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/nodes.rs index 0357d7460d..8cde46c211 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/nodes.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/nodes.rs @@ -22,11 +22,14 @@ pub struct NodeData { /// /// Runs multi source dijkstra using edges as sources, then distribute nodes /// using a grid based NMS. +#[allow(clippy::too_many_arguments)] pub fn place_nodes( cells: &mut SurfaceCells, voxel_size: f32, node_spacing_m: f32, node_wall_buffer_m: f32, + robot_radius_m: f32, + wall_penalty_weight: f32, state: &mut DijkstraState, out_nodes: &mut Vec, ) { @@ -39,9 +42,10 @@ pub fn place_nodes( collect_wall_adjacent_cells(cells, &mut wall_seeds); dijkstra(cells, &wall_seeds, state, Weight::Base); + let node_floor = node_wall_buffer_m.max(robot_radius_m); let candidates: Vec = cells .ids() - .filter(|&id| state.dist[id as usize] >= node_wall_buffer_m) + .filter(|&id| state.dist[id as usize] >= node_floor) .collect(); place_from_candidates( cells, @@ -53,7 +57,13 @@ pub fn place_nodes( out_nodes, ); - apply_wall_safe_penalty(cells, &state.dist, node_wall_buffer_m); + apply_wall_safe_penalty( + cells, + &state.dist, + node_wall_buffer_m, + robot_radius_m, + wall_penalty_weight, + ); } /// Sort candidates by descending wall distance, thin them with NMS against the @@ -86,12 +96,15 @@ fn place_from_candidates( /// Regional counterpart to place_nodes: recompute the wall-distance field and /// node placement inside the window, keeping cached nodes outside it as NMS /// seeds so spacing holds across the seam. +#[allow(clippy::too_many_arguments)] pub fn place_nodes_region( cells: &mut SurfaceCells, window: &AHashSet, voxel_size: f32, node_spacing_m: f32, node_wall_buffer_m: f32, + robot_radius_m: f32, + wall_penalty_weight: f32, wall_state: &mut DijkstraState, nodes: &mut Vec, ) { @@ -102,10 +115,11 @@ pub fn place_nodes_region( nodes.retain(|n| cells.is_live(n.cell_id) && !window.contains(&n.cell_id)); let kept: Vec = nodes.iter().map(|n| n.cell_id).collect(); + let node_floor = node_wall_buffer_m.max(robot_radius_m); let candidates: Vec = window .iter() .copied() - .filter(|&id| cells.is_live(id) && wall_state.dist[id as usize] >= node_wall_buffer_m) + .filter(|&id| cells.is_live(id) && wall_state.dist[id as usize] >= node_floor) .collect(); place_from_candidates( cells, @@ -117,7 +131,14 @@ pub fn place_nodes_region( nodes, ); - apply_wall_safe_penalty_region(cells, &wall_state.dist, node_wall_buffer_m, window); + apply_wall_safe_penalty_region( + cells, + &wall_state.dist, + node_wall_buffer_m, + robot_radius_m, + wall_penalty_weight, + window, + ); } /// Wall-adjacency over a cell subset, matching collect_wall_adjacent_cells. @@ -159,6 +180,8 @@ fn apply_wall_safe_penalty_region( cells: &mut SurfaceCells, dist: &[f32], buffer_m: f32, + robot_radius_m: f32, + weight: f32, window: &AHashSet, ) { let mut affected: AHashSet = AHashSet::with_capacity(window.len() * 2); @@ -169,7 +192,14 @@ fn apply_wall_safe_penalty_region( } } for id in affected { - scale_edges(cells.edges_mut(id), id, dist, buffer_m); + scale_edges( + cells.edges_mut(id), + id, + dist, + buffer_m, + robot_radius_m, + weight, + ); } } @@ -247,29 +277,50 @@ fn nms_grid( } /// Scale every edge cost by the average of its endpoint penalties, which -/// pushes shortest paths away from walls. Unreached cells have -/// dist == +INFINITY which collapses to penalty 1.0. -fn apply_wall_safe_penalty(cells: &mut SurfaceCells, dist: &[f32], buffer_m: f32) { +/// pushes shortest paths away from walls and forbids sub-radius cells. +/// Unreached cells have dist == +INFINITY which collapses to penalty 1.0. +fn apply_wall_safe_penalty( + cells: &mut SurfaceCells, + dist: &[f32], + buffer_m: f32, + robot_radius_m: f32, + weight: f32, +) { let mut edge_lists: Vec<(CellId, &mut Vec)> = cells.iter_edges_mut().collect(); edge_lists.par_iter_mut().for_each(|(src, edges)| { - scale_edges(edges, *src, dist, buffer_m); + scale_edges(edges, *src, dist, buffer_m, robot_radius_m, weight); }); } /// Rescale one cell's outgoing edges from base_cost. Idempotent, so a regional /// repass cannot compound the penalty. #[inline] -fn scale_edges(edges: &mut [Edge], src: CellId, dist: &[f32], buffer_m: f32) { - let pu = penalty_of(dist[src as usize], buffer_m); +fn scale_edges( + edges: &mut [Edge], + src: CellId, + dist: &[f32], + buffer_m: f32, + robot_radius_m: f32, + weight: f32, +) { + let pu = penalty_of(dist[src as usize], buffer_m, robot_radius_m, weight); for edge in edges.iter_mut() { - let pv = penalty_of(dist[edge.dest as usize], buffer_m); + let pv = penalty_of(dist[edge.dest as usize], buffer_m, robot_radius_m, weight); edge.cost = edge.base_cost * (pu + pv) / 2.0; } } +/// Cost multiplier for a cell at wall distance d. Cells closer than the robot +/// radius are impassable. Beyond it the penalty decays smoothly from +/// 1 + weight at the radius toward 1, with length scale buffer_m, so paths are +/// pushed toward open space with no hard cap and no flat zone. #[inline] -fn penalty_of(d: f32, buffer_m: f32) -> f32 { - (1.0 + (buffer_m - d) / buffer_m).max(1.0) +fn penalty_of(d: f32, buffer_m: f32, robot_radius_m: f32, weight: f32) -> f32 { + if d < robot_radius_m { + return f32::INFINITY; + } + let scale = buffer_m.max(1e-3); + 1.0 + weight * (-(d - robot_radius_m) / scale).exp() } #[cfg(test)] @@ -302,7 +353,7 @@ mod tests { let mut sc = build_cells(&open_patch(0, 0, 10), 2); let mut state = DijkstraState::default(); let mut nodes = Vec::new(); - place_nodes(&mut sc, VOXEL, 1.0, 0.3, &mut state, &mut nodes); + place_nodes(&mut sc, VOXEL, 1.0, 0.3, 0.0, 1.0, &mut state, &mut nodes); assert!(!nodes.is_empty()); for n in &nodes { let (ix, iy, _) = sc.coord(n.cell_id); @@ -321,7 +372,7 @@ mod tests { let mut sc = build_cells(&cells_in, 2); let mut state = DijkstraState::default(); let mut nodes = Vec::new(); - place_nodes(&mut sc, VOXEL, 1.0, 0.3, &mut state, &mut nodes); + place_nodes(&mut sc, VOXEL, 1.0, 0.3, 0.0, 1.0, &mut state, &mut nodes); assert!(!nodes.is_empty()); } @@ -332,7 +383,7 @@ mod tests { let mut sc = build_cells(&cells_in, 2); let mut state = DijkstraState::default(); let mut nodes = Vec::new(); - place_nodes(&mut sc, VOXEL, 1.0, 0.3, &mut state, &mut nodes); + place_nodes(&mut sc, VOXEL, 1.0, 0.3, 0.0, 1.0, &mut state, &mut nodes); assert!(nodes.len() >= 2); for i in 0..nodes.len() { for j in (i + 1)..nodes.len() { @@ -353,7 +404,7 @@ mod tests { let mut sc = build_cells(&cells_in, 2); let mut state = DijkstraState::default(); let mut nodes = Vec::new(); - place_nodes(&mut sc, VOXEL, 1.0, 0.3, &mut state, &mut nodes); + place_nodes(&mut sc, VOXEL, 1.0, 0.3, 0.0, 1.0, &mut state, &mut nodes); let id0 = sc.id((0, 0, 0)).unwrap(); let outbound = sc.neighbors(id0); assert!(!outbound.is_empty()); diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs index e60eb50eff..4a449cc067 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs @@ -76,6 +76,7 @@ fn best_iz_in_column( /// Plan path from start pose to goal pose using the node graph. /// Returns none if either of the poses can't be snapped to surface or if /// there is no valid path. +#[allow(clippy::too_many_arguments)] pub fn plan( plg: &PlannerGraph, start_pose: (f32, f32, f32), @@ -84,6 +85,7 @@ pub fn plan( z_tolerance_m: f32, node_spacing_m: f32, node_step_threshold_m: f32, + node_wall_buffer_m: f32, ) -> Option> { let start_coord = snap_pose_to_cell(&plg.surface_lookup, start_pose, voxel_size, z_tolerance_m)?; @@ -117,7 +119,7 @@ pub fn plan( let smooth_tol_cells = ((node_step_threshold_m / voxel_size).round() as i32).max(1); let cells = assemble_cells(plg, &node_seq, &lead_in, &goal_segment); - let cells = string_pull(plg, &cells, smooth_tol_cells); + let cells = string_pull(plg, &cells, smooth_tol_cells, node_wall_buffer_m); Some(cells_to_waypoints( plg, &cells, start_pose, goal_pose, voxel_size, )) @@ -333,8 +335,10 @@ fn cells_to_waypoints( } /// Shortcut runs of cells with straight on-surface segments, keeping the -/// farthest cell in line of sight from each anchor. -fn string_pull(plg: &PlannerGraph, cells: &[CellId], tol_cells: i32) -> Vec { +/// farthest cell in line of sight from each anchor. A shortcut is only taken +/// when it never passes closer than buffer_m to a wall, so smoothing cannot +/// erode the wall clearance the penalized routing built in. +fn string_pull(plg: &PlannerGraph, cells: &[CellId], tol_cells: i32, buffer_m: f32) -> Vec { if cells.len() <= 2 { return cells.to_vec(); } @@ -346,7 +350,7 @@ fn string_pull(plg: &PlannerGraph, cells: &[CellId], tol_cells: i32) -> Vec Vec bool { let (dx, dy, dz) = (b.0 - a.0, b.1 - a.1, b.2 - a.2); let samples = dx.abs().max(dy.abs()) * 2; @@ -383,15 +389,34 @@ fn los_on_surface( last_ix = ix; last_iy = iy; let iz_line = a.2 as f32 + t * dz as f32; - let Some(zs) = surface_lookup.get(&(ix, iy)) else { + let Some(zs) = plg.surface_lookup.get(&(ix, iy)) else { return false; }; - if !zs - .iter() - .any(|&iz| (iz as f32 - iz_line).abs() <= tol_cells as f32) - { + // Surface cell in this column nearest the interpolated segment height. + let mut nearest: Option<(f32, i32)> = None; + for &iz in zs { + let d = (iz as f32 - iz_line).abs(); + if nearest.is_none_or(|(bd, _)| d < bd) { + nearest = Some((d, iz)); + } + } + let Some((d, iz)) = nearest else { + return false; + }; + if d > tol_cells as f32 { return false; } + if let Some(id) = plg.cells.id((ix, iy, iz)) { + let wall_dist = plg + .wall_state + .dist + .get(id as usize) + .copied() + .unwrap_or(f32::INFINITY); + if wall_dist < buffer_m { + return false; + } + } } true } @@ -469,7 +494,7 @@ mod tests { start: (f32, f32, f32), goal: (f32, f32, f32), ) -> Option> { - plan(plg, start, goal, VOXEL, Z_TOL, 1.0, 0.25) + plan(plg, start, goal, VOXEL, Z_TOL, 1.0, 0.25, 0.3) } #[test] @@ -574,10 +599,11 @@ mod tests { for pair in wp[1..wp.len() - 1].windows(2) { assert!( los_on_surface( - &plg.surface_lookup, + &plg, waypoint_key(&pair[0]), waypoint_key(&pair[1]), - tol + tol, + 0.3 ), "segment {:?} -> {:?} leaves the surface", pair[0], @@ -605,6 +631,33 @@ mod tests { ); } + #[test] + fn string_pull_refuses_shortcut_through_sub_buffer_cell() { + // Straight strip: with open clearance the run collapses to its + // endpoints. Drop one mid cell below the buffer and the shortcut + // spanning it is refused, so the smoothed path retains that cell. + let mut plg = PlannerGraph::new(); + build_surface_lookup(&strip(10), &mut plg.surface_lookup); + build_surface_cells(&mut plg.cells, &plg.surface_lookup, VOXEL, 2); + let path: Vec = (0..10).map(|x| plg.cells.id((x, 0, 0)).unwrap()).collect(); + + plg.wall_state.dist = vec![f32::INFINITY; plg.cells.slot_capacity()]; + let open = string_pull(&plg, &path, 1, 0.3); + assert_eq!(open.len(), 2, "open strip should collapse to its endpoints"); + + let mid = plg.cells.id((5, 0, 0)).unwrap(); + plg.wall_state.dist[mid as usize] = 0.1; + let guarded = string_pull(&plg, &path, 1, 0.3); + assert!( + guarded.len() > 2, + "shortcut across a sub-buffer cell must be refused: {guarded:?}" + ); + assert!( + guarded.contains(&mid), + "smoothed path must still traverse the low-clearance cell" + ); + } + #[test] fn plan_enters_on_goalward_node_not_nearest() { // Robot sits past node 2 toward the goal. Entry must skip it for node 10. diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/python.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/python.rs index 13fdac5f57..d64160d573 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/python.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/python.rs @@ -21,6 +21,7 @@ pub struct MLSPlanner { #[pymethods] impl MLSPlanner { #[new] + #[allow(clippy::too_many_arguments)] #[pyo3(signature = ( *, voxel_size, @@ -30,6 +31,8 @@ impl MLSPlanner { node_spacing_m = 1.0, node_wall_buffer_m = 0.3, node_step_threshold_m = 0.25, + robot_radius_m = 0.2, + wall_penalty_weight = 4.0, ))] fn new( voxel_size: f32, @@ -39,6 +42,8 @@ impl MLSPlanner { node_spacing_m: f32, node_wall_buffer_m: f32, node_step_threshold_m: f32, + robot_radius_m: f32, + wall_penalty_weight: f32, ) -> PyResult { let config = Config { world_frame: String::new(), @@ -49,6 +54,8 @@ impl MLSPlanner { node_spacing_m, node_wall_buffer_m, node_step_threshold_m, + robot_radius_m, + wall_penalty_weight, }; config .validate() @@ -150,6 +157,28 @@ impl MLSPlanner { .into_pyarray(py) } + /// Surface cells as `(M, 4)` float32 rows `[x, y, z, clearance]`, where + /// clearance is the distance to the nearest untraversable edge. + fn surface_clearance_map<'py>(&self, py: Python<'py>) -> Bound<'py, PyArray2> { + let voxel_size = self.config.voxel_size; + let cells = self.planner.surface_clearance(); + let values: Vec = py.allow_threads(|| { + let mut out: Vec = Vec::with_capacity(cells.len() * 4); + for ((ix, iy, iz), clearance) in cells { + let (x, y, z) = surface_point_xyz(ix, iy, iz, voxel_size); + out.push(x); + out.push(y); + out.push(z); + out.push(clearance); + } + out + }); + let n = values.len() / 4; + Array2::from_shape_vec((n, 4), values) + .expect("4 elements pushed per cell") + .into_pyarray(py) + } + fn nodes<'py>(&self, py: Python<'py>) -> Bound<'py, PyArray2> { let graph = self.planner.graph(); let positions: Vec = py.allow_threads(|| { diff --git a/dimos/navigation/nav_3d/mls_planner/transformer.py b/dimos/navigation/nav_3d/mls_planner/transformer.py index ab7ef5a10c..ed606092a0 100644 --- a/dimos/navigation/nav_3d/mls_planner/transformer.py +++ b/dimos/navigation/nav_3d/mls_planner/transformer.py @@ -97,7 +97,7 @@ def __call__( tags={ **obs.tags, "voxel_map": planner.voxel_map(), - "surface_map": planner.surface_map(), + "surface_clearance": planner.surface_clearance_map(), "nodes": planner.nodes(), "node_edges": planner.node_edges(), "start": start, diff --git a/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py b/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py index 274ec83ee0..a860f86223 100644 --- a/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py +++ b/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py @@ -145,6 +145,18 @@ def _log_path(path: Path, entity: str) -> None: rr.log(entity, rr.LineStrips3D([points], colors=[[0, 255, 0]], radii=0.05)) +def _clearance_colors(clearance: np.ndarray, clamp_m: float) -> np.ndarray: + """Map per-cell wall clearance to a blue ramp, dark navy at low clearance + through light blue at high. The scale is clamped so the gradient resolves + near walls. Open cells with large or infinite clearance saturate light.""" + norm = np.nan_to_num(clearance / clamp_m, nan=1.0, posinf=1.0) + norm = np.clip(norm, 0.0, 1.0) + blocked = np.array([4.0, 8.0, 48.0]) + clear = np.array([150.0, 200.0, 255.0]) + rgb = blocked + norm[:, None] * (clear - blocked) + return rgb.astype(np.uint8) + + def main( dataset: str = typer.Argument(..., help="Dataset .db: bare name (cwd or data/) or path"), out: FsPath | None = typer.Option( @@ -161,8 +173,19 @@ def main( max_range: float = typer.Option(30.0, "--max-range", help="Max ray cast distance (m)"), ray_subsample: int = typer.Option(1, "--ray-subsample", help="Keep every Nth ray"), emit_every: int = typer.Option(1, "--emit-every", help="Replan every N lidar frames"), - robot_height: float = typer.Option(0.3, "--robot-height", help="Robot height (m)"), + robot_height: float = typer.Option(1.0, "--robot-height", help="Robot height (m)"), node_spacing: float = typer.Option(1.0, "--node-spacing", help="Graph node spacing (m)"), + node_wall_buffer: float = typer.Option( + 0.3, "--node-wall-buffer", help="Min wall clearance for nodes and smoothing (m)" + ), + robot_radius: float = typer.Option( + 0.2, + "--robot-radius", + help="Hard clearance floor; cells closer to a wall are impassable (m)", + ), + wall_penalty_weight: float = typer.Option( + 4.0, "--wall-penalty-weight", help="Soft wall-penalty strength at the robot radius" + ), goal: tuple[float, float, float] = typer.Option( (0.0, 0.0, 0.0), "--goal", @@ -172,6 +195,9 @@ def main( False, "--live", help="Also spawn the rerun viewer when --out is set" ), render_voxel: float = typer.Option(0.05, "--render-voxel", help="Rerun voxel render size (m)"), + clearance_clamp: float = typer.Option( + 1.0, "--clearance-clamp", help="Max clearance (m) for the surface color scale" + ), plot_out: FsPath | None = typer.Option( None, "--plot-out", help="Write an SVG timing/size plot here when the run ends" ), @@ -221,6 +247,9 @@ def main( voxel_size=voxel_size, robot_height=robot_height, node_spacing_m=node_spacing, + node_wall_buffer_m=node_wall_buffer, + robot_radius_m=robot_radius, + wall_penalty_weight=wall_penalty_weight, ) ) @@ -241,14 +270,18 @@ def main( if voxel_map.size: rr.log( "world/voxel_map", - rr.Points3D(voxel_map, colors=[[80, 80, 80]], radii=render_voxel / 2), + rr.Points3D(voxel_map, colors=[[180, 125, 125]], radii=render_voxel / 2), ) - surface = obs.tags["surface_map"] + surface = obs.tags["surface_clearance"] if surface.size: rr.log( "world/surface_map", - rr.Points3D(surface, colors=[[120, 120, 200]], radii=render_voxel / 2), + rr.Points3D( + surface[:, :3], + colors=_clearance_colors(surface[:, 3], clearance_clamp), + radii=render_voxel / 2, + ), ) nodes = obs.tags["nodes"] From a4b134fd20bb41d87ac70d826ccda0981ebb8710 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Thu, 11 Jun 2026 18:56:04 -0700 Subject: [PATCH 09/56] Make tests pass --- dimos/navigation/nav_3d/mls_planner/test_transformer.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/dimos/navigation/nav_3d/mls_planner/test_transformer.py b/dimos/navigation/nav_3d/mls_planner/test_transformer.py index 4b92f5cbef..afa0272a72 100644 --- a/dimos/navigation/nav_3d/mls_planner/test_transformer.py +++ b/dimos/navigation/nav_3d/mls_planner/test_transformer.py @@ -15,6 +15,9 @@ from __future__ import annotations import numpy as np +import pytest + +pytest.importorskip("dimos_mls_planner") from dimos.memory2.type.observation import Observation from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 From 255d6dd5c08b40b318b1c71274e81cf98820dd81 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Thu, 11 Jun 2026 19:40:47 -0700 Subject: [PATCH 10/56] Incremental builds --- dimos/mapping/ray_tracing/module.py | 11 ++- dimos/mapping/ray_tracing/rust/src/main.rs | 92 ++++++++++++++--- dimos/mapping/ray_tracing/rust/src/python.rs | 1 + .../ray_tracing/rust/src/voxel_ray_tracer.rs | 9 ++ .../nav_3d/mls_planner/mls_planner_native.py | 8 +- .../nav_3d/mls_planner/rust/src/main.rs | 98 ++++++++++++++++--- 6 files changed, 191 insertions(+), 28 deletions(-) diff --git a/dimos/mapping/ray_tracing/module.py b/dimos/mapping/ray_tracing/module.py index 5b215f9ff6..8224d25ea2 100644 --- a/dimos/mapping/ray_tracing/module.py +++ b/dimos/mapping/ray_tracing/module.py @@ -19,6 +19,7 @@ from dimos.core.native_module import NativeModule, NativeModuleConfig from dimos.core.stream import In, Out +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.spec import mapping @@ -47,10 +48,17 @@ class RayTracingVoxelMapConfig(NativeModuleConfig): graze_cos: float = 0.7 # Only spare a voxel whose neighborhood was hit within this many frames. recency_window: int = 15 + # Integrate every frame, publish maps every Nth frame. + emit_every: int = 1 class RayTracingVoxelMap(NativeModule, mapping.GlobalPointcloud): - """Rust voxel-map module with raycast clearing of dynamic objects.""" + """Rust voxel-map module with raycast clearing of dynamic objects. + + region_bounds describes the cylinder local_map covers, packed into a + PoseStamped. Position holds the center, orientation holds + (radius, z_min, z_max, 0). It shares the local_map stamp. + """ config: RayTracingVoxelMapConfig @@ -58,6 +66,7 @@ class RayTracingVoxelMap(NativeModule, mapping.GlobalPointcloud): odometry: In[Odometry] global_map: Out[PointCloud2] local_map: Out[PointCloud2] + region_bounds: Out[PoseStamped] # Verify protocol port compliance (mypy will flag missing ports) diff --git a/dimos/mapping/ray_tracing/rust/src/main.rs b/dimos/mapping/ray_tracing/rust/src/main.rs index d8de5f42ab..e9d3914098 100644 --- a/dimos/mapping/ray_tracing/rust/src/main.rs +++ b/dimos/mapping/ray_tracing/rust/src/main.rs @@ -8,6 +8,7 @@ use dimos_module::{error_throttled, run, warn_throttled, Input, LcmTransport, Mo use dimos_voxel_ray_tracing::voxel_ray_tracer::{ iter_global_points, update_map, Config, LocalBounds, VoxelKey, VoxelMap, }; +use lcm_msgs::geometry_msgs::{Point, Pose, PoseStamped, Quaternion}; use lcm_msgs::nav_msgs::Odometry; use lcm_msgs::sensor_msgs::{PointCloud2, PointField}; use lcm_msgs::std_msgs::{Header, Time}; @@ -26,11 +27,19 @@ struct RayTracingVoxelMap { #[output(encode = PointCloud2::encode)] local_map: Output, + // Region the local_map covers, packed into a PoseStamped: position holds + // the cylinder center, orientation holds (radius, z_min, z_max, 0). + // Stamped identically to local_map so consumers can pair them. + #[output(encode = PoseStamped::encode)] + region_bounds: Output, + #[config] config: Config, map: VoxelMap, last_origin: Option<(f32, f32, f32)>, + frame_count: u32, + batch_bbox: Option<([f32; 3], [f32; 3])>, } impl RayTracingVoxelMap { @@ -67,28 +76,83 @@ impl RayTracingVoxelMap { let live = update_map(&mut self.map, origin, &points, &self.config); + // Grow the batch bbox over this frame's live voxels and the origin, + // so the emitted region covers everything touched since the last emit. let half = voxel_size * 0.5; - let mut z_min = f32::INFINITY; - let mut z_max = f32::NEG_INFINITY; - let mut r_xy_max_sq = 0.0_f32; + let (mut lo, mut hi) = self + .batch_bbox + .unwrap_or(([f32::INFINITY; 3], [f32::NEG_INFINITY; 3])); + let mut grow = |x: f32, y: f32, z: f32| { + lo[0] = lo[0].min(x); + lo[1] = lo[1].min(y); + lo[2] = lo[2].min(z); + hi[0] = hi[0].max(x); + hi[1] = hi[1].max(y); + hi[2] = hi[2].max(z); + }; + grow(origin.0, origin.1, origin.2); for &(kx, ky, kz) in &live { - let cx = kx as f32 * voxel_size + half; - let cy = ky as f32 * voxel_size + half; - let cz = kz as f32 * voxel_size + half; - z_min = z_min.min(cz); - z_max = z_max.max(cz); - let dx = cx - origin.0; - let dy = cy - origin.1; - r_xy_max_sq = r_xy_max_sq.max(dx * dx + dy * dy); + grow( + kx as f32 * voxel_size + half, + ky as f32 * voxel_size + half, + kz as f32 * voxel_size + half, + ); + } + self.batch_bbox = Some((lo, hi)); + + self.frame_count += 1; + if !self.frame_count.is_multiple_of(self.config.emit_every) { + return; } + let Some((lo, hi)) = self.batch_bbox.take() else { + return; + }; + + // Cylinder enclosing the batch bbox plus the clearing margin. + let margin = self.config.shadow_depth + voxel_size; + let cx = (lo[0] + hi[0]) * 0.5; + let cy = (lo[1] + hi[1]) * 0.5; + let rx = (hi[0] - lo[0]) * 0.5 + margin; + let ry = (hi[1] - lo[1]) * 0.5 + margin; + let radius = (rx * rx + ry * ry).sqrt(); + let z_min = lo[2] - margin; + let z_max = hi[2] + margin; let cylinder = LocalBounds { - origin_x: origin.0, - origin_y: origin.1, - r_xy_max_sq, + origin_x: cx, + origin_y: cy, + r_xy_max_sq: radius * radius, z_min, z_max, }; + let bounds_msg = PoseStamped { + header: Header { + seq: 0, + stamp: msg.header.stamp.clone(), + frame_id: msg.header.frame_id.clone(), + }, + pose: Pose { + position: Point { + x: cx as f64, + y: cy as f64, + z: 0.0, + }, + orientation: Quaternion { + x: radius as f64, + y: z_min as f64, + z: z_max as f64, + w: 0.0, + }, + }, + }; + if let Err(e) = self.region_bounds.publish(&bounds_msg).await { + error_throttled!( + Duration::from_secs(1), + error = %e, + "Region bounds failed to publish", + ); + } + let (global_cloud, local_cloud) = build_pointclouds( &self.map, &live, diff --git a/dimos/mapping/ray_tracing/rust/src/python.rs b/dimos/mapping/ray_tracing/rust/src/python.rs index ff49f5ed04..8ac13b3b16 100644 --- a/dimos/mapping/ray_tracing/rust/src/python.rs +++ b/dimos/mapping/ray_tracing/rust/src/python.rs @@ -54,6 +54,7 @@ impl VoxelRayMapper { max_health, graze_cos, recency_window, + emit_every: 1, }; config .validate() diff --git a/dimos/mapping/ray_tracing/rust/src/voxel_ray_tracer.rs b/dimos/mapping/ray_tracing/rust/src/voxel_ray_tracer.rs index 9487c85bbd..7eacaba2f4 100644 --- a/dimos/mapping/ray_tracing/rust/src/voxel_ray_tracer.rs +++ b/dimos/mapping/ray_tracing/rust/src/voxel_ray_tracer.rs @@ -33,6 +33,9 @@ pub struct Config { /// Only spare a voxel whose neighborhood was hit within this many frames. /// A stale voxel can be cleared, even if it's a grazing hit. Large disables it. pub recency_window: u32, + /// Integrate every frame, publish maps every Nth frame. + #[validate(range(min = 1))] + pub emit_every: u32, } fn validate_health_range(cfg: &Config) -> Result<(), ValidationError> { @@ -650,6 +653,7 @@ mod tests { max_health: 1, graze_cos: 0.5, recency_window: 60, + emit_every: 1, } } @@ -809,6 +813,7 @@ mod tests { max_health: 1, graze_cos: 0.5, recency_window: 60, + emit_every: 1, }; // Build the floor over a y band so it is a 2d plane, not a wire. let max_x = 25.0_f32; @@ -961,6 +966,7 @@ mod tests { max_health: 1, graze_cos: 0.5, recency_window: 60, + emit_every: 1, }; // Staircase @@ -1032,6 +1038,7 @@ mod tests { max_health: 1, graze_cos: 0.5, recency_window: 60, + emit_every: 1, }; // Flat floor from the sensor out to a vertical wall. @@ -1091,6 +1098,7 @@ mod tests { max_health: 1, graze_cos, recency_window: 60, + emit_every: 1, }; // Staircase topped by a flat landing and a back wall. @@ -1219,6 +1227,7 @@ mod tests { max_health: 1, graze_cos: 0.5, recency_window, + emit_every: 1, }; let (mut map, _) = build_surface(&floor, voxel_size, cfg.max_health); let row: Vec = map diff --git a/dimos/navigation/nav_3d/mls_planner/mls_planner_native.py b/dimos/navigation/nav_3d/mls_planner/mls_planner_native.py index b90ccba370..9d046e0237 100644 --- a/dimos/navigation/nav_3d/mls_planner/mls_planner_native.py +++ b/dimos/navigation/nav_3d/mls_planner/mls_planner_native.py @@ -44,11 +44,17 @@ class MLSPlannerNativeConfig(NativeModuleConfig): class MLSPlannerNative(NativeModule): - """Rust-backed MLS planner.""" + """Rust-backed MLS planner. + + Feed either global_map (full rebuild per message) or the local_map plus + region_bounds pair from RayTracingVoxelMap (incremental region updates). + """ config: MLSPlannerNativeConfig global_map: In[PointCloud2] + local_map: In[PointCloud2] + region_bounds: In[PoseStamped] start_pose: In[PoseStamped] goal_pose: In[PoseStamped] diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/main.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/main.rs index c26f62e954..ace74d35b3 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/main.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/main.rs @@ -4,20 +4,28 @@ use std::time::{Duration, Instant, SystemTime, UNIX_EPOCH}; use dimos_mls_planner::edges::{edges_to_segments, PlannerGraph}; -use dimos_mls_planner::mls_planner::{Config, Planner}; +use dimos_mls_planner::mls_planner::{Config, Planner, RegionBounds}; use dimos_mls_planner::voxel::surface_point_xyz; use dimos_module::{error_throttled, run, warn_throttled, Input, LcmTransport, Module, Output}; use lcm_msgs::geometry_msgs::{Point, Pose, PoseStamped, Quaternion}; use lcm_msgs::nav_msgs::Path; use lcm_msgs::sensor_msgs::{PointCloud2, PointField}; use lcm_msgs::std_msgs::{Header, Time}; -use tracing::info; +use tracing::{debug, info}; #[derive(Module)] struct MlsPlanner { #[input(decode = PointCloud2::decode, handler = on_global_map)] global_map: Input, + // Incremental path: a local map slice paired by stamp with the region + // bounds it covers (see the ray tracer's region_bounds output). + #[input(decode = PointCloud2::decode, handler = on_local_map)] + local_map: Input, + + #[input(decode = PoseStamped::decode, handler = on_region_bounds)] + region_bounds: Input, + #[input(decode = PoseStamped::decode, handler = on_start_pose)] start_pose: Input, @@ -41,6 +49,8 @@ struct MlsPlanner { planner: Planner, latest_start: Option<(f32, f32, f32)>, + pending_local: Option, + pending_bounds: Option, } impl MlsPlanner { @@ -64,6 +74,76 @@ impl MlsPlanner { self.planner.update_global_map(&points, &self.config); let rebuild_ms = ms(t.elapsed()); + self.publish_graph().await; + + debug!( + global_map_points = points.len(), + voxels = self.planner.voxel_count(), + surface_cells = self.planner.surface().len(), + nodes = self.planner.graph().nodes.len(), + edges = self.planner.graph().node_edges.len(), + rebuild_ms, + "global_map processed", + ); + } + + async fn on_local_map(&mut self, msg: PointCloud2) { + self.pending_local = Some(msg); + self.try_region_update().await; + } + + async fn on_region_bounds(&mut self, msg: PoseStamped) { + self.pending_bounds = Some(msg); + self.try_region_update().await; + } + + /// Run the incremental update once a local map and its bounds with + /// matching stamps are both in hand. + async fn try_region_update(&mut self) { + let (Some(bounds_msg), Some(cloud)) = (&self.pending_bounds, &self.pending_local) else { + return; + }; + if !same_stamp(&bounds_msg.header.stamp, &cloud.header.stamp) { + return; + } + let bounds_msg = self.pending_bounds.take().expect("checked above"); + let cloud = self.pending_local.take().expect("checked above"); + + let points = match extract_xyz(&cloud) { + Ok(p) => p, + Err(e) => { + warn_throttled!( + Duration::from_secs(1), + error = %e, + "Failed to extract local map points, dropped a region update.", + ); + return; + } + }; + let bounds = RegionBounds { + origin_x: bounds_msg.pose.position.x as f32, + origin_y: bounds_msg.pose.position.y as f32, + radius: bounds_msg.pose.orientation.x as f32, + z_min: bounds_msg.pose.orientation.y as f32, + z_max: bounds_msg.pose.orientation.z as f32, + }; + + let t = Instant::now(); + self.planner.update_region(&points, &bounds, &self.config); + let update_ms = ms(t.elapsed()); + + self.publish_graph().await; + + debug!( + local_points = points.len(), + voxels = self.planner.voxel_count(), + nodes = self.planner.graph().nodes.len(), + update_ms, + "local region processed", + ); + } + + async fn publish_graph(&self) { let voxel_size = self.config.voxel_size; let frame = &self.config.world_frame; let graph = self.planner.graph(); @@ -81,16 +161,6 @@ impl MlsPlanner { let edges_path = build_segments_path(graph, voxel_size, frame, now()); publish_path(&self.node_edges, &edges_path).await; - - info!( - global_map_points = points.len(), - voxels = self.planner.voxel_count(), - surface_cells = self.planner.surface().len(), - nodes = graph.nodes.len(), - edges = graph.node_edges.len(), - rebuild_ms, - "global_map processed", - ); } async fn on_start_pose(&mut self, msg: PoseStamped) { @@ -132,6 +202,10 @@ fn ms(d: Duration) -> f64 { d.as_secs_f64() * 1000.0 } +fn same_stamp(a: &Time, b: &Time) -> bool { + a.sec == b.sec && a.nsec == b.nsec +} + async fn publish_cloud( out: &Output, points: &[(f32, f32, f32)], From f226179dbddc5bd1e5919855eea2734c36d474a7 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Thu, 11 Jun 2026 19:56:04 -0700 Subject: [PATCH 11/56] Fixes --- dimos/mapping/ray_tracing/test_transformer.py | 43 +++- dimos/mapping/ray_tracing/transformer.py | 4 +- .../nav_3d/mls_planner/rust/src/main.rs | 5 +- .../mls_planner/rust/src/mls_planner.rs | 29 +-- .../nav_3d/mls_planner/rust/src/nodes.rs | 6 +- .../nav_3d/mls_planner/rust/src/planner.rs | 30 ++- .../nav_3d/mls_planner/rust/src/python.rs | 7 +- .../nav_3d/mls_planner/test_transformer.py | 23 +- .../nav_3d/mls_planner/utils/plan_rrd.py | 207 +++++++----------- pyproject.toml | 2 + 10 files changed, 181 insertions(+), 175 deletions(-) diff --git a/dimos/mapping/ray_tracing/test_transformer.py b/dimos/mapping/ray_tracing/test_transformer.py index ce622f738e..7210c21eb7 100644 --- a/dimos/mapping/ray_tracing/test_transformer.py +++ b/dimos/mapping/ray_tracing/test_transformer.py @@ -50,6 +50,16 @@ def test_emit_every_n_yields_on_cadence_and_flushes_remainder() -> None: assert [r.tags["frame_count"] for r in results] == [3, 6, 7] +def test_pose_propagates_to_emitted_obs() -> None: + pose = (1.5, -2.0, 0.5) + obs = _obs(_cube(), ts=1.0, pose=pose) + + [emitted] = list(RayTraceMap()(iter([obs]))) + + assert emitted.pose_tuple is not None + assert emitted.pose_tuple[:3] == pose + + def test_poseless_obs_are_skipped() -> None: points = _cube() poseless = Observation(id=1, ts=0.0, pose=None, _data=PointCloud2.from_numpy(points)) @@ -60,6 +70,33 @@ def test_poseless_obs_are_skipped() -> None: assert [r.tags["frame_count"] for r in results] == [1] -def test_negative_emit_every_is_rejected() -> None: - with pytest.raises(ValueError): - RayTraceMap(emit_every=-1) +def _ring( + center: tuple[float, float], radius: float, z: float, n: int = 100 +) -> NDArray[np.float32]: + angles = np.linspace(0.0, 2.0 * np.pi, n, endpoint=False) + xs = center[0] + radius * np.cos(angles) + ys = center[1] + radius * np.sin(angles) + zs = np.full_like(xs, z) + return np.stack([xs, ys, zs], axis=1).astype(np.float32) + + +def test_emit_local_tags_region_bounds_around_origin() -> None: + margin = 0.2 + 0.1 + obs = _obs(_ring((2.0, 3.0), radius=1.0, z=0.0), ts=1.0, pose=(2.0, 3.0, 0.5)) + + [emitted] = list(RayTraceMap(emit_local=True)(iter([obs]))) + + cx, cy, radius, z_min, z_max = emitted.tags["region_bounds"] + assert (cx, cy) == pytest.approx((2.0, 3.0)) + assert radius == pytest.approx(1.0 + margin) + assert z_min == pytest.approx(0.0 - margin) + assert z_max == pytest.approx(0.0 + margin) + + +def test_emit_local_empty_frame_yields_zero_radius_region_at_robot() -> None: + empty = np.empty((0, 3), dtype=np.float32) + obs = _obs(empty, ts=1.0, pose=(1.0, 2.0, 3.0)) + + [emitted] = list(RayTraceMap(emit_local=True)(iter([obs]))) + + assert emitted.tags["region_bounds"] == pytest.approx((1.0, 2.0, 0.0, 3.0, 3.0)) diff --git a/dimos/mapping/ray_tracing/transformer.py b/dimos/mapping/ray_tracing/transformer.py index 1ec566e728..71d2a12051 100644 --- a/dimos/mapping/ray_tracing/transformer.py +++ b/dimos/mapping/ray_tracing/transformer.py @@ -72,7 +72,9 @@ def _local_bounds( ) points = points[np.isfinite(points).all(axis=1)] if points.size == 0: - rx, ry, rz, *_ = last_obs.pose_tuple + pose = last_obs.pose_tuple + assert pose is not None, "poseless obs are skipped upstream" + rx, ry, rz = pose[:3] return rx, ry, 0.0, rz, rz origins = np.asarray(batch_origins, dtype=np.float64) diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/main.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/main.rs index ace74d35b3..497a43534e 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/main.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/main.rs @@ -79,7 +79,7 @@ impl MlsPlanner { debug!( global_map_points = points.len(), voxels = self.planner.voxel_count(), - surface_cells = self.planner.surface().len(), + surface_cells = self.planner.surface().count(), nodes = self.planner.graph().nodes.len(), edges = self.planner.graph().node_edges.len(), rebuild_ms, @@ -151,8 +151,7 @@ impl MlsPlanner { let surface_points: Vec<(f32, f32, f32)> = self .planner .surface() - .iter() - .map(|&(ix, iy, iz)| surface_point_xyz(ix, iy, iz, voxel_size)) + .map(|(ix, iy, iz)| surface_point_xyz(ix, iy, iz, voxel_size)) .collect(); publish_cloud(&self.surface_map, &surface_points, frame, now()).await; diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs index cb926b514f..0b477b5f79 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs @@ -63,8 +63,7 @@ pub struct RebuildTimings { pub graph_ms: f64, } -/// Cylindrical region the planner re-derives from a local map slice. The xy -/// test mirrors the ray tracer so the slice is an exact replacement. +/// Cylindrical region the planner re-derives from a local map slice. pub struct RegionBounds { pub origin_x: f32, pub origin_y: f32, @@ -424,30 +423,18 @@ impl Planner { if self.graph.nodes.is_empty() { return None; } - planner::plan( - &self.graph, - start, - goal, - config.voxel_size, - config.robot_height, - config.node_spacing_m, - config.node_step_threshold_m, - config.node_wall_buffer_m, - ) + planner::plan(&self.graph, start, goal, config) } pub fn graph(&self) -> &PlannerGraph { &self.graph } - pub fn surface(&self) -> Vec { - let mut out: Vec = Vec::new(); - for (&(ix, iy), zs) in &self.graph.surface_lookup { - for &iz in zs { - out.push((ix, iy, iz)); - } - } - out + pub fn surface(&self) -> impl Iterator + '_ { + self.graph + .surface_lookup + .iter() + .flat_map(|(&(ix, iy), zs)| zs.iter().map(move |&iz| (ix, iy, iz))) } /// Surface cells paired with their wall clearance, the distance to the @@ -559,7 +546,7 @@ mod region_tests { } fn surface_set(p: &Planner) -> BTreeSet { - p.surface().into_iter().collect() + p.surface().collect() } fn voxel_set(p: &Planner) -> BTreeSet { diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/nodes.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/nodes.rs index 8cde46c211..bbe4ddf408 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/nodes.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/nodes.rs @@ -310,10 +310,8 @@ fn scale_edges( } } -/// Cost multiplier for a cell at wall distance d. Cells closer than the robot -/// radius are impassable. Beyond it the penalty decays smoothly from -/// 1 + weight at the radius toward 1, with length scale buffer_m, so paths are -/// pushed toward open space with no hard cap and no flat zone. +/// Cost multiplier at wall distance d. Infinite inside the robot radius, +/// then decays from 1 + weight toward 1 with length scale buffer_m. #[inline] fn penalty_of(d: f32, buffer_m: f32, robot_radius_m: f32, weight: f32) -> f32 { if d < robot_radius_m { diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs index 4a449cc067..ed00525e9d 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs @@ -9,6 +9,7 @@ use ahash::{AHashMap, AHashSet}; use crate::adjacency::{CellId, SurfaceCells, SurfaceLookup}; use crate::dijkstra::walk_preds; use crate::edges::{NodeEdgeIdx, NodeId, PlannerGraph, NO_NODE}; +use crate::mls_planner::Config; use crate::voxel::{surface_point_xyz, VoxelKey}; /// Robot-rooted candidate search radius, in multiples of node spacing. @@ -76,17 +77,14 @@ fn best_iz_in_column( /// Plan path from start pose to goal pose using the node graph. /// Returns none if either of the poses can't be snapped to surface or if /// there is no valid path. -#[allow(clippy::too_many_arguments)] pub fn plan( plg: &PlannerGraph, start_pose: (f32, f32, f32), goal_pose: (f32, f32, f32), - voxel_size: f32, - z_tolerance_m: f32, - node_spacing_m: f32, - node_step_threshold_m: f32, - node_wall_buffer_m: f32, + config: &Config, ) -> Option> { + let voxel_size = config.voxel_size; + let z_tolerance_m = config.robot_height; let start_coord = snap_pose_to_cell(&plg.surface_lookup, start_pose, voxel_size, z_tolerance_m)?; let goal_coord = snap_pose_to_cell(&plg.surface_lookup, goal_pose, voxel_size, z_tolerance_m)?; @@ -104,7 +102,7 @@ pub fn plan( // Rooted at the goal so one pass covers every node's cost-to-go. let (cost_to_go, pred_to_goal) = node_dijkstra(plg, goal_node); - let radius = (node_spacing_m * CANDIDATE_RADIUS_FACTOR).max(voxel_size); + let radius = (config.node_spacing_m * CANDIDATE_RADIUS_FACTOR).max(voxel_size); let (lead_in, node_seq) = select_entry( plg, start_cell, @@ -116,10 +114,10 @@ pub fn plan( )?; // Shortcut height tolerance in cells, tied to the traversable step. - let smooth_tol_cells = ((node_step_threshold_m / voxel_size).round() as i32).max(1); + let smooth_tol_cells = ((config.node_step_threshold_m / voxel_size).round() as i32).max(1); let cells = assemble_cells(plg, &node_seq, &lead_in, &goal_segment); - let cells = string_pull(plg, &cells, smooth_tol_cells, node_wall_buffer_m); + let cells = string_pull(plg, &cells, smooth_tol_cells, config.node_wall_buffer_m); Some(cells_to_waypoints( plg, &cells, start_pose, goal_pose, voxel_size, )) @@ -494,7 +492,19 @@ mod tests { start: (f32, f32, f32), goal: (f32, f32, f32), ) -> Option> { - plan(plg, start, goal, VOXEL, Z_TOL, 1.0, 0.25, 0.3) + let config = Config { + world_frame: "world".into(), + voxel_size: VOXEL, + robot_height: Z_TOL, + surface_dilation_passes: 0, + surface_erosion_passes: 0, + node_spacing_m: 1.0, + node_wall_buffer_m: 0.3, + node_step_threshold_m: 0.25, + robot_radius_m: 0.2, + wall_penalty_weight: 4.0, + }; + plan(plg, start, goal, &config) } #[test] diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/python.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/python.rs index d64160d573..376f98e206 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/python.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/python.rs @@ -11,6 +11,7 @@ use validator::Validate; use crate::edges::edges_to_segments; use crate::mls_planner::{Config, Planner, RegionBounds}; use crate::voxel::surface_point_xyz; +use crate::voxel::VoxelKey; #[pyclass] pub struct MLSPlanner { @@ -140,7 +141,7 @@ impl MLSPlanner { fn surface_map<'py>(&self, py: Python<'py>) -> Bound<'py, PyArray2> { let voxel_size = self.config.voxel_size; - let surface = self.planner.surface(); + let surface: Vec = self.planner.surface().collect(); let positions: Vec = py.allow_threads(|| { let mut out: Vec = Vec::with_capacity(surface.len() * 3); for (ix, iy, iz) in surface { @@ -157,7 +158,7 @@ impl MLSPlanner { .into_pyarray(py) } - /// Surface cells as `(M, 4)` float32 rows `[x, y, z, clearance]`, where + /// Surface cells as (M, 4) float32 rows of x, y, z, clearance, where /// clearance is the distance to the nearest untraversable edge. fn surface_clearance_map<'py>(&self, py: Python<'py>) -> Bound<'py, PyArray2> { let voxel_size = self.config.voxel_size; @@ -281,7 +282,7 @@ impl MLSPlanner { format!( "MLSPlanner(voxel_size={}, surface_cells={}, nodes={}, edges={})", self.config.voxel_size, - self.planner.surface().len(), + self.planner.surface().count(), graph.nodes.len(), graph.node_edges.len(), ) diff --git a/dimos/navigation/nav_3d/mls_planner/test_transformer.py b/dimos/navigation/nav_3d/mls_planner/test_transformer.py index afa0272a72..d808591e1d 100644 --- a/dimos/navigation/nav_3d/mls_planner/test_transformer.py +++ b/dimos/navigation/nav_3d/mls_planner/test_transformer.py @@ -15,6 +15,7 @@ from __future__ import annotations import numpy as np +from numpy.typing import NDArray import pytest pytest.importorskip("dimos_mls_planner") @@ -25,7 +26,7 @@ def _obs( - points: np.ndarray, + points: NDArray[np.float32], pose: tuple[float, float, float], region_bounds: tuple[float, float, float, float, float], ) -> Observation[PointCloud2]: @@ -38,6 +39,26 @@ def _obs( ) +def _flat_floor(half_extent: float = 3.0, spacing: float = 0.1) -> NDArray[np.float32]: + coords = np.arange(-half_extent, half_extent, spacing, dtype=np.float32) + xs, ys = np.meshgrid(coords, coords) + zs = np.zeros_like(xs) + return np.stack([xs.ravel(), ys.ravel(), zs.ravel()], axis=1) + + +def test_flat_floor_yields_populated_path_and_planned_true() -> None: + obs = _obs( + _flat_floor(), + pose=(-2.0, -2.0, 1.0), + region_bounds=(0.0, 0.0, 5.0, -1.0, 2.0), + ) + + [out] = list(MLSPlan(goal=(2.0, 2.0, 0.0), voxel_size=0.2, robot_height=1.0)(iter([obs]))) + + assert out.tags["planned"] is True + assert len(out.data.poses) >= 2 + + def test_start_z_is_dropped_by_robot_height() -> None: obs = _obs( np.zeros((1, 3), dtype=np.float32), diff --git a/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py b/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py index a860f86223..81629debf7 100644 --- a/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py +++ b/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py @@ -17,9 +17,10 @@ from __future__ import annotations from pathlib import Path as FsPath -import re -from typing import TYPE_CHECKING +from typing import Any +import numpy as np +from numpy.typing import NDArray import rerun as rr import typer @@ -29,21 +30,14 @@ from dimos.memory2.stream import Stream from dimos.memory2.transform import FnTransformer from dimos.memory2.type.observation import Observation -from dimos.memory2.vis.plot.plot import Plot from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.nav_msgs.Path import Path from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2, register_colormap_annotation from dimos.navigation.nav_3d.mls_planner.transformer import MLSPlan from dimos.utils.data import resolve_named_path -if TYPE_CHECKING: - import numpy as np - from numpy.typing import NDArray - TIMELINE = "ts" -PairObs = Observation[tuple[Observation[PointCloud2], Observation[Odometry]]] - TIMING_KEYS = ["voxelize_ms", "surfaces_ms", "graph_ms", "plan_ms", "total_ms"] SIZE_KEYS = ["voxels", "surface_cells", "nodes", "edges"] @@ -65,54 +59,9 @@ def _print_summary(streams: dict[str, dict[str, Stream[float]]]) -> None: print(f" {kind}/{key:<14} {mean:9.2f} {p50:9.2f} {p95:9.2f} {peak:9.2f}") -def _stitch_svgs(svgs: list[str]) -> str: - """Stack standalone SVGs vertically into one, namespacing each panel's ids - so matplotlib's reused ids do not collide.""" - panels: list[str] = [] - widths: list[float] = [] - offset = 0.0 - for i, svg in enumerate(svgs): - body = svg[svg.index("\n' - '\n' + "\n".join(panels) + "\n\n" - ) - - -def _save_plot(timing_streams: dict[str, Stream[float]], voxels: Stream[float], path: str) -> None: - panels: list[str] = [] - for key in TIMING_KEYS: - plot = Plot() - plot.add(timing_streams[key], label=key, connect=None) - panels.append(plot.to_svg()) - voxel_plot = Plot() - voxel_plot.add(voxels, label="voxels", connect=None) - panels.append(voxel_plot.to_svg()) - with open(path, "w") as f: - f.write(_stitch_svgs(panels)) - print(f"wrote {path}") - - -def _attach_pose_from_odom(pair_obs: PairObs) -> Observation[PointCloud2]: - lidar_obs = pair_obs.data[0] - odom_obs = pair_obs.data[1] +def _attach_pose_from_odom(pair_obs: Observation[Any]) -> Observation[PointCloud2]: + lidar_obs: Observation[PointCloud2] = pair_obs.data[0] + odom_obs: Observation[Odometry] = pair_obs.data[1] odom = odom_obs.data pose_tuple = ( float(odom.position.x), @@ -145,18 +94,79 @@ def _log_path(path: Path, entity: str) -> None: rr.log(entity, rr.LineStrips3D([points], colors=[[0, 255, 0]], radii=0.05)) -def _clearance_colors(clearance: np.ndarray, clamp_m: float) -> np.ndarray: - """Map per-cell wall clearance to a blue ramp, dark navy at low clearance - through light blue at high. The scale is clamped so the gradient resolves - near walls. Open cells with large or infinite clearance saturate light.""" - norm = np.nan_to_num(clearance / clamp_m, nan=1.0, posinf=1.0) - norm = np.clip(norm, 0.0, 1.0) - blocked = np.array([4.0, 8.0, 48.0]) - clear = np.array([150.0, 200.0, 255.0]) - rgb = blocked + norm[:, None] * (clear - blocked) +def _clearance_colors(clearance: NDArray[np.float32], clamp_m: float) -> NDArray[np.uint8]: + """Map per-cell wall clearance to a blue ramp, clamped so it resolves near walls.""" + norm = np.clip(np.nan_to_num(clearance / clamp_m, nan=1.0, posinf=1.0), 0.0, 1.0) + blocked = np.array([4.0, 8.0, 48.0], dtype=np.float64) + clear = np.array([150.0, 200.0, 255.0], dtype=np.float64) + rgb: NDArray[np.float64] = blocked + norm[:, None] * (clear - blocked) return rgb.astype(np.uint8) +def _log_frame( + obs: Observation[Path], + render_voxel: float, + clearance_clamp: float, + timing_streams: dict[str, Stream[float]], + size_streams: dict[str, Stream[float]], +) -> None: + rr.set_time(TIMELINE, timestamp=obs.ts) + + start = obs.tags["start"] + rr.log("world/start", rr.Points3D([start], colors=[[0, 255, 0]], radii=0.1)) + + voxel_map = obs.tags["voxel_map"] + if voxel_map.size: + rr.log( + "world/voxel_map", + rr.Points3D(voxel_map, colors=[[180, 125, 125]], radii=render_voxel / 2), + ) + + surface = obs.tags["surface_clearance"] + if surface.size: + rr.log( + "world/surface_map", + rr.Points3D( + surface[:, :3], + colors=_clearance_colors(surface[:, 3], clearance_clamp), + radii=render_voxel / 2, + ), + ) + + nodes = obs.tags["nodes"] + if nodes.size: + rr.log("world/nodes", rr.Points3D(nodes, colors=[[255, 200, 0]], radii=0.05)) + + edges = obs.tags["node_edges"] + _log_edges(edges, "world/node_edges") + _log_path(obs.data, "world/path") + + timings = obs.tags["timings"] + sizes = { + "voxels": obs.tags["voxels"], + "surface_cells": len(surface), + "nodes": len(nodes), + "edges": len(edges), + } + for key, value in timings.items(): + timing_streams[key].append(float(value), ts=obs.ts) + rr.log(f"metrics/timing/{key}", rr.Scalars(value)) + for key, value in sizes.items(): + size_streams[key].append(float(value), ts=obs.ts) + rr.log(f"metrics/size/{key}", rr.Scalars(value)) + + count = obs.tags.get("frame_count", "?") + planned = obs.tags.get("planned", False) + print( + f"frame_count={count} planned={planned} " + f"waypoints={len(obs.data.poses)} " + f"rebuild={timings['total_ms'] - timings['plan_ms']:.1f}ms " + f"plan={timings['plan_ms']:.1f}ms", + end="\r", + flush=True, + ) + + def main( dataset: str = typer.Argument(..., help="Dataset .db: bare name (cwd or data/) or path"), out: FsPath | None = typer.Option( @@ -187,9 +197,7 @@ def main( 4.0, "--wall-penalty-weight", help="Soft wall-penalty strength at the robot radius" ), goal: tuple[float, float, float] = typer.Option( - (0.0, 0.0, 0.0), - "--goal", - help="Planner goal xyz. Default is dataset-specific; override per recording.", + (0.0, 0.0, 0.0), "--goal", help="Planner goal xyz; override per recording" ), live: bool = typer.Option( False, "--live", help="Also spawn the rerun viewer when --out is set" @@ -198,9 +206,6 @@ def main( clearance_clamp: float = typer.Option( 1.0, "--clearance-clamp", help="Max clearance (m) for the surface color scale" ), - plot_out: FsPath | None = typer.Option( - None, "--plot-out", help="Write an SVG timing/size plot here when the run ends" - ), from_time: float | None = typer.Option( None, "--from-time", help="Start timestamp (s); default is the stream start" ), @@ -261,67 +266,11 @@ def main( try: for obs in pipeline: - rr.set_time(TIMELINE, timestamp=obs.ts) - - start = obs.tags["start"] - rr.log("world/start", rr.Points3D([start], colors=[[0, 255, 0]], radii=0.1)) - - voxel_map = obs.tags["voxel_map"] - if voxel_map.size: - rr.log( - "world/voxel_map", - rr.Points3D(voxel_map, colors=[[180, 125, 125]], radii=render_voxel / 2), - ) - - surface = obs.tags["surface_clearance"] - if surface.size: - rr.log( - "world/surface_map", - rr.Points3D( - surface[:, :3], - colors=_clearance_colors(surface[:, 3], clearance_clamp), - radii=render_voxel / 2, - ), - ) - - nodes = obs.tags["nodes"] - if nodes.size: - rr.log("world/nodes", rr.Points3D(nodes, colors=[[255, 200, 0]], radii=0.05)) - - edges = obs.tags["node_edges"] - _log_edges(edges, "world/node_edges") - _log_path(obs.data, "world/path") - - timings = obs.tags["timings"] - sizes = { - "voxels": obs.tags["voxels"], - "surface_cells": len(surface), - "nodes": len(nodes), - "edges": len(edges), - } - for key, value in timings.items(): - timing_streams[key].append(float(value), ts=obs.ts) - rr.log(f"metrics/timing/{key}", rr.Scalars(value)) - for key, value in sizes.items(): - size_streams[key].append(float(value), ts=obs.ts) - rr.log(f"metrics/size/{key}", rr.Scalars(value)) - - count = obs.tags.get("frame_count", "?") - planned = obs.tags.get("planned", False) - print( - f"frame_count={count} planned={planned} " - f"waypoints={len(obs.data.poses)} " - f"rebuild={timings['total_ms'] - timings['plan_ms']:.1f}ms " - f"plan={timings['plan_ms']:.1f}ms", - end="\r", - flush=True, - ) + _log_frame(obs, render_voxel, clearance_clamp, timing_streams, size_streams) except KeyboardInterrupt: print("\ninterrupted; reporting metrics for completed frames") finally: _print_summary({"timing": timing_streams, "size": size_streams}) - if plot_out is not None: - _save_plot(timing_streams, size_streams["voxels"], str(plot_out)) if out is not None: print(f"wrote {out}") diff --git a/pyproject.toml b/pyproject.toml index 6806733e06..e7f6fa8864 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -523,6 +523,8 @@ module = [ "cyclonedds", "cyclonedds.*", "dimos_lcm.*", + "dimos_mls_planner", + "dimos_voxel_ray_tracing", "etils", "faster_whisper", "geometry_msgs.*", From aa16c5d90a3c36c9c83bfa2c297dafa5cc5ca60d Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Fri, 12 Jun 2026 11:18:14 -0700 Subject: [PATCH 12/56] Parallel ray trace --- dimos/mapping/ray_tracing/rust/Cargo.lock | 59 ++++++++ dimos/mapping/ray_tracing/rust/Cargo.toml | 2 + dimos/mapping/ray_tracing/rust/flake.nix | 2 +- .../ray_tracing/rust/src/voxel_ray_tracer.rs | 135 ++++++++---------- 4 files changed, 123 insertions(+), 75 deletions(-) diff --git a/dimos/mapping/ray_tracing/rust/Cargo.lock b/dimos/mapping/ray_tracing/rust/Cargo.lock index 293fccf43f..122775cdb5 100644 --- a/dimos/mapping/ray_tracing/rust/Cargo.lock +++ b/dimos/mapping/ray_tracing/rust/Cargo.lock @@ -33,6 +33,12 @@ dependencies = [ "num-traits", ] +[[package]] +name = "arrayvec" +version = "0.7.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7c02d123df017efcdfbd739ef81735b36c5ba83ec3c59c80a9d7ecc718f92e50" + [[package]] name = "autocfg" version = "1.5.1" @@ -63,6 +69,31 @@ version = "1.0.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "9330f8b2ff13f34540b44e946ef35111825727b38d33286ef986142615121801" +[[package]] +name = "crossbeam-deque" +version = "0.8.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9dd111b7b7f7d55b72c0a6ae361660ee5853c9af73f70c3c2ef6858b950e2e51" +dependencies = [ + "crossbeam-epoch", + "crossbeam-utils", +] + +[[package]] +name = "crossbeam-epoch" +version = "0.9.18" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5b82ac4a3c2ca9c3460964f020e1402edd5753411d7737aa39c3714ad1b5420e" +dependencies = [ + "crossbeam-utils", +] + +[[package]] +name = "crossbeam-utils" +version = "0.8.21" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d0a5c400df2834b80a4c3327b3aad3a4c4cd4de0629063962b03235697506a28" + [[package]] name = "darling" version = "0.20.11" @@ -136,11 +167,13 @@ name = "dimos-voxel-ray-tracing" version = "0.1.0" dependencies = [ "ahash", + "arrayvec", "dimos-module", "lcm-msgs", "nalgebra", "numpy", "pyo3", + "rayon", "serde", "tokio", "tracing", @@ -158,6 +191,12 @@ dependencies = [ "syn", ] +[[package]] +name = "either" +version = "1.16.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "91622ff5e7162018101f2fea40d6ebf4a78bbe5a49736a2020649edf9693679e" + [[package]] name = "errno" version = "0.3.14" @@ -701,6 +740,26 @@ version = "0.2.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "60a357793950651c4ed0f3f52338f53b2f809f32d83a07f72909fa13e4c6c1e3" +[[package]] +name = "rayon" +version = "1.12.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fb39b166781f92d482534ef4b4b1b2568f42613b53e5b6c160e24cfbfa30926d" +dependencies = [ + "either", + "rayon-core", +] + +[[package]] +name = "rayon-core" +version = "1.13.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "22e18b0f0062d30d4230b2e85ff77fdfe4326feb054b9783a3460d8435c8ab91" +dependencies = [ + "crossbeam-deque", + "crossbeam-utils", +] + [[package]] name = "regex" version = "1.12.3" diff --git a/dimos/mapping/ray_tracing/rust/Cargo.toml b/dimos/mapping/ray_tracing/rust/Cargo.toml index f1a63d34d6..42c60f96bd 100644 --- a/dimos/mapping/ray_tracing/rust/Cargo.toml +++ b/dimos/mapping/ray_tracing/rust/Cargo.toml @@ -21,6 +21,8 @@ lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "r tokio = { version = "1", features = ["rt-multi-thread", "macros", "signal"] } serde = { version = "1", features = ["derive"] } ahash = "0.8" +arrayvec = "0.7" +rayon = "1" tracing = "0.1" pyo3 = { version = "0.25", features = ["extension-module", "abi3-py310"] } numpy = "0.25" diff --git a/dimos/mapping/ray_tracing/rust/flake.nix b/dimos/mapping/ray_tracing/rust/flake.nix index bb2fbfd52d..54721c779d 100644 --- a/dimos/mapping/ray_tracing/rust/flake.nix +++ b/dimos/mapping/ray_tracing/rust/flake.nix @@ -34,7 +34,7 @@ cargoRoot = "dimos/mapping/ray_tracing/rust"; buildAndTestSubdir = "dimos/mapping/ray_tracing/rust"; - cargoHash = "sha256-g30NaoLdtWT5YBsEnE4Xv+EMnI5HHFtZAUtdEL/VbKQ="; + cargoHash = "sha256-0d0dlNDvDplA7oWTyUWOCOlS74Zie8uMQ+ps6lXntOI="; meta.mainProgram = "voxel_ray_tracing"; }; diff --git a/dimos/mapping/ray_tracing/rust/src/voxel_ray_tracer.rs b/dimos/mapping/ray_tracing/rust/src/voxel_ray_tracer.rs index 7eacaba2f4..96aa4e70b1 100644 --- a/dimos/mapping/ray_tracing/rust/src/voxel_ray_tracer.rs +++ b/dimos/mapping/ray_tracing/rust/src/voxel_ray_tracer.rs @@ -2,7 +2,9 @@ // SPDX-License-Identifier: Apache-2.0 use ahash::{AHashMap, AHashSet}; +use arrayvec::ArrayVec; use nalgebra::{Matrix3, Vector3}; +use rayon::prelude::*; use serde::Deserialize; use validator::{Validate, ValidationError}; @@ -87,7 +89,7 @@ impl VoxelMap { .voxels .keys() .copied() - .map(|k| (k, pooled_normal(&self.voxels, k, voxel_size))) + .map(|k| (k, pooled_normal_and_recency(&self.voxels, k, voxel_size).0)) .collect(); for (k, n) in updates { self.voxels.get_mut(&k).unwrap().normal = n; @@ -97,6 +99,7 @@ impl VoxelMap { const NORMAL_MIN_POINTS: u32 = 3; const NORMAL_NEIGHBOR_RADIUS: i32 = 1; +const NEIGHBORHOOD_CAP: usize = (2 * NORMAL_NEIGHBOR_RADIUS as usize + 1).pow(3); const NORMAL_REWEIGHT_ITERS: u32 = 3; /// Neighbor weight falloff with plane distance, as a fraction of voxel size. const NORMAL_PLANE_SIGMA_FRAC: f32 = 0.5; @@ -192,15 +195,17 @@ struct Neighbor { centroid: Vector3, } -/// Find voxel's normal from its neighborhood. -fn pooled_normal( +/// Find voxel's normal and the most recent frame any voxel in its +/// neighborhood was hit, from one scan of the neighborhood. +fn pooled_normal_and_recency( voxels: &AHashMap, key: VoxelKey, voxel_size: f32, -) -> Option> { +) -> (Option>, u32) { let r = NORMAL_NEIGHBOR_RADIUS; - let mut nbs: Vec = Vec::new(); + let mut nbs: ArrayVec = ArrayVec::new(); let mut n_raw: u32 = 0; + let mut recency = 0; for dx in -r..=r { for dy in -r..=r { for dz in -r..=r { @@ -208,6 +213,7 @@ fn pooled_normal( let Some(v) = voxels.get(&nk) else { continue; }; + recency = recency.max(v.last_hit); if v.num_pts == 0 { continue; } @@ -228,12 +234,12 @@ fn pooled_normal( } } if n_raw < NORMAL_MIN_POINTS { - return None; + return (None, recency); } let sigma = NORMAL_PLANE_SIGMA_FRAC * voxel_size; let two_sig2 = 2.0 * sigma * sigma; - let mut weights = vec![1.0_f32; nbs.len()]; + let mut weights = [1.0_f32; NEIGHBORHOOD_CAP]; let mut cov = Matrix3::zeros(); for _ in 0..NORMAL_REWEIGHT_ITERS { let (mut wn, mut s, mut t) = (0.0_f32, Vector3::zeros(), Matrix3::zeros()); @@ -264,25 +270,9 @@ fn pooled_normal( // get rid of planes if we had to discard too many points to get a plane let kept: f32 = nbs.iter().zip(&weights).map(|(nb, &w)| w * nb.n).sum(); if kept < NORMAL_MIN_SUPPORT * n_raw as f32 { - return None; - } - fit_normal(cov) -} - -/// Most recent frame any voxel in the neighborhood was hit. -fn neighborhood_recency(voxels: &AHashMap, key: VoxelKey) -> u32 { - let r = NORMAL_NEIGHBOR_RADIUS; - let mut best = 0; - for dx in -r..=r { - for dy in -r..=r { - for dz in -r..=r { - if let Some(v) = voxels.get(&(key.0 + dx, key.1 + dy, key.2 + dz)) { - best = best.max(v.last_hit); - } - } - } + return (None, recency); } - best + (fit_normal(cov), recency) } /// Refit the cached normal and neighborhood recency of every voxel whose @@ -305,14 +295,11 @@ fn refresh_voxels( } } let updates: Vec<(VoxelKey, Option>, u32)> = dirty - .iter() + .par_iter() .filter(|k| map.voxels.contains_key(k)) .map(|&k| { - ( - k, - pooled_normal(&map.voxels, k, voxel_size), - neighborhood_recency(&map.voxels, k), - ) + let (normal, recency) = pooled_normal_and_recency(&map.voxels, k, voxel_size); + (k, normal, recency) }) .collect(); for (k, n, rec) in updates { @@ -326,16 +313,12 @@ fn refresh_voxels( /// Spare a clearing miss only when a grazing ray skims a recently hit planar /// surface. Stale or voxels with no normal are left to the health checks. fn should_spare( - voxels: &AHashMap, - key: VoxelKey, + c: &Voxel, ray_unit: Vector3, graze_cos: f32, frame: u32, recency_window: u32, ) -> bool { - let Some(c) = voxels.get(&key) else { - return false; - }; match c.normal { Some(n) => { frame.saturating_sub(c.recency) <= recency_window && ray_unit.dot(&n).abs() < graze_cos @@ -427,35 +410,46 @@ pub fn update_map( let frame = map.frame; let hits = live_voxels(points, cfg.voxel_size); - let mut misses: AHashSet = AHashSet::new(); let origin_voxel = world_to_voxel(origin.0, origin.1, origin.2, inv); let step = cfg.ray_subsample as usize; - for (i, &p) in points.iter().enumerate() { - if i % step != 0 { - continue; - } - let dx = p.0 - origin.0; - let dy = p.1 - origin.1; - let dz = p.2 - origin.2; - if dx * dx + dy * dy + dz * dz > max_range_sq { - continue; - } - let endpoint = world_to_voxel(p.0, p.1, p.2, inv); - find_misses_along_ray( - &mut misses, - &map.voxels, - origin, - p, - cfg.voxel_size, - cfg.shadow_depth, - cfg.grace_depth, - cfg.graze_cos, - frame, - cfg.recency_window, - origin_voxel, - endpoint, - ); - } + let voxels = &map.voxels; + let misses: AHashSet = points + .par_iter() + .enumerate() + .fold(AHashSet::new, |mut misses, (i, &p)| { + if i % step != 0 { + return misses; + } + let dx = p.0 - origin.0; + let dy = p.1 - origin.1; + let dz = p.2 - origin.2; + if dx * dx + dy * dy + dz * dz > max_range_sq { + return misses; + } + let endpoint = world_to_voxel(p.0, p.1, p.2, inv); + find_misses_along_ray( + &mut misses, + voxels, + origin, + p, + cfg.voxel_size, + cfg.shadow_depth, + cfg.grace_depth, + cfg.graze_cos, + frame, + cfg.recency_window, + origin_voxel, + endpoint, + ); + misses + }) + .reduce(AHashSet::new, |mut a, mut b| { + if a.len() < b.len() { + std::mem::swap(&mut a, &mut b); + } + a.extend(b); + a + }); // add new hits for v in &hits { @@ -623,17 +617,10 @@ fn find_misses_along_ray( continue; } - if map_voxels.contains_key(&(x, y, z)) - && !should_spare( - map_voxels, - (x, y, z), - ray_unit, - graze_cos, - frame, - recency_window, - ) - { - misses.insert((x, y, z)); + if let Some(c) = map_voxels.get(&(x, y, z)) { + if !should_spare(c, ray_unit, graze_cos, frame, recency_window) { + misses.insert((x, y, z)); + } } } } From ab89ea38cb183bb11c8514d9d507e6563022090e Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Fri, 12 Jun 2026 11:19:03 -0700 Subject: [PATCH 13/56] More generous starts --- .../nav_3d/mls_planner/rust/src/planner.rs | 136 ++++++++++++------ 1 file changed, 96 insertions(+), 40 deletions(-) diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs index ed00525e9d..aae5abec98 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs @@ -15,41 +15,53 @@ use crate::voxel::{surface_point_xyz, VoxelKey}; /// Robot-rooted candidate search radius, in multiples of node spacing. const CANDIDATE_RADIUS_FACTOR: f32 = 3.0; -/// Snap a pose to the best surface cell. -pub fn snap_pose_to_cell( +/// How far to search horizontally when snapping a pose to the surface. +/// A downward-pitched lidar cannot see the floor under the robot, so the +/// nearest mapped surface can start well outside the robot footprint. +const SNAP_SEARCH_RADIUS_M: f32 = 1.5; + +/// How many nearby snap candidates to try when connecting the start. +const MAX_SNAP_ATTEMPTS: usize = 64; + +/// Surface cells near the pose, nearest first in xy. +/// The nearest cell can sit on a small fragment disconnected from the main +/// surface, so callers that need connectivity should try candidates in order. +pub fn snap_candidates( surface_lookup: &SurfaceLookup, pose: (f32, f32, f32), voxel_size: f32, tolerance_m: f32, -) -> Option { +) -> Vec { let ix = (pose.0 / voxel_size).floor() as i32; let iy = (pose.1 / voxel_size).floor() as i32; let target_iz = (pose.2 / voxel_size).floor() as i32 - 1; let tol_cells = (tolerance_m / voxel_size).ceil() as i32; + let search_radius = (SNAP_SEARCH_RADIUS_M / voxel_size).ceil() as i32; - if let Some(cell) = best_iz_in_column(surface_lookup, ix, iy, target_iz, tol_cells) { - return Some(cell); - } - - const SEARCH_RADIUS: i32 = 5; - let mut best: Option<(i32, VoxelKey)> = None; - for dix in -SEARCH_RADIUS..=SEARCH_RADIUS { - for diy in -SEARCH_RADIUS..=SEARCH_RADIUS { - if dix == 0 && diy == 0 { - continue; - } - let Some(cell) = + let mut found: Vec<(i32, VoxelKey)> = Vec::new(); + for dix in -search_radius..=search_radius { + for diy in -search_radius..=search_radius { + if let Some(cell) = best_iz_in_column(surface_lookup, ix + dix, iy + diy, target_iz, tol_cells) - else { - continue; - }; - let d2 = dix * dix + diy * diy; - if best.is_none_or(|(bd, _)| d2 < bd) { - best = Some((d2, cell)); + { + found.push((dix * dix + diy * diy, cell)); } } } - best.map(|(_, c)| c) + found.sort_by_key(|&(d2, _)| d2); + found.into_iter().map(|(_, c)| c).collect() +} + +/// Snap a pose to the nearest surface cell. +pub fn snap_pose_to_cell( + surface_lookup: &SurfaceLookup, + pose: (f32, f32, f32), + voxel_size: f32, + tolerance_m: f32, +) -> Option { + snap_candidates(surface_lookup, pose, voxel_size, tolerance_m) + .into_iter() + .next() } fn best_iz_in_column( @@ -85,17 +97,41 @@ pub fn plan( ) -> Option> { let voxel_size = config.voxel_size; let z_tolerance_m = config.robot_height; - let start_coord = - snap_pose_to_cell(&plg.surface_lookup, start_pose, voxel_size, z_tolerance_m)?; - let goal_coord = snap_pose_to_cell(&plg.surface_lookup, goal_pose, voxel_size, z_tolerance_m)?; - let start_cell = plg.cells.id(start_coord)?; - let goal_cell = plg.cells.id(goal_coord)?; + let start_candidates = + snap_candidates(&plg.surface_lookup, start_pose, voxel_size, z_tolerance_m); + if start_candidates.is_empty() { + tracing::warn!( + ?start_pose, + "plan failed: start does not snap to any surface cell" + ); + return None; + } + let Some(goal_coord) = + snap_pose_to_cell(&plg.surface_lookup, goal_pose, voxel_size, z_tolerance_m) + else { + tracing::warn!( + ?goal_pose, + "plan failed: goal does not snap to any surface cell" + ); + return None; + }; + let Some(goal_cell) = plg.cells.id(goal_coord) else { + tracing::warn!(?goal_coord, "plan failed: goal cell is not in the graph"); + return None; + }; let node_cells: AHashSet = plg.nodes.iter().map(|n| n.cell_id).collect(); let goal_segment = walk_preds(&plg.cell_state, goal_cell); - let goal_node = *goal_segment.last()?; + let Some(&goal_node) = goal_segment.last() else { + tracing::warn!(?goal_coord, "plan failed: goal has no predecessor chain"); + return None; + }; if !node_cells.contains(&goal_node) { + tracing::warn!( + ?goal_coord, + "plan failed: goal region does not reach a graph node" + ); return None; } @@ -103,15 +139,33 @@ pub fn plan( let (cost_to_go, pred_to_goal) = node_dijkstra(plg, goal_node); let radius = (config.node_spacing_m * CANDIDATE_RADIUS_FACTOR).max(voxel_size); - let (lead_in, node_seq) = select_entry( - plg, - start_cell, - goal_node, - &cost_to_go, - &pred_to_goal, - &node_cells, - radius, - )?; + let mut entry: Option<(Vec, Vec)> = None; + for &candidate in start_candidates.iter().take(MAX_SNAP_ATTEMPTS) { + let Some(start_cell) = plg.cells.id(candidate) else { + continue; + }; + entry = select_entry( + plg, + start_cell, + goal_node, + &cost_to_go, + &pred_to_goal, + &node_cells, + radius, + ); + if entry.is_some() { + break; + } + } + let Some((lead_in, node_seq)) = entry else { + tracing::warn!( + candidates = start_candidates.len().min(MAX_SNAP_ATTEMPTS), + reachable_nodes = cost_to_go.len(), + total_nodes = plg.nodes.len(), + "plan failed: no start candidate connects to the goal component", + ); + return None; + }; // Shortcut height tolerance in cells, tied to the traversable step. let smooth_tol_cells = ((config.node_step_threshold_m / voxel_size).round() as i32).max(1); @@ -541,10 +595,12 @@ mod tests { #[test] fn plan_returns_none_if_disconnected() { + // The gap must exceed SNAP_SEARCH_RADIUS_M so no start candidate + // can relocate onto the goal island. let mut cells: Vec = (0..5).map(|x| (x, 0, 0)).collect(); - cells.extend((10..15).map(|x| (x, 0, 0))); - let plg = graph_with_nodes(&cells, &[(2, 0, 0), (12, 0, 0)]); - let result = plan_simple(&plg, (0.25, 0.0, 0.1), (1.25, 0.0, 0.1)); + cells.extend((30..35).map(|x| (x, 0, 0))); + let plg = graph_with_nodes(&cells, &[(2, 0, 0), (32, 0, 0)]); + let result = plan_simple(&plg, (0.25, 0.0, 0.1), (3.25, 0.0, 0.1)); assert!(result.is_none()); } From 469bf59cb88afd8329589f5517ed67483834cb5b Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Fri, 12 Jun 2026 12:17:23 -0700 Subject: [PATCH 14/56] Consolidate logic --- dimos/mapping/ray_tracing/module.py | 3 + dimos/mapping/ray_tracing/rust/src/main.rs | 50 ++++-------- dimos/mapping/ray_tracing/rust/src/python.rs | 62 ++++++++++----- .../ray_tracing/rust/src/voxel_ray_tracer.rs | 79 +++++++++++++++++++ dimos/mapping/ray_tracing/transformer.py | 26 ++---- dimos/mapping/ray_tracing/voxel_map.py | 4 +- dimos/mapping/ray_tracing/voxel_map.pyi | 14 +++- 7 files changed, 159 insertions(+), 79 deletions(-) diff --git a/dimos/mapping/ray_tracing/module.py b/dimos/mapping/ray_tracing/module.py index 8224d25ea2..e1788f4db8 100644 --- a/dimos/mapping/ray_tracing/module.py +++ b/dimos/mapping/ray_tracing/module.py @@ -50,6 +50,9 @@ class RayTracingVoxelMapConfig(NativeModuleConfig): recency_window: int = 15 # Integrate every frame, publish maps every Nth frame. emit_every: int = 1 + # Size the local region to this percentile of batch point distances, + # so a stray far hit cannot inflate the region the planner recomputes. + region_percentile: float = 95.0 class RayTracingVoxelMap(NativeModule, mapping.GlobalPointcloud): diff --git a/dimos/mapping/ray_tracing/rust/src/main.rs b/dimos/mapping/ray_tracing/rust/src/main.rs index e9d3914098..d3d000d644 100644 --- a/dimos/mapping/ray_tracing/rust/src/main.rs +++ b/dimos/mapping/ray_tracing/rust/src/main.rs @@ -6,7 +6,7 @@ use std::time::Duration; use ahash::AHashSet; use dimos_module::{error_throttled, run, warn_throttled, Input, LcmTransport, Module, Output}; use dimos_voxel_ray_tracing::voxel_ray_tracer::{ - iter_global_points, update_map, Config, LocalBounds, VoxelKey, VoxelMap, + batch_local_bounds, iter_global_points, update_map, Config, LocalBounds, VoxelKey, VoxelMap, }; use lcm_msgs::geometry_msgs::{Point, Pose, PoseStamped, Quaternion}; use lcm_msgs::nav_msgs::Odometry; @@ -39,7 +39,8 @@ struct RayTracingVoxelMap { map: VoxelMap, last_origin: Option<(f32, f32, f32)>, frame_count: u32, - batch_bbox: Option<([f32; 3], [f32; 3])>, + batch_points: Vec<(f32, f32, f32)>, + batch_origins: Vec<(f32, f32, f32)>, } impl RayTracingVoxelMap { @@ -76,47 +77,24 @@ impl RayTracingVoxelMap { let live = update_map(&mut self.map, origin, &points, &self.config); - // Grow the batch bbox over this frame's live voxels and the origin, - // so the emitted region covers everything touched since the last emit. - let half = voxel_size * 0.5; - let (mut lo, mut hi) = self - .batch_bbox - .unwrap_or(([f32::INFINITY; 3], [f32::NEG_INFINITY; 3])); - let mut grow = |x: f32, y: f32, z: f32| { - lo[0] = lo[0].min(x); - lo[1] = lo[1].min(y); - lo[2] = lo[2].min(z); - hi[0] = hi[0].max(x); - hi[1] = hi[1].max(y); - hi[2] = hi[2].max(z); - }; - grow(origin.0, origin.1, origin.2); - for &(kx, ky, kz) in &live { - grow( - kx as f32 * voxel_size + half, - ky as f32 * voxel_size + half, - kz as f32 * voxel_size + half, - ); - } - self.batch_bbox = Some((lo, hi)); + self.batch_points.extend_from_slice(&points); + self.batch_origins.push(origin); self.frame_count += 1; if !self.frame_count.is_multiple_of(self.config.emit_every) { return; } - let Some((lo, hi)) = self.batch_bbox.take() else { - return; - }; - // Cylinder enclosing the batch bbox plus the clearing margin. + // Percentile-bounded cylinder over the batch plus the clearing margin. let margin = self.config.shadow_depth + voxel_size; - let cx = (lo[0] + hi[0]) * 0.5; - let cy = (lo[1] + hi[1]) * 0.5; - let rx = (hi[0] - lo[0]) * 0.5 + margin; - let ry = (hi[1] - lo[1]) * 0.5 + margin; - let radius = (rx * rx + ry * ry).sqrt(); - let z_min = lo[2] - margin; - let z_max = hi[2] + margin; + let (cx, cy, radius, z_min, z_max) = batch_local_bounds( + &self.batch_points, + &self.batch_origins, + self.config.region_percentile, + margin, + ); + self.batch_points.clear(); + self.batch_origins.clear(); let cylinder = LocalBounds { origin_x: cx, origin_y: cy, diff --git a/dimos/mapping/ray_tracing/rust/src/python.rs b/dimos/mapping/ray_tracing/rust/src/python.rs index 8ac13b3b16..852516b9f7 100644 --- a/dimos/mapping/ray_tracing/rust/src/python.rs +++ b/dimos/mapping/ray_tracing/rust/src/python.rs @@ -8,9 +8,46 @@ use pyo3::prelude::*; use validator::Validate; use crate::voxel_ray_tracer::{ - iter_global_normals, iter_global_points, update_map, Config, LocalBounds, VoxelMap, + batch_local_bounds, iter_global_normals, iter_global_points, update_map, Config, LocalBounds, + VoxelMap, }; +fn extract_tuples(arr: &Bound<'_, PyAny>, name: &str) -> PyResult> { + let arr: PyReadonlyArray2<'_, f32> = arr.extract().map_err(|_| { + PyValueError::new_err(format!("{name} must be a (N, 3) float32 numpy array")) + })?; + let shape = arr.shape(); + if shape[1] != 3 { + return Err(PyValueError::new_err(format!( + "{name} must be (N, 3) float32, got shape {:?}", + shape + ))); + } + let view = arr.as_array(); + Ok((0..shape[0]) + .filter_map(|i| { + let x = view[[i, 0]]; + let y = view[[i, 1]]; + let z = view[[i, 2]]; + (x.is_finite() && y.is_finite() && z.is_finite()).then_some((x, y, z)) + }) + .collect()) +} + +/// Local region a batch of frames observed, as (cx, cy, radius, z_min, z_max). +/// Non-finite points are ignored. +#[pyfunction] +fn local_bounds( + points: &Bound<'_, PyAny>, + origins: &Bound<'_, PyAny>, + percentile: f32, + margin: f32, +) -> PyResult<(f32, f32, f32, f32, f32)> { + let pts = extract_tuples(points, "points")?; + let origs = extract_tuples(origins, "origins")?; + Ok(batch_local_bounds(&pts, &origs, percentile, margin)) +} + #[pyclass] pub struct VoxelRayMapper { config: Config, @@ -55,6 +92,7 @@ impl VoxelRayMapper { graze_cos, recency_window, emit_every: 1, + region_percentile: 95.0, }; config .validate() @@ -71,26 +109,7 @@ impl VoxelRayMapper { points: &Bound<'_, PyAny>, origin: (f32, f32, f32), ) -> PyResult<()> { - let points: PyReadonlyArray2<'_, f32> = points - .extract() - .map_err(|_| PyValueError::new_err("points must be a (N, 3) float32 numpy array"))?; - let shape = points.shape(); - if shape[1] != 3 { - return Err(PyValueError::new_err(format!( - "points must be (N, 3) float32, got shape {:?}", - shape - ))); - } - let arr = points.as_array(); - let n = shape[0]; - let pts: Vec<(f32, f32, f32)> = (0..n) - .filter_map(|i| { - let x = arr[[i, 0]]; - let y = arr[[i, 1]]; - let z = arr[[i, 2]]; - (x.is_finite() && y.is_finite() && z.is_finite()).then_some((x, y, z)) - }) - .collect(); + let pts = extract_tuples(points, "points")?; let cfg = &self.config; let map = &mut self.map; @@ -206,5 +225,6 @@ impl VoxelRayMapper { #[pymodule] fn dimos_voxel_ray_tracing(m: &Bound<'_, PyModule>) -> PyResult<()> { m.add_class::()?; + m.add_function(wrap_pyfunction!(local_bounds, m)?)?; Ok(()) } diff --git a/dimos/mapping/ray_tracing/rust/src/voxel_ray_tracer.rs b/dimos/mapping/ray_tracing/rust/src/voxel_ray_tracer.rs index 96aa4e70b1..ff995155f2 100644 --- a/dimos/mapping/ray_tracing/rust/src/voxel_ray_tracer.rs +++ b/dimos/mapping/ray_tracing/rust/src/voxel_ray_tracer.rs @@ -38,6 +38,10 @@ pub struct Config { /// Integrate every frame, publish maps every Nth frame. #[validate(range(min = 1))] pub emit_every: u32, + /// Size the local region to this percentile of batch point distances, + /// so a stray far hit cannot inflate the region the planner recomputes. + #[validate(range(min = 0.0, max = 100.0))] + pub region_percentile: f32, } fn validate_health_range(cfg: &Config) -> Result<(), ValidationError> { @@ -346,6 +350,48 @@ impl LocalBounds { } } +/// The local region a batch of frames observed, as (cx, cy, radius, z_min, +/// z_max). A cylinder centered on the mean origin, sized to a percentile of +/// the point distances so a stray far hit cannot inflate it. Points must be +/// finite. An empty batch yields a zero-radius region at the mean origin. +pub fn batch_local_bounds( + points: &[(f32, f32, f32)], + origins: &[(f32, f32, f32)], + percentile_pct: f32, + margin: f32, +) -> (f32, f32, f32, f32, f32) { + let n = origins.len().max(1) as f64; + let cx = (origins.iter().map(|o| o.0 as f64).sum::() / n) as f32; + let cy = (origins.iter().map(|o| o.1 as f64).sum::() / n) as f32; + if points.is_empty() { + let cz = (origins.iter().map(|o| o.2 as f64).sum::() / n) as f32; + return (cx, cy, 0.0, cz, cz); + } + + let mut dist: Vec = points.iter().map(|p| (p.0 - cx).hypot(p.1 - cy)).collect(); + let mut zs: Vec = points.iter().map(|p| p.2).collect(); + let radius = percentile(&mut dist, percentile_pct) + margin; + let z_min = percentile(&mut zs, 100.0 - percentile_pct) - margin; + let z_max = percentile(&mut zs, percentile_pct) + margin; + (cx, cy, radius, z_min, z_max) +} + +fn percentile(values: &mut [f32], p: f32) -> f32 { + let n = values.len(); + if n == 1 { + return values[0]; + } + let rank = (p as f64 / 100.0).clamp(0.0, 1.0) * (n - 1) as f64; + let lo = rank.floor() as usize; + let frac = (rank - lo as f64) as f32; + let (_, &mut v_lo, rest) = values.select_nth_unstable_by(lo, |a, b| a.total_cmp(b)); + if frac == 0.0 || rest.is_empty() { + return v_lo; + } + let v_hi = rest.iter().copied().fold(f32::INFINITY, f32::min); + v_lo + frac * (v_hi - v_lo) +} + pub fn iter_global_points( map: &VoxelMap, voxel_size: f32, @@ -641,6 +687,7 @@ mod tests { graze_cos: 0.5, recency_window: 60, emit_every: 1, + region_percentile: 95.0, } } @@ -688,6 +735,33 @@ mod tests { assert_eq!(misses, expected); } + #[test] + fn batch_bounds_ignore_far_outlier() { + let origins = [(1.0, 1.0, 0.5), (3.0, 1.0, 0.5)]; + let mut points: Vec<(f32, f32, f32)> = (0..99) + .map(|i| { + let a = i as f32 / 99.0 * std::f32::consts::TAU; + (2.0 + a.cos(), 1.0 + a.sin(), (i % 10) as f32 * 0.1) + }) + .collect(); + points.push((60.0, 1.0, 30.0)); + let (cx, cy, radius, z_min, z_max) = batch_local_bounds(&points, &origins, 95.0, 0.3); + assert_eq!(cx, 2.0); + assert_eq!(cy, 1.0); + assert!(radius < 2.0, "outlier inflated radius to {radius}"); + assert!(z_max < 2.0, "outlier inflated z_max to {z_max}"); + assert!((-0.5..=0.0).contains(&z_min), "z_min out of range: {z_min}"); + } + + #[test] + fn batch_bounds_empty_points_zero_radius() { + let origins = [(1.0, 2.0, 3.0)]; + let (cx, cy, radius, z_min, z_max) = batch_local_bounds(&[], &origins, 95.0, 0.3); + assert_eq!((cx, cy, radius), (1.0, 2.0, 0.0)); + assert_eq!(z_min, 3.0); + assert_eq!(z_max, 3.0); + } + #[test] fn hits_insert_voxels() { let cfg = basic_config(); @@ -801,6 +875,7 @@ mod tests { graze_cos: 0.5, recency_window: 60, emit_every: 1, + region_percentile: 95.0, }; // Build the floor over a y band so it is a 2d plane, not a wire. let max_x = 25.0_f32; @@ -954,6 +1029,7 @@ mod tests { graze_cos: 0.5, recency_window: 60, emit_every: 1, + region_percentile: 95.0, }; // Staircase @@ -1026,6 +1102,7 @@ mod tests { graze_cos: 0.5, recency_window: 60, emit_every: 1, + region_percentile: 95.0, }; // Flat floor from the sensor out to a vertical wall. @@ -1086,6 +1163,7 @@ mod tests { graze_cos, recency_window: 60, emit_every: 1, + region_percentile: 95.0, }; // Staircase topped by a flat landing and a back wall. @@ -1215,6 +1293,7 @@ mod tests { graze_cos: 0.5, recency_window, emit_every: 1, + region_percentile: 95.0, }; let (mut map, _) = build_surface(&floor, voxel_size, cfg.max_health); let row: Vec = map diff --git a/dimos/mapping/ray_tracing/transformer.py b/dimos/mapping/ray_tracing/transformer.py index 71d2a12051..2936e9234f 100644 --- a/dimos/mapping/ray_tracing/transformer.py +++ b/dimos/mapping/ray_tracing/transformer.py @@ -20,7 +20,7 @@ import open3d as o3d # type: ignore[import-untyped] import open3d.core as o3c # type: ignore[import-untyped] -from dimos.mapping.ray_tracing.voxel_map import VoxelRayMapper +from dimos.mapping.ray_tracing.voxel_map import VoxelRayMapper, local_bounds from dimos.memory2.transform import Transformer from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 @@ -62,30 +62,18 @@ def _local_bounds( ) -> tuple[float, float, float, float, float]: """Robot-centered cylinder sized to a percentile of the observed points. - An empty or all-nonfinite frame yields a zero-radius region at the robot. + An empty batch yields a zero-radius region at the robot. """ - margin = self._mapper_kwargs.get("shadow_depth", 0.2) + self.voxel_size - points = ( - np.concatenate(batch_points, axis=0) - if batch_points - else np.empty((0, 3), dtype=np.float32) - ) - points = points[np.isfinite(points).all(axis=1)] - if points.size == 0: + if not batch_origins: pose = last_obs.pose_tuple assert pose is not None, "poseless obs are skipped upstream" rx, ry, rz = pose[:3] return rx, ry, 0.0, rz, rz - origins = np.asarray(batch_origins, dtype=np.float64) - cx = float(origins[:, 0].mean()) - cy = float(origins[:, 1].mean()) - dist = np.hypot(points[:, 0] - cx, points[:, 1] - cy) - radius = float(np.percentile(dist, self.region_percentile)) + margin - lo_pct = 100.0 - self.region_percentile - z_min = float(np.percentile(points[:, 2], lo_pct)) - margin - z_max = float(np.percentile(points[:, 2], self.region_percentile)) + margin - return cx, cy, radius, z_min, z_max + points = np.concatenate(batch_points, axis=0) + origins = np.asarray(batch_origins, dtype=np.float32) + margin = self._mapper_kwargs.get("shadow_depth", 0.2) + self.voxel_size + return local_bounds(points, origins, self.region_percentile, margin) def _make_obs( self, diff --git a/dimos/mapping/ray_tracing/voxel_map.py b/dimos/mapping/ray_tracing/voxel_map.py index e362c6697c..ea5154f54e 100644 --- a/dimos/mapping/ray_tracing/voxel_map.py +++ b/dimos/mapping/ray_tracing/voxel_map.py @@ -17,11 +17,11 @@ from __future__ import annotations try: - from dimos_voxel_ray_tracing import VoxelRayMapper + from dimos_voxel_ray_tracing import VoxelRayMapper, local_bounds except ImportError as e: raise ImportError( "dimos_voxel_ray_tracing is not built. Run: " "uv run maturin develop --uv -m dimos/mapping/ray_tracing/rust/Cargo.toml" ) from e -__all__ = ["VoxelRayMapper"] +__all__ = ["VoxelRayMapper", "local_bounds"] diff --git a/dimos/mapping/ray_tracing/voxel_map.pyi b/dimos/mapping/ray_tracing/voxel_map.pyi index 32915ad3fa..278bbde231 100644 --- a/dimos/mapping/ray_tracing/voxel_map.pyi +++ b/dimos/mapping/ray_tracing/voxel_map.pyi @@ -71,4 +71,16 @@ class VoxelRayMapper: def __len__(self) -> int: ... def __repr__(self) -> str: ... -__all__ = ["VoxelRayMapper"] +def local_bounds( + points: NDArray[np.float32], + origins: NDArray[np.float32], + percentile: float, + margin: float, +) -> tuple[float, float, float, float, float]: + """Local region a batch of frames observed, as (cx, cy, radius, z_min, z_max). + + Non-finite points are ignored. + """ + ... + +__all__ = ["VoxelRayMapper", "local_bounds"] From 8b36969a1cc309313d2ab8679494ed70b761add6 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Fri, 12 Jun 2026 12:49:39 -0700 Subject: [PATCH 15/56] Separate publish rates for maps and remove timing --- dimos/mapping/ray_tracing/module.py | 9 +- dimos/mapping/ray_tracing/rust/src/main.rs | 196 ++++++++++-------- dimos/mapping/ray_tracing/rust/src/python.rs | 1 + .../ray_tracing/rust/src/voxel_ray_tracer.rs | 14 +- .../nav_3d/mls_planner/mls_planner.pyi | 7 - .../nav_3d/mls_planner/mls_planner_native.py | 4 +- .../nav_3d/mls_planner/rust/src/main.rs | 20 +- .../mls_planner/rust/src/mls_planner.rs | 75 ++----- .../nav_3d/mls_planner/rust/src/nodes.rs | 27 +++ .../nav_3d/mls_planner/rust/src/planner.rs | 11 +- .../nav_3d/mls_planner/rust/src/python.rs | 11 - .../nav_3d/mls_planner/test_transformer.py | 36 +++- .../nav_3d/mls_planner/transformer.py | 10 +- .../nav_3d/mls_planner/utils/plan_rrd.py | 11 +- 14 files changed, 228 insertions(+), 204 deletions(-) diff --git a/dimos/mapping/ray_tracing/module.py b/dimos/mapping/ray_tracing/module.py index e1788f4db8..a7fcab1d2f 100644 --- a/dimos/mapping/ray_tracing/module.py +++ b/dimos/mapping/ray_tracing/module.py @@ -48,8 +48,11 @@ class RayTracingVoxelMapConfig(NativeModuleConfig): graze_cos: float = 0.7 # Only spare a voxel whose neighborhood was hit within this many frames. recency_window: int = 15 - # Integrate every frame, publish maps every Nth frame. + # Integrate every frame, publish the local map and region bounds every + # Nth frame. Zero disables them. emit_every: int = 1 + # Publish the global map every Nth frame. Zero disables it. + global_emit_every: int = 1 # Size the local region to this percentile of batch point distances, # so a stray far hit cannot inflate the region the planner recomputes. region_percentile: float = 95.0 @@ -59,8 +62,8 @@ class RayTracingVoxelMap(NativeModule, mapping.GlobalPointcloud): """Rust voxel-map module with raycast clearing of dynamic objects. region_bounds describes the cylinder local_map covers, packed into a - PoseStamped. Position holds the center, orientation holds - (radius, z_min, z_max, 0). It shares the local_map stamp. + PoseStamped. Position holds the center. Orientation holds radius, z_min, + z_max, and zero. It shares the local_map stamp. """ config: RayTracingVoxelMapConfig diff --git a/dimos/mapping/ray_tracing/rust/src/main.rs b/dimos/mapping/ray_tracing/rust/src/main.rs index d3d000d644..45a9ab51a1 100644 --- a/dimos/mapping/ray_tracing/rust/src/main.rs +++ b/dimos/mapping/ray_tracing/rust/src/main.rs @@ -27,8 +27,8 @@ struct RayTracingVoxelMap { #[output(encode = PointCloud2::encode)] local_map: Output, - // Region the local_map covers, packed into a PoseStamped: position holds - // the cylinder center, orientation holds (radius, z_min, z_max, 0). + // Region the local_map covers, packed into a PoseStamped. Position holds + // the cylinder center and orientation holds radius, z_min, z_max, zero. // Stamped identically to local_map so consumers can pair them. #[output(encode = PoseStamped::encode)] region_bounds: Output, @@ -77,10 +77,33 @@ impl RayTracingVoxelMap { let live = update_map(&mut self.map, origin, &points, &self.config); - self.batch_points.extend_from_slice(&points); - self.batch_origins.push(origin); + // The batch only feeds the local region bounds, so don't let it grow + // when the local map is disabled. + if self.config.emit_every > 0 { + self.batch_points.extend_from_slice(&points); + self.batch_origins.push(origin); + } self.frame_count += 1; + if self + .frame_count + .is_multiple_of(self.config.global_emit_every) + { + let cloud = build_global_cloud( + &self.map, + &live, + voxel_size, + &msg.header.frame_id, + msg.header.stamp.clone(), + ); + if let Err(e) = self.global_map.publish(&cloud).await { + error_throttled!( + Duration::from_secs(1), + error = %e, + "Updated global voxel map failed to publish", + ); + } + } if !self.frame_count.is_multiple_of(self.config.emit_every) { return; } @@ -131,7 +154,7 @@ impl RayTracingVoxelMap { ); } - let (global_cloud, local_cloud) = build_pointclouds( + let local_cloud = build_local_cloud( &self.map, &live, voxel_size, @@ -139,13 +162,6 @@ impl RayTracingVoxelMap { &msg.header.frame_id, msg.header.stamp, ); - if let Err(e) = self.global_map.publish(&global_cloud).await { - error_throttled!( - Duration::from_secs(1), - error = %e, - "Updated global voxel map failed to publish", - ); - } if let Err(e) = self.local_map.publish(&local_cloud).await { error_throttled!( Duration::from_secs(1), @@ -222,93 +238,101 @@ fn read_f32_le(buf: &[u8], off: usize) -> f32 { f32::from_le_bytes(bytes) } -fn build_pointclouds( - map: &VoxelMap, - live: &AHashSet, +fn write_point(data: &mut Vec, n: &mut i32, x: f32, y: f32, z: f32) { + data.extend_from_slice(&x.to_le_bytes()); + data.extend_from_slice(&y.to_le_bytes()); + data.extend_from_slice(&z.to_le_bytes()); + data.extend_from_slice(&0.0_f32.to_le_bytes()); + *n += 1; +} + +/// Centers of live voxels not yet healthy enough to appear in the map. +fn unhealthy_live_centers<'a>( + map: &'a VoxelMap, + live: &'a AHashSet, voxel_size: f32, - cylinder: &LocalBounds, - frame_id: &str, - stamp: Time, -) -> (PointCloud2, PointCloud2) { +) -> impl Iterator + 'a { let half = voxel_size * 0.5; - let mut global_data = Vec::with_capacity((map.voxels.len() + live.len()) * 16); - let mut local_data = Vec::with_capacity(live.len() * 2 * 16); - let mut global_n: i32 = 0; - let mut local_n: i32 = 0; - - let write_point = |data: &mut Vec, n: &mut i32, x: f32, y: f32, z: f32| { - data.extend_from_slice(&x.to_le_bytes()); - data.extend_from_slice(&y.to_le_bytes()); - data.extend_from_slice(&z.to_le_bytes()); - data.extend_from_slice(&0.0_f32.to_le_bytes()); - *n += 1; - }; - - // add healthy voxels to global, and local if necessary - for (x, y, z) in iter_global_points(map, voxel_size) { - write_point(&mut global_data, &mut global_n, x, y, z); - if cylinder.contains(x, y, z) { - write_point(&mut local_data, &mut local_n, x, y, z); - } - } - - // add live voxels to both if they aren't already there - for &(kx, ky, kz) in live { + live.iter().filter_map(move |&(kx, ky, kz)| { if matches!(map.voxels.get(&(kx, ky, kz)), Some(c) if c.health > 0) { - continue; + return None; } - let x = kx as f32 * voxel_size + half; - let y = ky as f32 * voxel_size + half; - let z = kz as f32 * voxel_size + half; - write_point(&mut global_data, &mut global_n, x, y, z); - write_point(&mut local_data, &mut local_n, x, y, z); - } + Some(( + kx as f32 * voxel_size + half, + ky as f32 * voxel_size + half, + kz as f32 * voxel_size + half, + )) + }) +} +fn make_cloud(data: Vec, n: i32, frame_id: &str, stamp: Time) -> PointCloud2 { let make_field = |name: &str, off: i32| PointField { name: name.into(), offset: off, datatype: PointField::FLOAT32 as u8, count: 1, }; - let fields = vec![ - make_field("x", 0), - make_field("y", 4), - make_field("z", 8), - make_field("intensity", 12), - ]; - - let global_cloud = PointCloud2 { - header: Header { - seq: 0, - stamp: stamp.clone(), - frame_id: frame_id.into(), - }, - height: 1, - width: global_n, - fields: fields.clone(), - is_bigendian: false, - point_step: 16, - row_step: 16 * global_n, - data: global_data, - is_dense: true, - }; - let local_cloud = PointCloud2 { + PointCloud2 { header: Header { seq: 0, stamp, frame_id: frame_id.into(), }, height: 1, - width: local_n, - fields, + width: n, + fields: vec![ + make_field("x", 0), + make_field("y", 4), + make_field("z", 8), + make_field("intensity", 12), + ], is_bigendian: false, point_step: 16, - row_step: 16 * local_n, - data: local_data, + row_step: 16 * n, + data, is_dense: true, - }; + } +} - (global_cloud, local_cloud) +/// All healthy voxels plus this frame's live voxels. +fn build_global_cloud( + map: &VoxelMap, + live: &AHashSet, + voxel_size: f32, + frame_id: &str, + stamp: Time, +) -> PointCloud2 { + let mut data = Vec::with_capacity((map.voxels.len() + live.len()) * 16); + let mut n: i32 = 0; + for (x, y, z) in iter_global_points(map, voxel_size) { + write_point(&mut data, &mut n, x, y, z); + } + for (x, y, z) in unhealthy_live_centers(map, live, voxel_size) { + write_point(&mut data, &mut n, x, y, z); + } + make_cloud(data, n, frame_id, stamp) +} + +/// Healthy voxels from global map inside the cylinder plus this frame's live voxels. +fn build_local_cloud( + map: &VoxelMap, + live: &AHashSet, + voxel_size: f32, + cylinder: &LocalBounds, + frame_id: &str, + stamp: Time, +) -> PointCloud2 { + let mut data = Vec::with_capacity(live.len() * 2 * 16); + let mut n: i32 = 0; + for (x, y, z) in iter_global_points(map, voxel_size) { + if cylinder.contains(x, y, z) { + write_point(&mut data, &mut n, x, y, z); + } + } + for (x, y, z) in unhealthy_live_centers(map, live, voxel_size) { + write_point(&mut data, &mut n, x, y, z); + } + make_cloud(data, n, frame_id, stamp) } #[tokio::main] @@ -357,8 +381,8 @@ mod tests { z_min: 0.0, z_max: 1.0, }; - let (global, local) = - build_pointclouds(&map, &live, 1.0, &cylinder, "world", Time::default()); + let global = build_global_cloud(&map, &live, 1.0, "world", Time::default()); + let local = build_local_cloud(&map, &live, 1.0, &cylinder, "world", Time::default()); assert!(cloud_points(&global).contains(&voxel_center(0, 0, 0))); assert!(cloud_points(&local).contains(&voxel_center(0, 0, 0))); } @@ -375,8 +399,8 @@ mod tests { z_min: -10.0, z_max: 10.0, }; - let (global, local) = - build_pointclouds(&map, &live, 1.0, &cylinder, "world", Time::default()); + let global = build_global_cloud(&map, &live, 1.0, "world", Time::default()); + let local = build_local_cloud(&map, &live, 1.0, &cylinder, "world", Time::default()); assert!(cloud_points(&global).contains(&voxel_center(5, 0, 0))); assert!(!cloud_points(&local).contains(&voxel_center(5, 0, 0))); assert_eq!(local.width, 0); @@ -394,8 +418,8 @@ mod tests { z_min: 0.0, z_max: 1.0, }; - let (global, local) = - build_pointclouds(&map, &live, 1.0, &cylinder, "world", Time::default()); + let global = build_global_cloud(&map, &live, 1.0, "world", Time::default()); + let local = build_local_cloud(&map, &live, 1.0, &cylinder, "world", Time::default()); assert!(cloud_points(&global).contains(&voxel_center(0, 0, 5))); assert!(!cloud_points(&local).contains(&voxel_center(0, 0, 5))); assert_eq!(local.width, 0); @@ -413,8 +437,8 @@ mod tests { z_min: 0.0, z_max: 0.0, }; - let (global, local) = - build_pointclouds(&map, &live, 1.0, &cylinder, "world", Time::default()); + let global = build_global_cloud(&map, &live, 1.0, "world", Time::default()); + let local = build_local_cloud(&map, &live, 1.0, &cylinder, "world", Time::default()); assert!(cloud_points(&global).contains(&voxel_center(10, 10, 10))); assert!(cloud_points(&local).contains(&voxel_center(10, 10, 10))); } diff --git a/dimos/mapping/ray_tracing/rust/src/python.rs b/dimos/mapping/ray_tracing/rust/src/python.rs index 852516b9f7..99f81fc24b 100644 --- a/dimos/mapping/ray_tracing/rust/src/python.rs +++ b/dimos/mapping/ray_tracing/rust/src/python.rs @@ -92,6 +92,7 @@ impl VoxelRayMapper { graze_cos, recency_window, emit_every: 1, + global_emit_every: 1, region_percentile: 95.0, }; config diff --git a/dimos/mapping/ray_tracing/rust/src/voxel_ray_tracer.rs b/dimos/mapping/ray_tracing/rust/src/voxel_ray_tracer.rs index ff995155f2..96d4346577 100644 --- a/dimos/mapping/ray_tracing/rust/src/voxel_ray_tracer.rs +++ b/dimos/mapping/ray_tracing/rust/src/voxel_ray_tracer.rs @@ -35,9 +35,13 @@ pub struct Config { /// Only spare a voxel whose neighborhood was hit within this many frames. /// A stale voxel can be cleared, even if it's a grazing hit. Large disables it. pub recency_window: u32, - /// Integrate every frame, publish maps every Nth frame. - #[validate(range(min = 1))] + /// Integrate every frame, publish the local map and region bounds every + /// Nth frame. Zero disables them. + #[validate(range(min = 0))] pub emit_every: u32, + /// Publish the global map every Nth frame. Zero disables it. + #[validate(range(min = 0))] + pub global_emit_every: u32, /// Size the local region to this percentile of batch point distances, /// so a stray far hit cannot inflate the region the planner recomputes. #[validate(range(min = 0.0, max = 100.0))] @@ -687,6 +691,7 @@ mod tests { graze_cos: 0.5, recency_window: 60, emit_every: 1, + global_emit_every: 1, region_percentile: 95.0, } } @@ -875,6 +880,7 @@ mod tests { graze_cos: 0.5, recency_window: 60, emit_every: 1, + global_emit_every: 1, region_percentile: 95.0, }; // Build the floor over a y band so it is a 2d plane, not a wire. @@ -1029,6 +1035,7 @@ mod tests { graze_cos: 0.5, recency_window: 60, emit_every: 1, + global_emit_every: 1, region_percentile: 95.0, }; @@ -1102,6 +1109,7 @@ mod tests { graze_cos: 0.5, recency_window: 60, emit_every: 1, + global_emit_every: 1, region_percentile: 95.0, }; @@ -1163,6 +1171,7 @@ mod tests { graze_cos, recency_window: 60, emit_every: 1, + global_emit_every: 1, region_percentile: 95.0, }; @@ -1293,6 +1302,7 @@ mod tests { graze_cos: 0.5, recency_window, emit_every: 1, + global_emit_every: 1, region_percentile: 95.0, }; let (mut map, _) = build_surface(&floor, voxel_size, cfg.max_health); diff --git a/dimos/navigation/nav_3d/mls_planner/mls_planner.pyi b/dimos/navigation/nav_3d/mls_planner/mls_planner.pyi index 68cc5dea93..f0d589e3dd 100644 --- a/dimos/navigation/nav_3d/mls_planner/mls_planner.pyi +++ b/dimos/navigation/nav_3d/mls_planner/mls_planner.pyi @@ -85,13 +85,6 @@ class MLSPlanner: """Accumulated occupied voxel centers as (N, 3) float32, for visualization.""" ... - def last_timings(self) -> dict[str, float]: - """Per-substep wall-clock ms of the last update_global_map. - - Keys are voxelize_ms, surfaces_ms, and graph_ms. - """ - ... - def clear(self) -> None: """Drop the graph and buffered state.""" ... diff --git a/dimos/navigation/nav_3d/mls_planner/mls_planner_native.py b/dimos/navigation/nav_3d/mls_planner/mls_planner_native.py index 9d046e0237..acc0fd6460 100644 --- a/dimos/navigation/nav_3d/mls_planner/mls_planner_native.py +++ b/dimos/navigation/nav_3d/mls_planner/mls_planner_native.py @@ -46,8 +46,8 @@ class MLSPlannerNativeConfig(NativeModuleConfig): class MLSPlannerNative(NativeModule): """Rust-backed MLS planner. - Feed either global_map (full rebuild per message) or the local_map plus - region_bounds pair from RayTracingVoxelMap (incremental region updates). + Feed either global_map, which rebuilds fully per message, or the local_map + plus region_bounds pair from RayTracingVoxelMap for incremental updates. """ config: MLSPlannerNativeConfig diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/main.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/main.rs index 497a43534e..0d1bd95c20 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/main.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/main.rs @@ -1,7 +1,7 @@ // Copyright 2026 Dimensional Inc. // SPDX-License-Identifier: Apache-2.0 -use std::time::{Duration, Instant, SystemTime, UNIX_EPOCH}; +use std::time::{Duration, SystemTime, UNIX_EPOCH}; use dimos_mls_planner::edges::{edges_to_segments, PlannerGraph}; use dimos_mls_planner::mls_planner::{Config, Planner, RegionBounds}; @@ -19,7 +19,7 @@ struct MlsPlanner { global_map: Input, // Incremental path: a local map slice paired by stamp with the region - // bounds it covers (see the ray tracer's region_bounds output). + // bounds it covers, published by the ray tracer alongside local_map. #[input(decode = PointCloud2::decode, handler = on_local_map)] local_map: Input, @@ -70,9 +70,7 @@ impl MlsPlanner { return; } - let t = Instant::now(); self.planner.update_global_map(&points, &self.config); - let rebuild_ms = ms(t.elapsed()); self.publish_graph().await; @@ -82,7 +80,6 @@ impl MlsPlanner { surface_cells = self.planner.surface().count(), nodes = self.planner.graph().nodes.len(), edges = self.planner.graph().node_edges.len(), - rebuild_ms, "global_map processed", ); } @@ -128,9 +125,7 @@ impl MlsPlanner { z_max: bounds_msg.pose.orientation.z as f32, }; - let t = Instant::now(); self.planner.update_region(&points, &bounds, &self.config); - let update_ms = ms(t.elapsed()); self.publish_graph().await; @@ -138,7 +133,6 @@ impl MlsPlanner { local_points = points.len(), voxels = self.planner.voxel_count(), nodes = self.planner.graph().nodes.len(), - update_ms, "local region processed", ); } @@ -167,6 +161,7 @@ impl MlsPlanner { self.latest_start = Some((p.x as f32, p.y as f32, p.z as f32)); // Drop any previous plan so the visualizer doesn't show a stale path // rooted at the old start. + info!("canceling any active path, start pose changed"); publish_path(&self.path, &empty_path(&self.config.world_frame, now())).await; } @@ -179,7 +174,6 @@ impl MlsPlanner { let p = &msg.pose.position; let goal = (p.x as f32, p.y as f32, p.z as f32); - let t_plan = Instant::now(); let waypoints = match self.planner.plan(start, goal, &self.config) { Some(wp) => wp, None => { @@ -188,19 +182,13 @@ impl MlsPlanner { return; } }; - let plan_ms = ms(t_plan.elapsed()); - let stamp = now(); let path_msg = build_path_from_waypoints(&waypoints, &self.config.world_frame, stamp); - info!(waypoints = waypoints.len(), plan_ms, "path planned"); + info!(waypoints = waypoints.len(), "path planned"); publish_path(&self.path, &path_msg).await; } } -fn ms(d: Duration) -> f64 { - d.as_secs_f64() * 1000.0 -} - fn same_stamp(a: &Time, b: &Time) -> bool { a.sec == b.sec && a.nsec == b.nsec } diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs index 0b477b5f79..f519a5d7cb 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs @@ -3,8 +3,6 @@ //! Config and the owned-state Planner that builds and queries the MLS graph. -use std::time::Instant; - use ahash::AHashSet; use rayon::prelude::*; use serde::Deserialize; @@ -55,14 +53,6 @@ fn default_wall_penalty_weight() -> f32 { 4.0 } -/// Wall-clock cost in ms of each stage of the most recent map update. -#[derive(Default, Clone, Copy)] -pub struct RebuildTimings { - pub voxelize_ms: f64, - pub surfaces_ms: f64, - pub graph_ms: f64, -} - /// Cylindrical region the planner re-derives from a local map slice. pub struct RegionBounds { pub origin_x: f32, @@ -100,7 +90,6 @@ pub struct Planner { graph: PlannerGraph, voxel_map: AHashSet, by_col: ColumnIz, - last_timings: RebuildTimings, } impl Planner { @@ -108,14 +97,11 @@ impl Planner { let voxel_size = config.voxel_size; let clearance = (config.robot_height / voxel_size).ceil() as i32; - let t_voxelize = Instant::now(); self.voxel_map.clear(); for &p in points { self.voxel_map.insert(voxelize(p, voxel_size)); } - let voxelize_ms = ms(t_voxelize.elapsed()); - let t_surfaces = Instant::now(); let mut surface: Vec = Vec::new(); extract_surfaces( &self.voxel_map, @@ -126,19 +112,12 @@ impl Planner { &mut surface, ); build_surface_lookup(&surface, &mut self.graph.surface_lookup); - let surfaces_ms = ms(t_surfaces.elapsed()); - let graph_ms = self.rebuild_graph(config); - - self.last_timings = RebuildTimings { - voxelize_ms, - surfaces_ms, - graph_ms, - }; + self.rebuild_graph(config); } - /// Re-derive the planner from a local map slice, recomputing only where - /// voxels changed and reusing everything else. + /// Update planner artifacts within a local region instead of recomputing + /// the entire planner on the entire map. pub fn update_region( &mut self, local_points: &[(f32, f32, f32)], @@ -149,21 +128,13 @@ impl Planner { let clearance = (config.robot_height / voxel_size).ceil() as i32; let pad = (config.surface_dilation_passes + config.surface_erosion_passes) as i32; - let t_voxelize = Instant::now(); let changed = self.replace_region_voxels(local_points, bounds, voxel_size); - let voxelize_ms = ms(t_voxelize.elapsed()); // No voxel changed, so surfaces and the graph are untouched. let Some((bx0, bx1, by0, by1)) = changed else { - self.last_timings = RebuildTimings { - voxelize_ms, - surfaces_ms: 0.0, - graph_ms: 0.0, - }; return; }; - let t_surfaces = Instant::now(); // A changed voxel column shifts surfaces only within pad of it, so the // write-back box is the changed-column bbox grown by pad. let write = (bx0 - pad, bx1 + pad, by0 - pad, by1 + pad); @@ -175,17 +146,8 @@ impl Planner { write, ); let (added, removed) = self.replace_surface_region(write, &new_cells); - let surfaces_ms = ms(t_surfaces.elapsed()); - let t_graph = Instant::now(); self.rebuild_region_graph(added, removed, config); - let graph_ms = ms(t_graph.elapsed()); - - self.last_timings = RebuildTimings { - voxelize_ms, - surfaces_ms, - graph_ms, - }; } /// Patch cells for the changed surface, then re-place nodes and edges over @@ -288,9 +250,9 @@ impl Planner { bb.bounds() } - /// Replace the surface_lookup entries for columns in the write box with the - /// freshly extracted cells. Returns the added and removed surface cells so - /// the graph can be patched surgically. + /// Replace the surface_lookup entries for columns in the write box with + /// the freshly extracted cells. Returns the added and removed cells so + /// only the affected parts of the graph get patched. fn replace_surface_region( &mut self, write: (i32, i32, i32, i32), @@ -309,15 +271,17 @@ impl Planner { } let new: AHashSet = new_cells.iter().copied().collect(); + let mut touched: AHashSet<(i32, i32)> = AHashSet::new(); for &(ix, iy, iz) in new_cells { self.graph .surface_lookup .entry((ix, iy)) .or_default() .push(iz); + touched.insert((ix, iy)); } - for &(ix, iy, _) in new_cells { - if let Some(zs) = self.graph.surface_lookup.get_mut(&(ix, iy)) { + for col in touched { + if let Some(zs) = self.graph.surface_lookup.get_mut(&col) { zs.sort_unstable(); zs.dedup(); } @@ -329,12 +293,10 @@ impl Planner { } /// Rebuild all cells from surface_lookup, then nodes and edges. - /// Returns graph_ms. - fn rebuild_graph(&mut self, config: &Config) -> f64 { + fn rebuild_graph(&mut self, config: &Config) { let voxel_size = config.voxel_size; let step = (config.node_step_threshold_m / voxel_size).floor() as i32; - let t_graph = Instant::now(); build_surface_cells( &mut self.graph.cells, &self.graph.surface_lookup, @@ -342,7 +304,6 @@ impl Planner { step, ); self.rebuild_nodes(config); - ms(t_graph.elapsed()) } /// Live cells within the changed-cell bbox grown by the node-graph margin, @@ -392,7 +353,7 @@ impl Planner { ids.into_iter().collect() } - /// Place nodes and build node edges from the current cells (full rebuild). + /// Full rebuild of nodes and node edges from the current cells. fn rebuild_nodes(&mut self, config: &Config) { place_nodes( &mut self.graph.cells, @@ -458,14 +419,6 @@ impl Planner { pub fn voxel_keys(&self) -> impl Iterator + '_ { self.voxel_map.iter().copied() } - - pub fn last_timings(&self) -> RebuildTimings { - self.last_timings - } -} - -fn ms(d: std::time::Duration) -> f64 { - d.as_secs_f64() * 1000.0 } /// Running inclusive xy bounding box of changed columns. @@ -615,8 +568,8 @@ mod region_tests { .filter(|&p| !bounds.contains_voxel(voxelize(p, cfg.voxel_size), cfg.voxel_size)) .collect(); - // Seed the cylinder with a stack of junk voxels not present in `all`, - // so update_region must clear them (and the surface they induce). + // Seed the cylinder with a stack of junk voxels not present in the + // world, so update_region must clear them and the surface they induce. let mut seeded = outside.clone(); for iz in 3..8 { seeded.push((2.05, 2.05, iz as f32 * cfg.voxel_size + 0.05)); diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/nodes.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/nodes.rs index bbe4ddf408..a6e3160b27 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/nodes.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/nodes.rs @@ -396,6 +396,33 @@ mod tests { } } + #[test] + fn wall_penalty_weight_scales_edge_costs() { + // On a 1-wide strip every cell is wall-adjacent, so the penalty + // multiplier is exactly 1 + weight and edge cost is base times it. + let cells_in: Vec = (0..10).map(|ix| (ix, 0, 0)).collect(); + let cost_with = |weight: f32| { + let mut sc = build_cells(&cells_in, 2); + let mut state = DijkstraState::default(); + let mut nodes = Vec::new(); + place_nodes( + &mut sc, VOXEL, 1.0, 0.3, 0.0, weight, &mut state, &mut nodes, + ); + let id = sc.id((5, 0, 0)).unwrap(); + sc.neighbors(id)[0].cost + }; + let unweighted = cost_with(0.0); + assert!( + (unweighted - VOXEL).abs() < 1e-5, + "zero weight must leave the geometric cost, got {unweighted}" + ); + assert!( + (cost_with(4.0) - 5.0 * VOXEL).abs() < 1e-5, + "weight 4 at the wall must scale cost by 5" + ); + assert!(cost_with(4.0) > cost_with(1.0)); + } + #[test] fn wall_cells_scale_outbound_cost() { let cells_in: Vec = (0..10).map(|ix| (ix, 0, 0)).collect(); diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs index aae5abec98..162e6b6e72 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs @@ -387,9 +387,8 @@ fn cells_to_waypoints( } /// Shortcut runs of cells with straight on-surface segments, keeping the -/// farthest cell in line of sight from each anchor. A shortcut is only taken -/// when it never passes closer than buffer_m to a wall, so smoothing cannot -/// erode the wall clearance the penalized routing built in. +/// farthest cell in line of sight from each anchor. Shortcuts never pass +/// still account for the wall buffer, so even the new path should be safe. fn string_pull(plg: &PlannerGraph, cells: &[CellId], tol_cells: i32, buffer_m: f32) -> Vec { if cells.len() <= 2 { return cells.to_vec(); @@ -414,10 +413,8 @@ fn string_pull(plg: &PlannerGraph, cells: &[CellId], tol_cells: i32, buffer_m: f out } -/// True if every column the segment crosses holds a surface cell within -/// tol_cells of the interpolated segment height, and that cell stays at least -/// buffer_m from the nearest wall. Cells without a wall-distance value are -/// treated as open, so an unpopulated field reduces to a pure on-surface test. +/// True if every column the segment crosses has a surface cell near the +/// segment height that is at least buffer_m from the nearest wall. fn los_on_surface( plg: &PlannerGraph, a: VoxelKey, diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/python.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/python.rs index 376f98e206..4f70de39e4 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/python.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/python.rs @@ -5,7 +5,6 @@ use numpy::ndarray::Array2; use numpy::{IntoPyArray, PyArray2, PyReadonlyArray2, PyUntypedArrayMethods}; use pyo3::exceptions::PyValueError; use pyo3::prelude::*; -use pyo3::types::PyDict; use validator::Validate; use crate::edges::edges_to_segments; @@ -263,16 +262,6 @@ impl MLSPlanner { .into_pyarray(py) } - /// Per-substep wall-clock cost (ms) of the most recent map update. - fn last_timings<'py>(&self, py: Python<'py>) -> PyResult> { - let t = self.planner.last_timings(); - let d = PyDict::new(py); - d.set_item("voxelize_ms", t.voxelize_ms)?; - d.set_item("surfaces_ms", t.surfaces_ms)?; - d.set_item("graph_ms", t.graph_ms)?; - Ok(d) - } - fn clear(&mut self) { self.planner = Planner::default(); } diff --git a/dimos/navigation/nav_3d/mls_planner/test_transformer.py b/dimos/navigation/nav_3d/mls_planner/test_transformer.py index d808591e1d..3fca1fb081 100644 --- a/dimos/navigation/nav_3d/mls_planner/test_transformer.py +++ b/dimos/navigation/nav_3d/mls_planner/test_transformer.py @@ -57,6 +57,40 @@ def test_flat_floor_yields_populated_path_and_planned_true() -> None: assert out.tags["planned"] is True assert len(out.data.poses) >= 2 + assert out.tags["voxels"] > 0 + assert out.tags["voxel_map"].shape == (out.tags["voxels"], 3) + assert out.tags["surface_clearance"].shape[1] == 4 + assert set(out.tags["timings"]) == {"update_ms", "plan_ms", "total_ms"} + + +def test_poseless_obs_is_skipped() -> None: + points = _flat_floor() + poseless = Observation( + id=1, + ts=0.0, + pose=None, + tags={"region_bounds": (0.0, 0.0, 5.0, -1.0, 2.0)}, + _data=PointCloud2.from_numpy(points), + ) + posed = _obs(points, pose=(-2.0, -2.0, 1.0), region_bounds=(0.0, 0.0, 5.0, -1.0, 2.0)) + + results = list( + MLSPlan(goal=(2.0, 2.0, 0.0), voxel_size=0.2, robot_height=1.0)(iter([poseless, posed])) + ) + + assert len(results) == 1 + + +def test_missing_region_bounds_raises() -> None: + obs = Observation( + id=0, + ts=0.0, + pose=(0.0, 0.0, 0.0), + _data=PointCloud2.from_numpy(np.zeros((1, 3), dtype=np.float32)), + ) + + with pytest.raises(ValueError, match="local map slices"): + list(MLSPlan(goal=(1.0, 1.0, 0.0))(iter([obs]))) def test_start_z_is_dropped_by_robot_height() -> None: @@ -72,7 +106,7 @@ def test_start_z_is_dropped_by_robot_height() -> None: def test_no_route_yields_empty_path_with_planned_false() -> None: - # Random points form no traversable surface, so plan() returns None. + # Random points form no traversable surface, so planning fails. rng = np.random.default_rng(0) obs = _obs( rng.random((50, 3)).astype(np.float32), diff --git a/dimos/navigation/nav_3d/mls_planner/transformer.py b/dimos/navigation/nav_3d/mls_planner/transformer.py index ed606092a0..f27a620b2f 100644 --- a/dimos/navigation/nav_3d/mls_planner/transformer.py +++ b/dimos/navigation/nav_3d/mls_planner/transformer.py @@ -83,14 +83,18 @@ def __call__( "MLSPlan consumes local map slices; construct RayTraceMap(emit_local=True)" ) ox, oy, radius, z_min, z_max = bounds + t_update = time.perf_counter() planner.update_region(obs.data.points_f32(), (ox, oy), radius, z_min, z_max) t_plan = time.perf_counter() waypoints = planner.plan(start, self.goal) - plan_ms = (time.perf_counter() - t_plan) * 1000 + t_done = time.perf_counter() path = self._path_from_waypoints(waypoints, obs.ts) - timings = {**planner.last_timings(), "plan_ms": plan_ms} - timings["total_ms"] = sum(timings.values()) + timings = { + "update_ms": (t_plan - t_update) * 1000, + "plan_ms": (t_done - t_plan) * 1000, + "total_ms": (t_done - t_update) * 1000, + } yield obs.derive( data=path, diff --git a/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py b/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py index 81629debf7..bfa5638c01 100644 --- a/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py +++ b/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py @@ -17,7 +17,6 @@ from __future__ import annotations from pathlib import Path as FsPath -from typing import Any import numpy as np from numpy.typing import NDArray @@ -38,7 +37,7 @@ TIMELINE = "ts" -TIMING_KEYS = ["voxelize_ms", "surfaces_ms", "graph_ms", "plan_ms", "total_ms"] +TIMING_KEYS = ["update_ms", "plan_ms", "total_ms"] SIZE_KEYS = ["voxels", "surface_cells", "nodes", "edges"] @@ -59,9 +58,11 @@ def _print_summary(streams: dict[str, dict[str, Stream[float]]]) -> None: print(f" {kind}/{key:<14} {mean:9.2f} {p50:9.2f} {p95:9.2f} {peak:9.2f}") -def _attach_pose_from_odom(pair_obs: Observation[Any]) -> Observation[PointCloud2]: - lidar_obs: Observation[PointCloud2] = pair_obs.data[0] - odom_obs: Observation[Odometry] = pair_obs.data[1] +PairObs = Observation[tuple[Observation[PointCloud2], Observation[Odometry]]] + + +def _attach_pose_from_odom(pair_obs: PairObs) -> Observation[PointCloud2]: + lidar_obs, odom_obs = pair_obs.data odom = odom_obs.data pose_tuple = ( float(odom.position.x), From 0d16a727389d443f6b1bc31f8afe1974e8c713ce Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Fri, 12 Jun 2026 13:19:53 -0700 Subject: [PATCH 16/56] Bring back check --- dimos/mapping/ray_tracing/test_transformer.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/dimos/mapping/ray_tracing/test_transformer.py b/dimos/mapping/ray_tracing/test_transformer.py index 7210c21eb7..44ccb82ad2 100644 --- a/dimos/mapping/ray_tracing/test_transformer.py +++ b/dimos/mapping/ray_tracing/test_transformer.py @@ -50,6 +50,11 @@ def test_emit_every_n_yields_on_cadence_and_flushes_remainder() -> None: assert [r.tags["frame_count"] for r in results] == [3, 6, 7] +def test_negative_emit_every_is_rejected() -> None: + with pytest.raises(ValueError, match="emit_every"): + RayTraceMap(emit_every=-1) + + def test_pose_propagates_to_emitted_obs() -> None: pose = (1.5, -2.0, 0.5) obs = _obs(_cube(), ts=1.0, pose=pose) From c7788ca6d5304b359f13dfbcf46df56179224d3a Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Fri, 12 Jun 2026 13:54:11 -0700 Subject: [PATCH 17/56] Local map bug fix --- dimos/mapping/ray_tracing/rust/src/main.rs | 17 ++++++---- .../mls_planner/rust/src/mls_planner.rs | 31 +++++++++++++++++-- 2 files changed, 39 insertions(+), 9 deletions(-) diff --git a/dimos/mapping/ray_tracing/rust/src/main.rs b/dimos/mapping/ray_tracing/rust/src/main.rs index 45a9ab51a1..ba2ca2a0fe 100644 --- a/dimos/mapping/ray_tracing/rust/src/main.rs +++ b/dimos/mapping/ray_tracing/rust/src/main.rs @@ -313,7 +313,7 @@ fn build_global_cloud( make_cloud(data, n, frame_id, stamp) } -/// Healthy voxels from global map inside the cylinder plus this frame's live voxels. +/// Healthy voxels and this frame's live voxels, all inside the cylinder. fn build_local_cloud( map: &VoxelMap, live: &AHashSet, @@ -330,7 +330,9 @@ fn build_local_cloud( } } for (x, y, z) in unhealthy_live_centers(map, live, voxel_size) { - write_point(&mut data, &mut n, x, y, z); + if cylinder.contains(x, y, z) { + write_point(&mut data, &mut n, x, y, z); + } } make_cloud(data, n, frame_id, stamp) } @@ -426,20 +428,23 @@ mod tests { } #[test] - fn local_map_always_includes_live_voxels() { + fn live_voxels_follow_the_cylinder_in_local_map() { let map = VoxelMap::default(); let mut live: AHashSet = AHashSet::new(); + live.insert((1, 0, 0)); live.insert((10, 10, 10)); let cylinder = LocalBounds { origin_x: 0.0, origin_y: 0.0, - r_xy_max_sq: 0.0, + r_xy_max_sq: 4.0, z_min: 0.0, - z_max: 0.0, + z_max: 1.0, }; let global = build_global_cloud(&map, &live, 1.0, "world", Time::default()); let local = build_local_cloud(&map, &live, 1.0, &cylinder, "world", Time::default()); + assert!(cloud_points(&global).contains(&voxel_center(1, 0, 0))); assert!(cloud_points(&global).contains(&voxel_center(10, 10, 10))); - assert!(cloud_points(&local).contains(&voxel_center(10, 10, 10))); + assert!(cloud_points(&local).contains(&voxel_center(1, 0, 0))); + assert!(!cloud_points(&local).contains(&voxel_center(10, 10, 10))); } } diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs index f519a5d7cb..deb35d1e23 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs @@ -200,9 +200,9 @@ impl Planner { ); } - /// Replace the cylinder's voxels with the local map points and keep the - /// per-column index in sync. Returns the column bbox of changed voxels, or - /// None if nothing changed. Bounded by the cylinder, never the whole map. + /// Replace the cylinder's voxels with the local map points, ignoring + /// points outside it. Returns the column bbox of changed voxels, or None + /// if nothing changed. fn replace_region_voxels( &mut self, local_points: &[(f32, f32, f32)], @@ -242,6 +242,9 @@ impl Planner { remove_from_by_col(&mut self.by_col, k); } for &k in &new_set { + if !bounds.contains_voxel(k, voxel_size) { + continue; + } if self.voxel_map.insert(k) { bb.add(k.0, k.1); add_to_by_col(&mut self.by_col, k); @@ -593,6 +596,28 @@ mod region_tests { ); } + /// A point outside the region bounds must not enter the planner's voxel + /// map, where it could never be cleared and would inflate the rebuild box. + #[test] + fn region_update_ignores_points_outside_bounds() { + let cfg = test_config(); + let bounds = RegionBounds { + origin_x: 2.0, + origin_y: 2.0, + radius: 1.0, + z_min: -1.0, + z_max: 2.0, + }; + let inside = (2.05, 2.05, 0.05); + let outside = (10.05, 10.05, 0.05); + + let mut p = Planner::default(); + p.update_region(&[inside, outside], &bounds, &cfg); + + assert!(p.voxel_map.contains(&voxelize(inside, cfg.voxel_size))); + assert!(!p.voxel_map.contains(&voxelize(outside, cfg.voxel_size))); + } + /// Floor 8m x 8m with a wall at x=4m that only a gap at y in [3.5, 4.5] /// passes through, so crossing the wall is a non-trivial route. fn big_world() -> Vec<(f32, f32, f32)> { From 86aa7c3f2d5c1734331ae36bf9110f9678ee91e2 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Fri, 12 Jun 2026 16:34:27 -0700 Subject: [PATCH 18/56] Logging --- dimos/mapping/ray_tracing/transformer.py | 4 ++++ dimos/navigation/nav_3d/mls_planner/transformer.py | 4 ++++ dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py | 2 +- 3 files changed, 9 insertions(+), 1 deletion(-) diff --git a/dimos/mapping/ray_tracing/transformer.py b/dimos/mapping/ray_tracing/transformer.py index 2936e9234f..e4afbd9287 100644 --- a/dimos/mapping/ray_tracing/transformer.py +++ b/dimos/mapping/ray_tracing/transformer.py @@ -23,6 +23,7 @@ from dimos.mapping.ray_tracing.voxel_map import VoxelRayMapper, local_bounds from dimos.memory2.transform import Transformer from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: from collections.abc import Iterator @@ -31,6 +32,8 @@ from dimos.memory2.type.observation import Observation +logger = setup_logger() + class RayTraceMap(Transformer[PointCloud2, PointCloud2]): """Accumulate world-frame lidar into a voxel map with raycast clearing.""" @@ -109,6 +112,7 @@ def __call__( for obs in upstream: if obs.pose_tuple is None: + logger.debug("RayTraceMap: obs %s has no pose; skipping", obs.id) continue x, y, z, *_ = obs.pose_tuple pts = obs.data.points_f32() diff --git a/dimos/navigation/nav_3d/mls_planner/transformer.py b/dimos/navigation/nav_3d/mls_planner/transformer.py index f27a620b2f..3f9bc09b7a 100644 --- a/dimos/navigation/nav_3d/mls_planner/transformer.py +++ b/dimos/navigation/nav_3d/mls_planner/transformer.py @@ -22,6 +22,7 @@ from dimos.msgs.nav_msgs.Path import Path from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.navigation.nav_3d.mls_planner.mls_planner import MLSPlanner +from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: from collections.abc import Iterator @@ -31,6 +32,8 @@ from dimos.memory2.type.observation import Observation +logger = setup_logger() + class MLSPlan(Transformer[PointCloud2, Path]): """Plan paths from current pose to a fixed goal over an accumulating voxel map.""" @@ -73,6 +76,7 @@ def __call__( ) for obs in upstream: if obs.pose_tuple is None: + logger.debug("MLSPlan: obs %s has no pose; skipping", obs.id) continue x, y, z, *_ = obs.pose_tuple start = (float(x), float(y), float(z) - self.robot_height) diff --git a/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py b/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py index bfa5638c01..70fb38ae36 100644 --- a/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py +++ b/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Dev replay util: run a lidar+odometry .db through RayTraceMap and MLSPlan into rerun.""" +"""Replay a lidar+odometry .db through RayTraceMap and MLSPlan into rerun.""" from __future__ import annotations From c21be70e107061966a6803ecf04ad6bc8d151254 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Thu, 11 Jun 2026 15:01:35 -0700 Subject: [PATCH 19/56] Kronk nav --- .../navigation/basic_path_follower/module.py | 160 ++++++++++++++++++ .../basic_path_follower/test_module.py | 121 +++++++++++++ .../nav_3d/mls_planner/goal_relay.py | 70 ++++++++ dimos/robot/all_blueprints.py | 3 + .../navigation/unitree_go2_nav_3d.py | 62 +++++++ 5 files changed, 416 insertions(+) create mode 100644 dimos/navigation/basic_path_follower/module.py create mode 100644 dimos/navigation/basic_path_follower/test_module.py create mode 100644 dimos/navigation/nav_3d/mls_planner/goal_relay.py create mode 100644 dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py diff --git a/dimos/navigation/basic_path_follower/module.py b/dimos/navigation/basic_path_follower/module.py new file mode 100644 index 0000000000..399fccf002 --- /dev/null +++ b/dimos/navigation/basic_path_follower/module.py @@ -0,0 +1,160 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from __future__ import annotations + +import math +from threading import Event, RLock, Thread +import time +from typing import Any + +from dimos_lcm.std_msgs import Bool # type: ignore[import-untyped] +import numpy as np +from reactivex.disposable import Disposable + +from dimos.core.core import rpc +from dimos.core.module import Module, ModuleConfig +from dimos.core.stream import In, Out +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.nav_msgs.Path import Path +from dimos.utils.logging_config import setup_logger +from dimos.utils.trigonometry import angle_diff + +logger = setup_logger() + + +class BasicPathFollowerConfig(ModuleConfig): + speed: float = 0.5 + control_frequency: float = 10.0 + goal_tolerance: float = 0.3 + lookahead_m: float = 0.6 + heading_gain: float = 1.5 + max_angular: float = 1.0 + # Rotate in place instead of advancing when heading error exceeds this. + rotate_threshold: float = 0.6 + + +class BasicPathFollower(Module): + """Follow a planned path by chasing a lookahead point with P-controlled heading. + + Consumes world-frame paths and odometry. Publishes nav_cmd_vel until the + last waypoint is within goal tolerance, then publishes goal_reached. + An empty path or a stop_movement message cancels the current path. + """ + + config: BasicPathFollowerConfig + + path: In[Path] + odometry: In[Odometry] + stop_movement: In[Bool] + + nav_cmd_vel: Out[Twist] + goal_reached: Out[Bool] + + def __init__(self, **kwargs: Any) -> None: + super().__init__(**kwargs) + self._lock = RLock() + self._current_odom: PoseStamped | None = None + self._thread: Thread | None = None + self._stop_event = Event() + + @rpc + def start(self) -> None: + super().start() + self.register_disposable(Disposable(self.odometry.subscribe(self._on_odometry))) + self.register_disposable(Disposable(self.path.subscribe(self._on_path))) + if self.stop_movement.transport is not None: + self.register_disposable(Disposable(self.stop_movement.subscribe(self._on_stop))) + + @rpc + def stop(self) -> None: + self._cancel() + super().stop() + + def _on_odometry(self, msg: Odometry) -> None: + with self._lock: + self._current_odom = msg.to_pose_stamped() + + def _on_path(self, path: Path) -> None: + self._cancel() + waypoints = np.array([[p.position.x, p.position.y] for p in path.poses]) + if len(waypoints) == 0: + return + logger.info("Following new path", waypoints=len(waypoints)) + stop_event = Event() + thread = Thread(target=self._follow, args=(waypoints, stop_event), daemon=True) + with self._lock: + self._stop_event = stop_event + self._thread = thread + thread.start() + + def _on_stop(self, msg: Bool) -> None: + if msg.data: + self._cancel() + + def _cancel(self) -> None: + with self._lock: + self._stop_event.set() + self._thread = None + self.nav_cmd_vel.publish(Twist()) + + def _follow(self, waypoints: np.ndarray, stop_event: Event) -> None: + period = 1.0 / self.config.control_frequency + while not stop_event.is_set(): + start_time = time.perf_counter() + + with self._lock: + odom = self._current_odom + + if odom is None: + stop_event.wait(period) + continue + + position = np.array([odom.position.x, odom.position.y]) + if float(np.linalg.norm(waypoints[-1] - position)) < self.config.goal_tolerance: + self.nav_cmd_vel.publish(Twist()) + self.goal_reached.publish(Bool(True)) + logger.info("Goal reached") + return + + target = self._lookahead_point(waypoints, position) + yaw_error = angle_diff( + math.atan2(target[1] - position[1], target[0] - position[0]), + odom.orientation.euler[2], + ) + + angular = max( + -self.config.max_angular, + min(self.config.max_angular, self.config.heading_gain * yaw_error), + ) + linear = 0.0 + if abs(yaw_error) <= self.config.rotate_threshold: + linear = self.config.speed * max(0.0, math.cos(yaw_error)) + + self.nav_cmd_vel.publish(Twist(Vector3(linear, 0, 0), Vector3(0, 0, angular))) + + elapsed = time.perf_counter() - start_time + stop_event.wait(max(0.0, period - elapsed)) + + def _lookahead_point(self, waypoints: np.ndarray, position: np.ndarray) -> np.ndarray: + closest = int(np.argmin(np.linalg.norm(waypoints - position, axis=1))) + travelled = 0.0 + for i in range(closest + 1, len(waypoints)): + travelled += float(np.linalg.norm(waypoints[i] - waypoints[i - 1])) + if travelled >= self.config.lookahead_m: + return np.asarray(waypoints[i]) + return np.asarray(waypoints[-1]) diff --git a/dimos/navigation/basic_path_follower/test_module.py b/dimos/navigation/basic_path_follower/test_module.py new file mode 100644 index 0000000000..4616660bb3 --- /dev/null +++ b/dimos/navigation/basic_path_follower/test_module.py @@ -0,0 +1,121 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from __future__ import annotations + +from collections.abc import Generator +import time + +from dimos_lcm.std_msgs import Bool # type: ignore[import-untyped] +import pytest + +from dimos.msgs.geometry_msgs.Pose import Pose +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.nav_msgs.Path import Path +from dimos.navigation.basic_path_follower.module import BasicPathFollower + + +def _odom(x: float = 0.0, y: float = 0.0, yaw_quat: tuple = (0, 0, 0, 1)) -> Odometry: + qx, qy, qz, qw = yaw_quat + return Odometry(ts=time.time(), frame_id="odom", pose=Pose(x, y, 0.0, qx, qy, qz, qw)) + + +def _path(*xy: tuple) -> Path: + poses = [PoseStamped(ts=time.time(), frame_id="odom", position=(x, y, 0.0)) for x, y in xy] + return Path(ts=time.time(), frame_id="odom", poses=poses) + + +def _wait_for(predicate, timeout: float = 1.0) -> bool: + deadline = time.time() + timeout + while time.time() < deadline: + if predicate(): + return True + time.sleep(0.01) + return False + + +@pytest.fixture() +def follower() -> Generator[tuple[BasicPathFollower, list, list], None, None]: + module = BasicPathFollower(control_frequency=100.0) + cmds: list = [] + reached: list = [] + unsubs = [ + module.nav_cmd_vel.subscribe(cmds.append), + module.goal_reached.subscribe(reached.append), + ] + try: + yield module, cmds, reached + finally: + module._cancel() + for unsub in unsubs: + unsub() + module._close_module() + + +def test_drives_forward_along_path(follower): + module, cmds, _ = follower + module._on_odometry(_odom(0.0, 0.0)) + module._on_path(_path((1.0, 0.0), (2.0, 0.0), (3.0, 0.0))) + + assert _wait_for(lambda: any(c.linear.x > 0 for c in cmds)) + moving = [c for c in cmds if c.linear.x > 0] + assert abs(moving[0].angular.z) < 0.1 + + +def test_rotates_in_place_when_facing_away(follower): + module, cmds, _ = follower + module._on_odometry(_odom(0.0, 0.0, yaw_quat=(0, 0, 1, 0))) + module._on_path(_path((1.0, 0.0), (2.0, 0.0))) + + assert _wait_for(lambda: len(cmds) > 0 and cmds[-1].linear.x == 0) + assert abs(cmds[-1].angular.z) > 0 + + +def test_goal_reached_when_near_last_waypoint(follower): + module, cmds, reached = follower + module._on_odometry(_odom(2.9, 0.0)) + module._on_path(_path((1.0, 0.0), (2.0, 0.0), (3.0, 0.0))) + + assert _wait_for(lambda: len(reached) == 1) + assert reached[0].data + assert cmds[-1].linear.x == 0 + assert cmds[-1].angular.z == 0 + + +def test_stop_movement_cancels(follower): + module, cmds, _ = follower + module._on_odometry(_odom(0.0, 0.0)) + module._on_path(_path((1.0, 0.0), (2.0, 0.0))) + assert _wait_for(lambda: any(c.linear.x > 0 for c in cmds)) + + module._on_stop(Bool(True)) + time.sleep(0.05) + count = len(cmds) + time.sleep(0.1) + assert len(cmds) == count + assert cmds[-1].linear.x == 0 + + +def test_empty_path_cancels(follower): + module, cmds, _ = follower + module._on_odometry(_odom(0.0, 0.0)) + module._on_path(_path((1.0, 0.0), (2.0, 0.0))) + assert _wait_for(lambda: any(c.linear.x > 0 for c in cmds)) + + module._on_path(_path()) + time.sleep(0.05) + count = len(cmds) + time.sleep(0.1) + assert len(cmds) == count diff --git a/dimos/navigation/nav_3d/mls_planner/goal_relay.py b/dimos/navigation/nav_3d/mls_planner/goal_relay.py new file mode 100644 index 0000000000..3e7588f3ca --- /dev/null +++ b/dimos/navigation/nav_3d/mls_planner/goal_relay.py @@ -0,0 +1,70 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from __future__ import annotations + +import math +import time +from typing import Any + +from reactivex.disposable import Disposable + +from dimos.core.core import rpc +from dimos.core.module import Module +from dimos.core.stream import In, Out +from dimos.msgs.geometry_msgs.PointStamped import PointStamped +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +class GoalRelay(Module): + """Pair each goal with the latest odometry into start and goal poses for the planner. + + The MLS planner plans once per goal_pose using the most recent start_pose, + so both are sent together at goal time rather than streaming odometry. + """ + + odometry: In[Odometry] + goal: In[PointStamped] + + start_pose: Out[PoseStamped] + goal_pose: Out[PoseStamped] + + def __init__(self, **kwargs: Any) -> None: + super().__init__(**kwargs) + self._latest_odom: Odometry | None = None + + @rpc + def start(self) -> None: + super().start() + self.register_disposable(Disposable(self.odometry.subscribe(self._on_odometry))) + self.register_disposable(Disposable(self.goal.subscribe(self._on_goal))) + + def _on_odometry(self, msg: Odometry) -> None: + self._latest_odom = msg + + def _on_goal(self, point: PointStamped) -> None: + if not (math.isfinite(point.x) and math.isfinite(point.y) and math.isfinite(point.z)): + return + odom = self._latest_odom + if odom is None: + logger.warning("GoalRelay received a goal before any odometry, dropping it") + return + self.start_pose.publish(odom.to_pose_stamped()) + # Let start_pose land before the goal triggers planning. + time.sleep(0.05) + self.goal_pose.publish(point.to_pose_stamped()) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 24d90dd7fe..5dfd0b3571 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -111,6 +111,7 @@ "unitree-go2-keyboard-teleop": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_keyboard_teleop:unitree_go2_keyboard_teleop", "unitree-go2-markers": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2:unitree_go2_markers", "unitree-go2-memory": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2:unitree_go2_memory", + "unitree-go2-nav-3d": "dimos.robot.unitree.go2.blueprints.navigation.unitree_go2_nav_3d:unitree_go2_nav_3d", "unitree-go2-relocalization": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2:unitree_go2_relocalization", "unitree-go2-ros": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2_ros:unitree_go2_ros", "unitree-go2-security": "dimos.robot.unitree.go2.blueprints.agentic.unitree_go2_security:unitree_go2_security", @@ -137,6 +138,7 @@ "b1-connection-module": "dimos.robot.unitree.b1.connection.B1ConnectionModule", "camera-module": "dimos.hardware.sensors.camera.module.CameraModule", "cartesian-motion-controller": "dimos.manipulation.control.servo_control.cartesian_motion_controller.CartesianMotionController", + "basic-path-follower": "dimos.navigation.basic_path_follower.module.BasicPathFollower", "click-start-goal-router": "dimos.navigation.nav_stack.modules.click_start_goal_router.click_start_goal_router.ClickStartGoalRouter", "control-coordinator": "dimos.control.coordinator.ControlCoordinator", "cost-mapper": "dimos.mapping.costmapper.CostMapper", @@ -165,6 +167,7 @@ "go2-fleet-connection": "dimos.robot.unitree.go2.fleet_connection.Go2FleetConnection", "go2-memory": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2.Go2Memory", "go2-teleop-module": "dimos.teleop.quest.quest_extensions.Go2TeleopModule", + "goal-relay": "dimos.navigation.nav_3d.mls_planner.goal_relay.GoalRelay", "google-maps-skill-container": "dimos.agents.skills.google_maps_skill_container.GoogleMapsSkillContainer", "gps-nav-skill-container": "dimos.agents.skills.gps_nav_skill.GpsNavSkillContainer", "grasping-module": "dimos.manipulation.grasping.grasping.GraspingModule", diff --git a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py new file mode 100644 index 0000000000..19cd7cca4a --- /dev/null +++ b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py @@ -0,0 +1,62 @@ +#!/usr/bin/env python3 +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Go2 3D nav stack: fastlio odometry, raytraced voxel map, MLS planning, path following. + +Expects a mid360 + fastlio running against a lidar mounted on the robot. +The go2's built-in lidar and odometry are remapped aside and unused for +navigation. The map, paths, and robot pose all live in fastlio's world frame. +""" + +import os + +from dimos.core.coordination.blueprints import autoconnect +from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 +from dimos.mapping.ray_tracing.module import RayTracingVoxelMap +from dimos.navigation.basic_path_follower.module import BasicPathFollower +from dimos.navigation.movement_manager.movement_manager import MovementManager +from dimos.navigation.nav_3d.mls_planner.goal_relay import GoalRelay +from dimos.navigation.nav_3d.mls_planner.mls_planner_native import MLSPlannerNative +from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic import unitree_go2_basic +from dimos.robot.unitree.go2.connection import GO2Connection + +voxel_size = 0.05 +# Height of the head-mounted lidar above the ground while standing. +go2_lidar_height = 0.5 + +unitree_go2_nav_3d = autoconnect( + unitree_go2_basic.remappings( + [ + (GO2Connection, "lidar", "lidar_l1"), + (GO2Connection, "odom", "odom_go2"), + ] + ), + FastLio2.blueprint( + host_ip=os.getenv("LIDAR_HOST_IP", "192.168.1.5"), + lidar_ip=os.getenv("LIDAR_IP", "192.168.1.155"), + map_freq=-1.0, + ).remappings([(FastLio2, "global_map", "global_map_fastlio")]), + RayTracingVoxelMap.blueprint(voxel_size=voxel_size), + MLSPlannerNative.blueprint( + world_frame="odom", + voxel_size=voxel_size, + robot_height=go2_lidar_height, + ), + GoalRelay.blueprint(), + BasicPathFollower.blueprint(), + MovementManager.blueprint(), +).global_config(n_workers=10, robot_model="unitree_go2") + +__all__ = ["unitree_go2_nav_3d"] From bf44130e8799bb7375c7e12fc745cbf593296df8 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Thu, 11 Jun 2026 15:41:37 -0700 Subject: [PATCH 20/56] Config --- .../go2/blueprints/navigation/unitree_go2_nav_3d.py | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py index 19cd7cca4a..4ea115e0b6 100644 --- a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py +++ b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py @@ -23,21 +23,30 @@ import os from dimos.core.coordination.blueprints import autoconnect +from dimos.core.global_config import global_config from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 from dimos.mapping.ray_tracing.module import RayTracingVoxelMap from dimos.navigation.basic_path_follower.module import BasicPathFollower from dimos.navigation.movement_manager.movement_manager import MovementManager from dimos.navigation.nav_3d.mls_planner.goal_relay import GoalRelay from dimos.navigation.nav_3d.mls_planner.mls_planner_native import MLSPlannerNative -from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic import unitree_go2_basic +from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic import rerun_config from dimos.robot.unitree.go2.connection import GO2Connection +from dimos.visualization.vis_module import vis_module voxel_size = 0.05 # Height of the head-mounted lidar above the ground while standing. go2_lidar_height = 0.5 +camera_hz = 1.0 + +_nav_rerun_config = { + **rerun_config, + "max_hz": {**rerun_config["max_hz"], "world/color_image": camera_hz}, +} unitree_go2_nav_3d = autoconnect( - unitree_go2_basic.remappings( + vis_module(viewer_backend=global_config.viewer, rerun_config=_nav_rerun_config), + GO2Connection.blueprint().remappings( [ (GO2Connection, "lidar", "lidar_l1"), (GO2Connection, "odom", "odom_go2"), From 3705e193a2d6d2ef43700f71d802940bf359d78f Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Thu, 11 Jun 2026 16:02:09 -0700 Subject: [PATCH 21/56] Transform --- .../navigation/unitree_go2_nav_3d.py | 20 ++++++++++++++++++- 1 file changed, 19 insertions(+), 1 deletion(-) diff --git a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py index 4ea115e0b6..ffbabe3365 100644 --- a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py +++ b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py @@ -21,6 +21,7 @@ """ import os +from typing import Any from dimos.core.coordination.blueprints import autoconnect from dimos.core.global_config import global_config @@ -34,14 +35,31 @@ from dimos.robot.unitree.go2.connection import GO2Connection from dimos.visualization.vis_module import vis_module -voxel_size = 0.05 +voxel_size = 0.1 # Height of the head-mounted lidar above the ground while standing. go2_lidar_height = 0.5 camera_hz = 1.0 + +def _static_robot_body(rr: Any) -> list[Any]: + """Go2-shaped box on fastlio's body frame, counter-rotated for the lidar pitch.""" + return [ + rr.Boxes3D(half_sizes=[0.35, 0.155, 0.2], colors=[(0, 255, 127)]), + rr.Transform3D( + parent_frame="tf#/body", + rotation=rr.RotationAxisAngle(axis=(0, 1, 0), degrees=-45.0), + ), + ] + + _nav_rerun_config = { **rerun_config, "max_hz": {**rerun_config["max_hz"], "world/color_image": camera_hz}, + # base_link tf comes from the go2 internal odometry, which is not the map + # frame. Anchor the robot box to fastlio's body frame instead and hide the + # camera frustum that rides base_link. + "static": {"world/tf/body": _static_robot_body}, + "visual_override": {**rerun_config["visual_override"], "world/camera_info": None}, } unitree_go2_nav_3d = autoconnect( From 510cafebff9b32bded73f4d4fa75c3549e8e4df5 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Thu, 11 Jun 2026 16:11:58 -0700 Subject: [PATCH 22/56] Increase search radius --- .../go2/blueprints/navigation/unitree_go2_nav_3d.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py index ffbabe3365..78da504e03 100644 --- a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py +++ b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py @@ -59,7 +59,12 @@ def _static_robot_body(rr: Any) -> list[Any]: # frame. Anchor the robot box to fastlio's body frame instead and hide the # camera frustum that rides base_link. "static": {"world/tf/body": _static_robot_body}, - "visual_override": {**rerun_config["visual_override"], "world/camera_info": None}, + "visual_override": { + **rerun_config["visual_override"], + "world/camera_info": None, + # Default LineSegments3D rendering lifts edges 1.7m off the surface. + "world/node_edges": lambda msg: msg.to_rerun(z_offset=0.05), + }, } unitree_go2_nav_3d = autoconnect( From c9872e8d7abebea0df91000e26366401fa98eec7 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Thu, 11 Jun 2026 16:39:17 -0700 Subject: [PATCH 23/56] Fix --- .../go2/blueprints/navigation/unitree_go2_nav_3d.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py index 78da504e03..ccc027e1e9 100644 --- a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py +++ b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py @@ -41,6 +41,11 @@ camera_hz = 1.0 +def _render_node_edges_flat(msg: Any) -> Any: + """Default LineSegments3D rendering lifts edges 1.7m off the surface.""" + return msg.to_rerun(z_offset=0.05) + + def _static_robot_body(rr: Any) -> list[Any]: """Go2-shaped box on fastlio's body frame, counter-rotated for the lidar pitch.""" return [ @@ -62,8 +67,7 @@ def _static_robot_body(rr: Any) -> list[Any]: "visual_override": { **rerun_config["visual_override"], "world/camera_info": None, - # Default LineSegments3D rendering lifts edges 1.7m off the surface. - "world/node_edges": lambda msg: msg.to_rerun(z_offset=0.05), + "world/node_edges": _render_node_edges_flat, }, } From 0d83ed6d91d6fe341c548198c53bf2fe7d07685f Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Thu, 11 Jun 2026 17:20:27 -0700 Subject: [PATCH 24/56] Tuning --- .../go2/blueprints/navigation/unitree_go2_nav_3d.py | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py index ccc027e1e9..e40611d502 100644 --- a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py +++ b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py @@ -38,7 +38,6 @@ voxel_size = 0.1 # Height of the head-mounted lidar above the ground while standing. go2_lidar_height = 0.5 -camera_hz = 1.0 def _render_node_edges_flat(msg: Any) -> Any: @@ -59,7 +58,11 @@ def _static_robot_body(rr: Any) -> list[Any]: _nav_rerun_config = { **rerun_config, - "max_hz": {**rerun_config["max_hz"], "world/color_image": camera_hz}, + "max_hz": { + **rerun_config["max_hz"], + "world/global_map": 1.0, + "world/local_map": 1.0, + }, # base_link tf comes from the go2 internal odometry, which is not the map # frame. Anchor the robot box to fastlio's body frame instead and hide the # camera frustum that rides base_link. @@ -67,6 +70,8 @@ def _static_robot_body(rr: Any) -> list[Any]: "visual_override": { **rerun_config["visual_override"], "world/camera_info": None, + "world/color_image": None, + "world/lidar": None, "world/node_edges": _render_node_edges_flat, }, } @@ -91,7 +96,7 @@ def _static_robot_body(rr: Any) -> list[Any]: robot_height=go2_lidar_height, ), GoalRelay.blueprint(), - BasicPathFollower.blueprint(), + BasicPathFollower.blueprint(lookahead_m=1.2, heading_gain=0.8, max_angular=0.6), MovementManager.blueprint(), ).global_config(n_workers=10, robot_model="unitree_go2") From b065e8c00aa3038a63f7fa0c176fea7bcf42cdfc Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Thu, 11 Jun 2026 17:26:30 -0700 Subject: [PATCH 25/56] Fixes --- .../go2/blueprints/navigation/unitree_go2_nav_3d.py | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py index e40611d502..9a3d9e90e8 100644 --- a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py +++ b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py @@ -40,11 +40,6 @@ go2_lidar_height = 0.5 -def _render_node_edges_flat(msg: Any) -> Any: - """Default LineSegments3D rendering lifts edges 1.7m off the surface.""" - return msg.to_rerun(z_offset=0.05) - - def _static_robot_body(rr: Any) -> list[Any]: """Go2-shaped box on fastlio's body frame, counter-rotated for the lidar pitch.""" return [ @@ -63,6 +58,7 @@ def _static_robot_body(rr: Any) -> list[Any]: "world/global_map": 1.0, "world/local_map": 1.0, }, + "memory_limit": "1GB", # base_link tf comes from the go2 internal odometry, which is not the map # frame. Anchor the robot box to fastlio's body frame instead and hide the # camera frustum that rides base_link. @@ -72,7 +68,9 @@ def _static_robot_body(rr: Any) -> list[Any]: "world/camera_info": None, "world/color_image": None, "world/lidar": None, - "world/node_edges": _render_node_edges_flat, + "world/surface_map": None, + "world/nodes": None, + "world/node_edges": None, }, } From f48921428b953f809a8bf80c4490ef4f59e2249d Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Thu, 11 Jun 2026 17:54:33 -0700 Subject: [PATCH 26/56] Incremental builds --- .../unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py index 9a3d9e90e8..60243900db 100644 --- a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py +++ b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py @@ -87,12 +87,14 @@ def _static_robot_body(rr: Any) -> list[Any]: lidar_ip=os.getenv("LIDAR_IP", "192.168.1.155"), map_freq=-1.0, ).remappings([(FastLio2, "global_map", "global_map_fastlio")]), - RayTracingVoxelMap.blueprint(voxel_size=voxel_size), + RayTracingVoxelMap.blueprint(voxel_size=voxel_size, emit_every=10), + # global_map is remapped off so the planner runs purely on the + # incremental local_map + region_bounds pair. MLSPlannerNative.blueprint( world_frame="odom", voxel_size=voxel_size, robot_height=go2_lidar_height, - ), + ).remappings([(MLSPlannerNative, "global_map", "global_map_unused")]), GoalRelay.blueprint(), BasicPathFollower.blueprint(lookahead_m=1.2, heading_gain=0.8, max_angular=0.6), MovementManager.blueprint(), From 748fd4aebe927cec41b4e538bdf7857650e8501e Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Thu, 11 Jun 2026 18:40:48 -0700 Subject: [PATCH 27/56] Correct unintuitive code --- .../unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py index 60243900db..84eea5293c 100644 --- a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py +++ b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py @@ -40,6 +40,11 @@ go2_lidar_height = 0.5 +def _render_global_map(msg: Any) -> Any: + # don't use a min z height because why would we want that + return msg.to_rerun() + + def _static_robot_body(rr: Any) -> list[Any]: """Go2-shaped box on fastlio's body frame, counter-rotated for the lidar pitch.""" return [ @@ -65,6 +70,7 @@ def _static_robot_body(rr: Any) -> list[Any]: "static": {"world/tf/body": _static_robot_body}, "visual_override": { **rerun_config["visual_override"], + "world/global_map": _render_global_map, "world/camera_info": None, "world/color_image": None, "world/lidar": None, From 62a4f6a9f5b118ab2c672f22a03ae44bdb4247f2 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Fri, 12 Jun 2026 17:21:48 -0700 Subject: [PATCH 28/56] Fix registry order and rerun config typing --- dimos/robot/all_blueprints.py | 2 +- dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 5dfd0b3571..6f969ca951 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -136,9 +136,9 @@ "arm-teleop-module": "dimos.teleop.quest.quest_extensions.ArmTeleopModule", "b-box-navigation-module": "dimos.navigation.bbox_navigation.BBoxNavigationModule", "b1-connection-module": "dimos.robot.unitree.b1.connection.B1ConnectionModule", + "basic-path-follower": "dimos.navigation.basic_path_follower.module.BasicPathFollower", "camera-module": "dimos.hardware.sensors.camera.module.CameraModule", "cartesian-motion-controller": "dimos.manipulation.control.servo_control.cartesian_motion_controller.CartesianMotionController", - "basic-path-follower": "dimos.navigation.basic_path_follower.module.BasicPathFollower", "click-start-goal-router": "dimos.navigation.nav_stack.modules.click_start_goal_router.click_start_goal_router.ClickStartGoalRouter", "control-coordinator": "dimos.control.coordinator.ControlCoordinator", "cost-mapper": "dimos.mapping.costmapper.CostMapper", diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py index 928b2dc23a..2fb7d09094 100644 --- a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py @@ -87,7 +87,7 @@ def _go2_rerun_blueprint() -> Any: ) -rerun_config = { +rerun_config: dict[str, Any] = { "blueprint": _go2_rerun_blueprint, # Custom converters for specific rerun entity paths # Normally all these would be specified in their respectative modules From 5bc1999a02d004e1e8bfd4cd6b741d9fe33f68fd Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Fri, 12 Jun 2026 17:49:55 -0700 Subject: [PATCH 29/56] Remove tests for code that will be deleted --- .../basic_path_follower/test_module.py | 121 ------------------ 1 file changed, 121 deletions(-) delete mode 100644 dimos/navigation/basic_path_follower/test_module.py diff --git a/dimos/navigation/basic_path_follower/test_module.py b/dimos/navigation/basic_path_follower/test_module.py deleted file mode 100644 index 4616660bb3..0000000000 --- a/dimos/navigation/basic_path_follower/test_module.py +++ /dev/null @@ -1,121 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from __future__ import annotations - -from collections.abc import Generator -import time - -from dimos_lcm.std_msgs import Bool # type: ignore[import-untyped] -import pytest - -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.msgs.nav_msgs.Path import Path -from dimos.navigation.basic_path_follower.module import BasicPathFollower - - -def _odom(x: float = 0.0, y: float = 0.0, yaw_quat: tuple = (0, 0, 0, 1)) -> Odometry: - qx, qy, qz, qw = yaw_quat - return Odometry(ts=time.time(), frame_id="odom", pose=Pose(x, y, 0.0, qx, qy, qz, qw)) - - -def _path(*xy: tuple) -> Path: - poses = [PoseStamped(ts=time.time(), frame_id="odom", position=(x, y, 0.0)) for x, y in xy] - return Path(ts=time.time(), frame_id="odom", poses=poses) - - -def _wait_for(predicate, timeout: float = 1.0) -> bool: - deadline = time.time() + timeout - while time.time() < deadline: - if predicate(): - return True - time.sleep(0.01) - return False - - -@pytest.fixture() -def follower() -> Generator[tuple[BasicPathFollower, list, list], None, None]: - module = BasicPathFollower(control_frequency=100.0) - cmds: list = [] - reached: list = [] - unsubs = [ - module.nav_cmd_vel.subscribe(cmds.append), - module.goal_reached.subscribe(reached.append), - ] - try: - yield module, cmds, reached - finally: - module._cancel() - for unsub in unsubs: - unsub() - module._close_module() - - -def test_drives_forward_along_path(follower): - module, cmds, _ = follower - module._on_odometry(_odom(0.0, 0.0)) - module._on_path(_path((1.0, 0.0), (2.0, 0.0), (3.0, 0.0))) - - assert _wait_for(lambda: any(c.linear.x > 0 for c in cmds)) - moving = [c for c in cmds if c.linear.x > 0] - assert abs(moving[0].angular.z) < 0.1 - - -def test_rotates_in_place_when_facing_away(follower): - module, cmds, _ = follower - module._on_odometry(_odom(0.0, 0.0, yaw_quat=(0, 0, 1, 0))) - module._on_path(_path((1.0, 0.0), (2.0, 0.0))) - - assert _wait_for(lambda: len(cmds) > 0 and cmds[-1].linear.x == 0) - assert abs(cmds[-1].angular.z) > 0 - - -def test_goal_reached_when_near_last_waypoint(follower): - module, cmds, reached = follower - module._on_odometry(_odom(2.9, 0.0)) - module._on_path(_path((1.0, 0.0), (2.0, 0.0), (3.0, 0.0))) - - assert _wait_for(lambda: len(reached) == 1) - assert reached[0].data - assert cmds[-1].linear.x == 0 - assert cmds[-1].angular.z == 0 - - -def test_stop_movement_cancels(follower): - module, cmds, _ = follower - module._on_odometry(_odom(0.0, 0.0)) - module._on_path(_path((1.0, 0.0), (2.0, 0.0))) - assert _wait_for(lambda: any(c.linear.x > 0 for c in cmds)) - - module._on_stop(Bool(True)) - time.sleep(0.05) - count = len(cmds) - time.sleep(0.1) - assert len(cmds) == count - assert cmds[-1].linear.x == 0 - - -def test_empty_path_cancels(follower): - module, cmds, _ = follower - module._on_odometry(_odom(0.0, 0.0)) - module._on_path(_path((1.0, 0.0), (2.0, 0.0))) - assert _wait_for(lambda: any(c.linear.x > 0 for c in cmds)) - - module._on_path(_path()) - time.sleep(0.05) - count = len(cmds) - time.sleep(0.1) - assert len(cmds) == count From ac06ea4bfcfeaa998a48a8ae658dbca9df49ff64 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Sat, 13 Jun 2026 13:53:52 -0700 Subject: [PATCH 30/56] Toggle go2 lidar and camera plus config --- .../navigation/unitree_go2_nav_3d.py | 4 ++-- dimos/robot/unitree/go2/connection.py | 18 +++++++++++------- 2 files changed, 13 insertions(+), 9 deletions(-) diff --git a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py index 84eea5293c..63c3dc712e 100644 --- a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py +++ b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py @@ -82,7 +82,7 @@ def _static_robot_body(rr: Any) -> list[Any]: unitree_go2_nav_3d = autoconnect( vis_module(viewer_backend=global_config.viewer, rerun_config=_nav_rerun_config), - GO2Connection.blueprint().remappings( + GO2Connection.blueprint(lidar=False, camera=False).remappings( [ (GO2Connection, "lidar", "lidar_l1"), (GO2Connection, "odom", "odom_go2"), @@ -93,7 +93,7 @@ def _static_robot_body(rr: Any) -> list[Any]: lidar_ip=os.getenv("LIDAR_IP", "192.168.1.155"), map_freq=-1.0, ).remappings([(FastLio2, "global_map", "global_map_fastlio")]), - RayTracingVoxelMap.blueprint(voxel_size=voxel_size, emit_every=10), + RayTracingVoxelMap.blueprint(voxel_size=voxel_size, emit_every=1, global_emit_every=300), # global_map is remapped off so the planner runs purely on the # incremental local_map + region_bounds pair. MLSPlannerNative.blueprint( diff --git a/dimos/robot/unitree/go2/connection.py b/dimos/robot/unitree/go2/connection.py index 5568a473ef..54f84cb3b3 100644 --- a/dimos/robot/unitree/go2/connection.py +++ b/dimos/robot/unitree/go2/connection.py @@ -67,6 +67,8 @@ class Go2Mode(str, Enum): class ConnectionConfig(ModuleConfig): ip: str = Field(default_factory=lambda m: m["g"].robot_ip) mode: Go2Mode = Go2Mode.DEFAULT + lidar: bool = True + camera: bool = True class Go2ConnectionProtocol(Protocol): @@ -241,16 +243,18 @@ def onimage(image: Image) -> None: self.color_image.publish(image) self._latest_video_frame = image - self.register_disposable(self.connection.lidar_stream().subscribe(self.lidar.publish)) + if self.config.lidar: + self.register_disposable(self.connection.lidar_stream().subscribe(self.lidar.publish)) self.register_disposable(self.connection.odom_stream().subscribe(self._publish_tf)) - self.register_disposable(self.connection.video_stream().subscribe(onimage)) self.register_disposable(Disposable(self.cmd_vel.subscribe(self.move))) - self._camera_info_thread = Thread( - target=self.publish_camera_info, - daemon=True, - ) - self._camera_info_thread.start() + if self.config.camera: + self.register_disposable(self.connection.video_stream().subscribe(onimage)) + self._camera_info_thread = Thread( + target=self.publish_camera_info, + daemon=True, + ) + self._camera_info_thread.start() self.standup() time.sleep(3) From 2c94cd48341d02dad6d391a4db27b69aa233dde8 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Sat, 13 Jun 2026 14:25:53 -0700 Subject: [PATCH 31/56] Better follower --- .../navigation/basic_path_follower/module.py | 41 ++++++++++++++++--- .../navigation/unitree_go2_nav_3d.py | 4 +- 2 files changed, 37 insertions(+), 8 deletions(-) diff --git a/dimos/navigation/basic_path_follower/module.py b/dimos/navigation/basic_path_follower/module.py index 399fccf002..1e7034dd43 100644 --- a/dimos/navigation/basic_path_follower/module.py +++ b/dimos/navigation/basic_path_follower/module.py @@ -151,10 +151,39 @@ def _follow(self, waypoints: np.ndarray, stop_event: Event) -> None: stop_event.wait(max(0.0, period - elapsed)) def _lookahead_point(self, waypoints: np.ndarray, position: np.ndarray) -> np.ndarray: - closest = int(np.argmin(np.linalg.norm(waypoints - position, axis=1))) - travelled = 0.0 - for i in range(closest + 1, len(waypoints)): - travelled += float(np.linalg.norm(waypoints[i] - waypoints[i - 1])) - if travelled >= self.config.lookahead_m: - return np.asarray(waypoints[i]) + if len(waypoints) == 1: + return np.asarray(waypoints[0]) + + # Project onto the path, then march lookahead_m along it and interpolate. + # Returning an actual waypoint would let the target jump to a distant + # vertex and cut the corner. + seg_idx, start = self._project_onto_path(waypoints, position) + remaining = self.config.lookahead_m + for i in range(seg_idx, len(waypoints) - 1): + end = waypoints[i + 1] + seg = end - start + seg_len = float(np.linalg.norm(seg)) + if seg_len >= remaining: + return np.asarray(start + (remaining / seg_len) * seg) + remaining -= seg_len + start = end return np.asarray(waypoints[-1]) + + def _project_onto_path( + self, waypoints: np.ndarray, position: np.ndarray + ) -> tuple[int, np.ndarray]: + best_idx = 0 + best_point = np.asarray(waypoints[0]) + best_dist = math.inf + for i in range(len(waypoints) - 1): + a = waypoints[i] + ab = waypoints[i + 1] - a + denom = float(np.dot(ab, ab)) + t = 0.0 if denom == 0 else float(np.clip(np.dot(position - a, ab) / denom, 0.0, 1.0)) + proj = a + t * ab + dist = float(np.linalg.norm(position - proj)) + if dist < best_dist: + best_dist = dist + best_idx = i + best_point = proj + return best_idx, best_point diff --git a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py index 63c3dc712e..9462fb08b6 100644 --- a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py +++ b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py @@ -61,7 +61,7 @@ def _static_robot_body(rr: Any) -> list[Any]: "max_hz": { **rerun_config["max_hz"], "world/global_map": 1.0, - "world/local_map": 1.0, + "world/local_map": 0, }, "memory_limit": "1GB", # base_link tf comes from the go2 internal odometry, which is not the map @@ -102,7 +102,7 @@ def _static_robot_body(rr: Any) -> list[Any]: robot_height=go2_lidar_height, ).remappings([(MLSPlannerNative, "global_map", "global_map_unused")]), GoalRelay.blueprint(), - BasicPathFollower.blueprint(lookahead_m=1.2, heading_gain=0.8, max_angular=0.6), + BasicPathFollower.blueprint(lookahead_m=0.5, heading_gain=0.8, max_angular=0.6), MovementManager.blueprint(), ).global_config(n_workers=10, robot_model="unitree_go2") From 273ffa579b6331349493749c2ccc842bb262678b Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Sat, 13 Jun 2026 14:39:47 -0700 Subject: [PATCH 32/56] Replan --- .../navigation/basic_path_follower/module.py | 94 +++++++++---------- .../nav_3d/mls_planner/goal_relay.py | 72 +++++++++++--- .../navigation/unitree_go2_nav_3d.py | 4 +- 3 files changed, 105 insertions(+), 65 deletions(-) diff --git a/dimos/navigation/basic_path_follower/module.py b/dimos/navigation/basic_path_follower/module.py index 1e7034dd43..f0a6ee8dd3 100644 --- a/dimos/navigation/basic_path_follower/module.py +++ b/dimos/navigation/basic_path_follower/module.py @@ -23,6 +23,7 @@ import numpy as np from reactivex.disposable import Disposable +from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out @@ -69,8 +70,9 @@ def __init__(self, **kwargs: Any) -> None: super().__init__(**kwargs) self._lock = RLock() self._current_odom: PoseStamped | None = None - self._thread: Thread | None = None + self._waypoints: np.ndarray | None = None self._stop_event = Event() + self._thread: Thread | None = None @rpc def start(self) -> None: @@ -79,10 +81,15 @@ def start(self) -> None: self.register_disposable(Disposable(self.path.subscribe(self._on_path))) if self.stop_movement.transport is not None: self.register_disposable(Disposable(self.stop_movement.subscribe(self._on_stop))) + self._thread = Thread(target=self._follow, daemon=True) + self._thread.start() @rpc def stop(self) -> None: - self._cancel() + self._stop_event.set() + if self._thread is not None: + self._thread.join(timeout=DEFAULT_THREAD_JOIN_TIMEOUT) + self.nav_cmd_vel.publish(Twist()) super().stop() def _on_odometry(self, msg: Odometry) -> None: @@ -90,65 +97,54 @@ def _on_odometry(self, msg: Odometry) -> None: self._current_odom = msg.to_pose_stamped() def _on_path(self, path: Path) -> None: - self._cancel() waypoints = np.array([[p.position.x, p.position.y] for p in path.poses]) - if len(waypoints) == 0: - return - logger.info("Following new path", waypoints=len(waypoints)) - stop_event = Event() - thread = Thread(target=self._follow, args=(waypoints, stop_event), daemon=True) with self._lock: - self._stop_event = stop_event - self._thread = thread - thread.start() + self._waypoints = waypoints if len(waypoints) else None def _on_stop(self, msg: Bool) -> None: if msg.data: - self._cancel() - - def _cancel(self) -> None: - with self._lock: - self._stop_event.set() - self._thread = None - self.nav_cmd_vel.publish(Twist()) + with self._lock: + self._waypoints = None + self.nav_cmd_vel.publish(Twist()) - def _follow(self, waypoints: np.ndarray, stop_event: Event) -> None: + def _follow(self) -> None: period = 1.0 / self.config.control_frequency - while not stop_event.is_set(): + while not self._stop_event.is_set(): start_time = time.perf_counter() - with self._lock: odom = self._current_odom + waypoints = self._waypoints + if odom is not None and waypoints is not None: + self._step(odom, waypoints) + elapsed = time.perf_counter() - start_time + self._stop_event.wait(max(0.0, period - elapsed)) - if odom is None: - stop_event.wait(period) - continue - - position = np.array([odom.position.x, odom.position.y]) - if float(np.linalg.norm(waypoints[-1] - position)) < self.config.goal_tolerance: - self.nav_cmd_vel.publish(Twist()) - self.goal_reached.publish(Bool(True)) - logger.info("Goal reached") - return - - target = self._lookahead_point(waypoints, position) - yaw_error = angle_diff( - math.atan2(target[1] - position[1], target[0] - position[0]), - odom.orientation.euler[2], - ) - - angular = max( - -self.config.max_angular, - min(self.config.max_angular, self.config.heading_gain * yaw_error), - ) - linear = 0.0 - if abs(yaw_error) <= self.config.rotate_threshold: - linear = self.config.speed * max(0.0, math.cos(yaw_error)) - - self.nav_cmd_vel.publish(Twist(Vector3(linear, 0, 0), Vector3(0, 0, angular))) + def _step(self, odom: PoseStamped, waypoints: np.ndarray) -> None: + position = np.array([odom.position.x, odom.position.y]) + if float(np.linalg.norm(waypoints[-1] - position)) < self.config.goal_tolerance: + self.nav_cmd_vel.publish(Twist()) + with self._lock: + if self._waypoints is waypoints: + self._waypoints = None + self.goal_reached.publish(Bool(True)) + logger.info("Goal reached") + return - elapsed = time.perf_counter() - start_time - stop_event.wait(max(0.0, period - elapsed)) + target = self._lookahead_point(waypoints, position) + yaw_error = angle_diff( + math.atan2(target[1] - position[1], target[0] - position[0]), + odom.orientation.euler[2], + ) + + angular = max( + -self.config.max_angular, + min(self.config.max_angular, self.config.heading_gain * yaw_error), + ) + linear = 0.0 + if abs(yaw_error) <= self.config.rotate_threshold: + linear = self.config.speed * max(0.0, math.cos(yaw_error)) + + self.nav_cmd_vel.publish(Twist(Vector3(linear, 0, 0), Vector3(0, 0, angular))) def _lookahead_point(self, waypoints: np.ndarray, position: np.ndarray) -> np.ndarray: if len(waypoints) == 1: diff --git a/dimos/navigation/nav_3d/mls_planner/goal_relay.py b/dimos/navigation/nav_3d/mls_planner/goal_relay.py index 3e7588f3ca..52aeb2816d 100644 --- a/dimos/navigation/nav_3d/mls_planner/goal_relay.py +++ b/dimos/navigation/nav_3d/mls_planner/goal_relay.py @@ -15,13 +15,16 @@ from __future__ import annotations import math +from threading import Event, RLock, Thread import time from typing import Any +from dimos_lcm.std_msgs import Bool # type: ignore[import-untyped] from reactivex.disposable import Disposable +from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT from dimos.core.core import rpc -from dimos.core.module import Module +from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out from dimos.msgs.geometry_msgs.PointStamped import PointStamped from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped @@ -31,40 +34,79 @@ logger = setup_logger() +class GoalRelayConfig(ModuleConfig): + replan_hz: float = 10.0 + + class GoalRelay(Module): - """Pair each goal with the latest odometry into start and goal poses for the planner. + """Drive the planner from the active goal and the latest odometry. - The MLS planner plans once per goal_pose using the most recent start_pose, - so both are sent together at goal time rather than streaming odometry. + The MLS planner replans once per goal_pose using the most recent + start_pose. This holds the active goal and republishes start_pose plus + goal_pose at replan_hz, so the planner keeps replanning from the robot's + current pose while it follows the path. Replanning stops on goal_reached + or a non-finite (cancel) goal. """ + config: GoalRelayConfig + odometry: In[Odometry] goal: In[PointStamped] + goal_reached: In[Bool] start_pose: Out[PoseStamped] goal_pose: Out[PoseStamped] def __init__(self, **kwargs: Any) -> None: super().__init__(**kwargs) + self._lock = RLock() self._latest_odom: Odometry | None = None + self._goal: PointStamped | None = None + self._stop_event = Event() + self._thread: Thread | None = None @rpc def start(self) -> None: super().start() self.register_disposable(Disposable(self.odometry.subscribe(self._on_odometry))) self.register_disposable(Disposable(self.goal.subscribe(self._on_goal))) + if self.goal_reached.transport is not None: + self.register_disposable(Disposable(self.goal_reached.subscribe(self._on_goal_reached))) + self._thread = Thread(target=self._replan, daemon=True) + self._thread.start() + + @rpc + def stop(self) -> None: + self._stop_event.set() + if self._thread is not None: + self._thread.join(timeout=DEFAULT_THREAD_JOIN_TIMEOUT) + super().stop() def _on_odometry(self, msg: Odometry) -> None: - self._latest_odom = msg + with self._lock: + self._latest_odom = msg def _on_goal(self, point: PointStamped) -> None: - if not (math.isfinite(point.x) and math.isfinite(point.y) and math.isfinite(point.z)): - return - odom = self._latest_odom - if odom is None: - logger.warning("GoalRelay received a goal before any odometry, dropping it") - return - self.start_pose.publish(odom.to_pose_stamped()) - # Let start_pose land before the goal triggers planning. - time.sleep(0.05) - self.goal_pose.publish(point.to_pose_stamped()) + finite = math.isfinite(point.x) and math.isfinite(point.y) and math.isfinite(point.z) + with self._lock: + self._goal = point if finite else None + + def _on_goal_reached(self, msg: Bool) -> None: + if msg.data: + with self._lock: + self._goal = None + + def _replan(self) -> None: + period = 1.0 / self.config.replan_hz + while not self._stop_event.is_set(): + start_time = time.perf_counter() + with self._lock: + goal = self._goal + odom = self._latest_odom + if goal is not None and odom is not None: + self.start_pose.publish(odom.to_pose_stamped()) + # Let start_pose land before the goal triggers planning. + self._stop_event.wait(0.05) + self.goal_pose.publish(goal.to_pose_stamped()) + elapsed = time.perf_counter() - start_time + self._stop_event.wait(max(0.0, period - elapsed)) diff --git a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py index 9462fb08b6..b8e509f6e6 100644 --- a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py +++ b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py @@ -93,7 +93,9 @@ def _static_robot_body(rr: Any) -> list[Any]: lidar_ip=os.getenv("LIDAR_IP", "192.168.1.155"), map_freq=-1.0, ).remappings([(FastLio2, "global_map", "global_map_fastlio")]), - RayTracingVoxelMap.blueprint(voxel_size=voxel_size, emit_every=1, global_emit_every=300), + RayTracingVoxelMap.blueprint( + voxel_size=voxel_size, emit_every=1, global_emit_every=300, max_health=5 + ), # global_map is remapped off so the planner runs purely on the # incremental local_map + region_bounds pair. MLSPlannerNative.blueprint( From 3af48f665888029d014cf935c217d4f7cab9f98c Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Sat, 13 Jun 2026 14:47:37 -0700 Subject: [PATCH 33/56] Fix --- dimos/navigation/basic_path_follower/module.py | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/dimos/navigation/basic_path_follower/module.py b/dimos/navigation/basic_path_follower/module.py index f0a6ee8dd3..fe3896b1c3 100644 --- a/dimos/navigation/basic_path_follower/module.py +++ b/dimos/navigation/basic_path_follower/module.py @@ -54,7 +54,8 @@ class BasicPathFollower(Module): Consumes world-frame paths and odometry. Publishes nav_cmd_vel until the last waypoint is within goal tolerance, then publishes goal_reached. - An empty path or a stop_movement message cancels the current path. + A stop_movement message cancels the current path. Empty paths are ignored + so continuous replanning does not stutter the follower. """ config: BasicPathFollowerConfig @@ -97,9 +98,14 @@ def _on_odometry(self, msg: Odometry) -> None: self._current_odom = msg.to_pose_stamped() def _on_path(self, path: Path) -> None: + # The planner clears its plan with an empty path on every start-pose + # change and on a failed replan. Under continuous replanning that is not + # a stop, so keep the last path. Stops arrive via stop_movement. + if len(path.poses) == 0: + return waypoints = np.array([[p.position.x, p.position.y] for p in path.poses]) with self._lock: - self._waypoints = waypoints if len(waypoints) else None + self._waypoints = waypoints def _on_stop(self, msg: Bool) -> None: if msg.data: From f9271eb7981ce09d7f0b41af8f7f934cb9734368 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Mon, 15 Jun 2026 11:35:36 -0700 Subject: [PATCH 34/56] Fix --- .../blueprints/navigation/unitree_go2_nav_3d.py | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py index b8e509f6e6..2f32da9ec0 100644 --- a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py +++ b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py @@ -45,6 +45,14 @@ def _render_global_map(msg: Any) -> Any: return msg.to_rerun() +def _render_path(msg: Any) -> Any: + # The planner clears its plan with an empty path on every start-pose change. + # Logging those would blank the line. Drop them so the last path stays shown. + if len(msg.poses) == 0: + return None + return msg + + def _static_robot_body(rr: Any) -> list[Any]: """Go2-shaped box on fastlio's body frame, counter-rotated for the lidar pitch.""" return [ @@ -61,9 +69,9 @@ def _static_robot_body(rr: Any) -> list[Any]: "max_hz": { **rerun_config["max_hz"], "world/global_map": 1.0, - "world/local_map": 0, + "world/local_map": 1.0, }, - "memory_limit": "1GB", + "memory_limit": "256MB", # base_link tf comes from the go2 internal odometry, which is not the map # frame. Anchor the robot box to fastlio's body frame instead and hide the # camera frustum that rides base_link. @@ -71,6 +79,7 @@ def _static_robot_body(rr: Any) -> list[Any]: "visual_override": { **rerun_config["visual_override"], "world/global_map": _render_global_map, + "world/path": _render_path, "world/camera_info": None, "world/color_image": None, "world/lidar": None, @@ -94,7 +103,7 @@ def _static_robot_body(rr: Any) -> list[Any]: map_freq=-1.0, ).remappings([(FastLio2, "global_map", "global_map_fastlio")]), RayTracingVoxelMap.blueprint( - voxel_size=voxel_size, emit_every=1, global_emit_every=300, max_health=5 + voxel_size=voxel_size, emit_every=1, global_emit_every=1500, max_health=5 ), # global_map is remapped off so the planner runs purely on the # incremental local_map + region_bounds pair. From 0076e980602b3704bbb1e0271658ddbdee40e5bf Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Mon, 15 Jun 2026 12:04:21 -0700 Subject: [PATCH 35/56] Debug timing logging --- .../navigation/basic_path_follower/module.py | 22 +++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/dimos/navigation/basic_path_follower/module.py b/dimos/navigation/basic_path_follower/module.py index fe3896b1c3..d76a2a4e0b 100644 --- a/dimos/navigation/basic_path_follower/module.py +++ b/dimos/navigation/basic_path_follower/module.py @@ -74,6 +74,8 @@ def __init__(self, **kwargs: Any) -> None: self._waypoints: np.ndarray | None = None self._stop_event = Event() self._thread: Thread | None = None + self._path_count = 0 + self._stats_last = 0.0 @rpc def start(self) -> None: @@ -82,6 +84,7 @@ def start(self) -> None: self.register_disposable(Disposable(self.path.subscribe(self._on_path))) if self.stop_movement.transport is not None: self.register_disposable(Disposable(self.stop_movement.subscribe(self._on_stop))) + self._stats_last = time.perf_counter() self._thread = Thread(target=self._follow, daemon=True) self._thread.start() @@ -106,6 +109,7 @@ def _on_path(self, path: Path) -> None: waypoints = np.array([[p.position.x, p.position.y] for p in path.poses]) with self._lock: self._waypoints = waypoints + self._path_count += 1 def _on_stop(self, msg: Bool) -> None: if msg.data: @@ -122,9 +126,27 @@ def _follow(self) -> None: waypoints = self._waypoints if odom is not None and waypoints is not None: self._step(odom, waypoints) + self._maybe_log_stats(odom, waypoints) elapsed = time.perf_counter() - start_time self._stop_event.wait(max(0.0, period - elapsed)) + # DEBUG: replanning telemetry, remove before merge + def _maybe_log_stats(self, odom: PoseStamped | None, waypoints: np.ndarray | None) -> None: + now = time.perf_counter() + elapsed = now - self._stats_last + if elapsed < 2.0: + return + self._stats_last = now + with self._lock: + count = self._path_count + self._path_count = 0 + if odom is None or waypoints is None: + return + rate = count / elapsed + position = np.array([odom.position.x, odom.position.y]) + lag = float(np.linalg.norm(waypoints[0] - position)) + logger.info("path follower stats", replan_hz=round(rate, 1), path_lag_m=round(lag, 2)) + def _step(self, odom: PoseStamped, waypoints: np.ndarray) -> None: position = np.array([odom.position.x, odom.position.y]) if float(np.linalg.norm(waypoints[-1] - position)) < self.config.goal_tolerance: From 89ed5f2eb0f5bc13c3b7e9b1dfe8742193a66777 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Mon, 15 Jun 2026 12:50:13 -0700 Subject: [PATCH 36/56] Fix --- dimos/navigation/basic_path_follower/module.py | 8 +++++--- dimos/navigation/nav_3d/mls_planner/goal_relay.py | 13 +++++++++---- 2 files changed, 14 insertions(+), 7 deletions(-) diff --git a/dimos/navigation/basic_path_follower/module.py b/dimos/navigation/basic_path_follower/module.py index d76a2a4e0b..a4a12ca3c2 100644 --- a/dimos/navigation/basic_path_follower/module.py +++ b/dimos/navigation/basic_path_follower/module.py @@ -140,11 +140,13 @@ def _maybe_log_stats(self, odom: PoseStamped | None, waypoints: np.ndarray | Non with self._lock: count = self._path_count self._path_count = 0 - if odom is None or waypoints is None: + if count == 0: return rate = count / elapsed - position = np.array([odom.position.x, odom.position.y]) - lag = float(np.linalg.norm(waypoints[0] - position)) + lag = float("nan") + if odom is not None and waypoints is not None: + position = np.array([odom.position.x, odom.position.y]) + lag = float(np.linalg.norm(waypoints[0] - position)) logger.info("path follower stats", replan_hz=round(rate, 1), path_lag_m=round(lag, 2)) def _step(self, odom: PoseStamped, waypoints: np.ndarray) -> None: diff --git a/dimos/navigation/nav_3d/mls_planner/goal_relay.py b/dimos/navigation/nav_3d/mls_planner/goal_relay.py index 52aeb2816d..01fd7e6631 100644 --- a/dimos/navigation/nav_3d/mls_planner/goal_relay.py +++ b/dimos/navigation/nav_3d/mls_planner/goal_relay.py @@ -36,6 +36,7 @@ class GoalRelayConfig(ModuleConfig): replan_hz: float = 10.0 + goal_tolerance: float = 0.3 class GoalRelay(Module): @@ -104,9 +105,13 @@ def _replan(self) -> None: goal = self._goal odom = self._latest_odom if goal is not None and odom is not None: - self.start_pose.publish(odom.to_pose_stamped()) - # Let start_pose land before the goal triggers planning. - self._stop_event.wait(0.05) - self.goal_pose.publish(goal.to_pose_stamped()) + start = odom.to_pose_stamped() + # Stop replanning once we are at the goal, otherwise the planner + # spins and the follower loops on goal_reached forever. + if math.hypot(start.x - goal.x, start.y - goal.y) >= self.config.goal_tolerance: + self.start_pose.publish(start) + # Let start_pose land before the goal triggers planning. + self._stop_event.wait(0.05) + self.goal_pose.publish(goal.to_pose_stamped()) elapsed = time.perf_counter() - start_time self._stop_event.wait(max(0.0, period - elapsed)) From 5f564a13dccf8beed930a879aef0430285840750 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Mon, 15 Jun 2026 14:57:19 -0700 Subject: [PATCH 37/56] Downsample --- .../navigation/unitree_go2_nav_3d.py | 28 +++++++++++++++---- 1 file changed, 22 insertions(+), 6 deletions(-) diff --git a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py index 2f32da9ec0..d70a6c992b 100644 --- a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py +++ b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py @@ -27,6 +27,7 @@ from dimos.core.global_config import global_config from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 from dimos.mapping.ray_tracing.module import RayTracingVoxelMap +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.navigation.basic_path_follower.module import BasicPathFollower from dimos.navigation.movement_manager.movement_manager import MovementManager from dimos.navigation.nav_3d.mls_planner.goal_relay import GoalRelay @@ -40,9 +41,22 @@ go2_lidar_height = 0.5 -def _render_global_map(msg: Any) -> Any: - # don't use a min z height because why would we want that - return msg.to_rerun() +def _downsampled(stride: int) -> Any: + """Stride a cloud before rendering. Cuts viewer encode and stream weight. + + The full-rate cloud still goes to the planner over LCM. This only thins + what the bridge sends to the viewer. + """ + + def render(msg: Any) -> Any: + points = msg.points_f32() + if len(points) == 0: + return None + if len(points) <= stride: + return msg + return PointCloud2.from_numpy(points[::stride], frame_id=msg.frame_id) + + return render def _render_path(msg: Any) -> Any: @@ -68,8 +82,9 @@ def _static_robot_body(rr: Any) -> list[Any]: **rerun_config, "max_hz": { **rerun_config["max_hz"], - "world/global_map": 1.0, - "world/local_map": 1.0, + # pose and path are unthrottled (not listed) for live high-rate viz. + "world/local_map": 5.0, + "world/global_map": 0.5, }, "memory_limit": "256MB", # base_link tf comes from the go2 internal odometry, which is not the map @@ -78,7 +93,8 @@ def _static_robot_body(rr: Any) -> list[Any]: "static": {"world/tf/body": _static_robot_body}, "visual_override": { **rerun_config["visual_override"], - "world/global_map": _render_global_map, + "world/global_map": _downsampled(8), + "world/local_map": _downsampled(3), "world/path": _render_path, "world/camera_info": None, "world/color_image": None, From 2bf3c82e38f83d25fc92d87a356ce4a35aeb47b7 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Mon, 15 Jun 2026 15:01:31 -0700 Subject: [PATCH 38/56] Fix --- .../navigation/unitree_go2_nav_3d.py | 26 +++++++++---------- 1 file changed, 12 insertions(+), 14 deletions(-) diff --git a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py index d70a6c992b..1ace0c2d33 100644 --- a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py +++ b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py @@ -20,6 +20,7 @@ navigation. The map, paths, and robot pose all live in fastlio's world frame. """ +import functools import os from typing import Any @@ -41,22 +42,19 @@ go2_lidar_height = 0.5 -def _downsampled(stride: int) -> Any: +def _downsample(msg: Any, stride: int) -> Any: """Stride a cloud before rendering. Cuts viewer encode and stream weight. The full-rate cloud still goes to the planner over LCM. This only thins - what the bridge sends to the viewer. + what the bridge sends to the viewer. Module-level so it stays picklable + when the vis config is shipped to the worker process. """ - - def render(msg: Any) -> Any: - points = msg.points_f32() - if len(points) == 0: - return None - if len(points) <= stride: - return msg - return PointCloud2.from_numpy(points[::stride], frame_id=msg.frame_id) - - return render + points = msg.points_f32() + if len(points) == 0: + return None + if len(points) <= stride: + return msg + return PointCloud2.from_numpy(points[::stride], frame_id=msg.frame_id) def _render_path(msg: Any) -> Any: @@ -93,8 +91,8 @@ def _static_robot_body(rr: Any) -> list[Any]: "static": {"world/tf/body": _static_robot_body}, "visual_override": { **rerun_config["visual_override"], - "world/global_map": _downsampled(8), - "world/local_map": _downsampled(3), + "world/global_map": functools.partial(_downsample, stride=8), + "world/local_map": functools.partial(_downsample, stride=3), "world/path": _render_path, "world/camera_info": None, "world/color_image": None, From 92bd67bc627f0019b7fefcdf0ca3b6916bcdcd1c Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Mon, 15 Jun 2026 15:09:30 -0700 Subject: [PATCH 39/56] Remove bad code --- .../navigation/unitree_go2_nav_3d.py | 26 +++++-------------- 1 file changed, 6 insertions(+), 20 deletions(-) diff --git a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py index 1ace0c2d33..2f32da9ec0 100644 --- a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py +++ b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py @@ -20,7 +20,6 @@ navigation. The map, paths, and robot pose all live in fastlio's world frame. """ -import functools import os from typing import Any @@ -28,7 +27,6 @@ from dimos.core.global_config import global_config from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 from dimos.mapping.ray_tracing.module import RayTracingVoxelMap -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.navigation.basic_path_follower.module import BasicPathFollower from dimos.navigation.movement_manager.movement_manager import MovementManager from dimos.navigation.nav_3d.mls_planner.goal_relay import GoalRelay @@ -42,19 +40,9 @@ go2_lidar_height = 0.5 -def _downsample(msg: Any, stride: int) -> Any: - """Stride a cloud before rendering. Cuts viewer encode and stream weight. - - The full-rate cloud still goes to the planner over LCM. This only thins - what the bridge sends to the viewer. Module-level so it stays picklable - when the vis config is shipped to the worker process. - """ - points = msg.points_f32() - if len(points) == 0: - return None - if len(points) <= stride: - return msg - return PointCloud2.from_numpy(points[::stride], frame_id=msg.frame_id) +def _render_global_map(msg: Any) -> Any: + # don't use a min z height because why would we want that + return msg.to_rerun() def _render_path(msg: Any) -> Any: @@ -80,9 +68,8 @@ def _static_robot_body(rr: Any) -> list[Any]: **rerun_config, "max_hz": { **rerun_config["max_hz"], - # pose and path are unthrottled (not listed) for live high-rate viz. - "world/local_map": 5.0, - "world/global_map": 0.5, + "world/global_map": 1.0, + "world/local_map": 1.0, }, "memory_limit": "256MB", # base_link tf comes from the go2 internal odometry, which is not the map @@ -91,8 +78,7 @@ def _static_robot_body(rr: Any) -> list[Any]: "static": {"world/tf/body": _static_robot_body}, "visual_override": { **rerun_config["visual_override"], - "world/global_map": functools.partial(_downsample, stride=8), - "world/local_map": functools.partial(_downsample, stride=3), + "world/global_map": _render_global_map, "world/path": _render_path, "world/camera_info": None, "world/color_image": None, From cadcf7ca07113871421779c78220201ed3fdde95 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Mon, 15 Jun 2026 15:34:59 -0700 Subject: [PATCH 40/56] Replan time logging --- dimos/navigation/basic_path_follower/module.py | 15 ++++++++++++++- .../nav_3d/mls_planner/rust/src/main.rs | 16 ++++++++++++++-- dimos/visualization/rerun/bridge.py | 8 ++++++++ 3 files changed, 36 insertions(+), 3 deletions(-) diff --git a/dimos/navigation/basic_path_follower/module.py b/dimos/navigation/basic_path_follower/module.py index a4a12ca3c2..8915c121e7 100644 --- a/dimos/navigation/basic_path_follower/module.py +++ b/dimos/navigation/basic_path_follower/module.py @@ -76,6 +76,8 @@ def __init__(self, **kwargs: Any) -> None: self._thread: Thread | None = None self._path_count = 0 self._stats_last = 0.0 + self._last_path_t = 0.0 + self._max_gap = 0.0 @rpc def start(self) -> None: @@ -107,7 +109,11 @@ def _on_path(self, path: Path) -> None: if len(path.poses) == 0: return waypoints = np.array([[p.position.x, p.position.y] for p in path.poses]) + now = time.perf_counter() with self._lock: + if self._last_path_t: + self._max_gap = max(self._max_gap, now - self._last_path_t) + self._last_path_t = now self._waypoints = waypoints self._path_count += 1 @@ -139,7 +145,9 @@ def _maybe_log_stats(self, odom: PoseStamped | None, waypoints: np.ndarray | Non self._stats_last = now with self._lock: count = self._path_count + gap = self._max_gap self._path_count = 0 + self._max_gap = 0.0 if count == 0: return rate = count / elapsed @@ -147,7 +155,12 @@ def _maybe_log_stats(self, odom: PoseStamped | None, waypoints: np.ndarray | Non if odom is not None and waypoints is not None: position = np.array([odom.position.x, odom.position.y]) lag = float(np.linalg.norm(waypoints[0] - position)) - logger.info("path follower stats", replan_hz=round(rate, 1), path_lag_m=round(lag, 2)) + logger.debug( + "path follower stats", + replan_hz=round(rate, 1), + max_gap_ms=round(gap * 1000), + path_lag_m=round(lag, 2), + ) def _step(self, odom: PoseStamped, waypoints: np.ndarray) -> None: position = np.array([odom.position.x, odom.position.y]) diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/main.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/main.rs index 0d1bd95c20..96374e9804 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/main.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/main.rs @@ -1,7 +1,7 @@ // Copyright 2026 Dimensional Inc. // SPDX-License-Identifier: Apache-2.0 -use std::time::{Duration, SystemTime, UNIX_EPOCH}; +use std::time::{Duration, Instant, SystemTime, UNIX_EPOCH}; use dimos_mls_planner::edges::{edges_to_segments, PlannerGraph}; use dimos_mls_planner::mls_planner::{Config, Planner, RegionBounds}; @@ -51,6 +51,7 @@ struct MlsPlanner { latest_start: Option<(f32, f32, f32)>, pending_local: Option, pending_bounds: Option, + last_path_at: Option, } impl MlsPlanner { @@ -174,6 +175,7 @@ impl MlsPlanner { let p = &msg.pose.position; let goal = (p.x as f32, p.y as f32, p.z as f32); + let plan_start = Instant::now(); let waypoints = match self.planner.plan(start, goal, &self.config) { Some(wp) => wp, None => { @@ -182,9 +184,19 @@ impl MlsPlanner { return; } }; + let plan_ms = plan_start.elapsed().as_secs_f64() * 1e3; + let produced = Instant::now(); + let since_last_ms = self + .last_path_at + .map_or(-1.0, |t| (produced - t).as_secs_f64() * 1e3); + self.last_path_at = Some(produced); + let stamp = now(); let path_msg = build_path_from_waypoints(&waypoints, &self.config.world_frame, stamp); - info!(waypoints = waypoints.len(), "path planned"); + debug!( + waypoints = waypoints.len(), + plan_ms, since_last_ms, "path planned" + ); publish_path(&self.path, &path_msg).await; } } diff --git a/dimos/visualization/rerun/bridge.py b/dimos/visualization/rerun/bridge.py index 2f5fb1efa9..2d43767c94 100644 --- a/dimos/visualization/rerun/bridge.py +++ b/dimos/visualization/rerun/bridge.py @@ -289,6 +289,14 @@ def _on_message(self, msg: Any, topic: Any) -> None: if not rerun_data: return + # Place data on the timeline by capture time, not by when the bridge got + # to it. Otherwise heavy messages (clouds) log late and desync from light + # ones (path, pose) captured at the same instant. View on the "capture" + # timeline for synchronized playback. + ts = getattr(msg, "ts", None) + if ts is not None: + rr.set_time("capture", timestamp=ts) + # TFMessage for example returns list of (entity_path, archetype) tuples if is_rerun_multi(rerun_data): for path, archetype in rerun_data: From 773a8106e9e7082fcd4050dee161c8d1edc5ac68 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Mon, 15 Jun 2026 16:24:37 -0700 Subject: [PATCH 41/56] Replan on map updates --- .../nav_3d/mls_planner/goal_relay.py | 75 ++----------------- .../nav_3d/mls_planner/mls_planner_native.py | 1 + .../nav_3d/mls_planner/rust/src/main.rs | 54 +++++++++---- .../mls_planner/rust/src/mls_planner.rs | 4 + .../nav_3d/mls_planner/rust/src/planner.rs | 1 + .../nav_3d/mls_planner/rust/src/python.rs | 3 + 6 files changed, 58 insertions(+), 80 deletions(-) diff --git a/dimos/navigation/nav_3d/mls_planner/goal_relay.py b/dimos/navigation/nav_3d/mls_planner/goal_relay.py index 01fd7e6631..5731e1abfe 100644 --- a/dimos/navigation/nav_3d/mls_planner/goal_relay.py +++ b/dimos/navigation/nav_3d/mls_planner/goal_relay.py @@ -14,104 +14,45 @@ from __future__ import annotations -import math -from threading import Event, RLock, Thread -import time -from typing import Any - -from dimos_lcm.std_msgs import Bool # type: ignore[import-untyped] from reactivex.disposable import Disposable -from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out from dimos.msgs.geometry_msgs.PointStamped import PointStamped from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.utils.logging_config import setup_logger - -logger = setup_logger() class GoalRelayConfig(ModuleConfig): - replan_hz: float = 10.0 - goal_tolerance: float = 0.3 + pass class GoalRelay(Module): - """Drive the planner from the active goal and the latest odometry. + """Adapt odometry and goal points to the planner's PoseStamped inputs. - The MLS planner replans once per goal_pose using the most recent - start_pose. This holds the active goal and republishes start_pose plus - goal_pose at replan_hz, so the planner keeps replanning from the robot's - current pose while it follows the path. Replanning stops on goal_reached - or a non-finite (cancel) goal. + The MLS planner consumes start and goal as PoseStamped and replans itself + on map updates. This stateless adapter just relays: each odometry becomes a + start_pose, each goal becomes a goal_pose. A non-finite goal is relayed + through so the planner can clear its active goal. """ config: GoalRelayConfig odometry: In[Odometry] goal: In[PointStamped] - goal_reached: In[Bool] start_pose: Out[PoseStamped] goal_pose: Out[PoseStamped] - def __init__(self, **kwargs: Any) -> None: - super().__init__(**kwargs) - self._lock = RLock() - self._latest_odom: Odometry | None = None - self._goal: PointStamped | None = None - self._stop_event = Event() - self._thread: Thread | None = None - @rpc def start(self) -> None: super().start() self.register_disposable(Disposable(self.odometry.subscribe(self._on_odometry))) self.register_disposable(Disposable(self.goal.subscribe(self._on_goal))) - if self.goal_reached.transport is not None: - self.register_disposable(Disposable(self.goal_reached.subscribe(self._on_goal_reached))) - self._thread = Thread(target=self._replan, daemon=True) - self._thread.start() - - @rpc - def stop(self) -> None: - self._stop_event.set() - if self._thread is not None: - self._thread.join(timeout=DEFAULT_THREAD_JOIN_TIMEOUT) - super().stop() def _on_odometry(self, msg: Odometry) -> None: - with self._lock: - self._latest_odom = msg + self.start_pose.publish(msg.to_pose_stamped()) def _on_goal(self, point: PointStamped) -> None: - finite = math.isfinite(point.x) and math.isfinite(point.y) and math.isfinite(point.z) - with self._lock: - self._goal = point if finite else None - - def _on_goal_reached(self, msg: Bool) -> None: - if msg.data: - with self._lock: - self._goal = None - - def _replan(self) -> None: - period = 1.0 / self.config.replan_hz - while not self._stop_event.is_set(): - start_time = time.perf_counter() - with self._lock: - goal = self._goal - odom = self._latest_odom - if goal is not None and odom is not None: - start = odom.to_pose_stamped() - # Stop replanning once we are at the goal, otherwise the planner - # spins and the follower loops on goal_reached forever. - if math.hypot(start.x - goal.x, start.y - goal.y) >= self.config.goal_tolerance: - self.start_pose.publish(start) - # Let start_pose land before the goal triggers planning. - self._stop_event.wait(0.05) - self.goal_pose.publish(goal.to_pose_stamped()) - elapsed = time.perf_counter() - start_time - self._stop_event.wait(max(0.0, period - elapsed)) + self.goal_pose.publish(point.to_pose_stamped()) diff --git a/dimos/navigation/nav_3d/mls_planner/mls_planner_native.py b/dimos/navigation/nav_3d/mls_planner/mls_planner_native.py index acc0fd6460..24a86411ae 100644 --- a/dimos/navigation/nav_3d/mls_planner/mls_planner_native.py +++ b/dimos/navigation/nav_3d/mls_planner/mls_planner_native.py @@ -41,6 +41,7 @@ class MLSPlannerNativeConfig(NativeModuleConfig): node_step_threshold_m: float = 0.25 robot_radius_m: float = 0.2 wall_penalty_weight: float = 4.0 + goal_tolerance: float = 0.3 class MLSPlannerNative(NativeModule): diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/main.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/main.rs index 96374e9804..98360e8105 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/main.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/main.rs @@ -11,7 +11,7 @@ use lcm_msgs::geometry_msgs::{Point, Pose, PoseStamped, Quaternion}; use lcm_msgs::nav_msgs::Path; use lcm_msgs::sensor_msgs::{PointCloud2, PointField}; use lcm_msgs::std_msgs::{Header, Time}; -use tracing::{debug, info}; +use tracing::debug; #[derive(Module)] struct MlsPlanner { @@ -49,6 +49,7 @@ struct MlsPlanner { planner: Planner, latest_start: Option<(f32, f32, f32)>, + active_goal: Option<(f32, f32, f32)>, pending_local: Option, pending_bounds: Option, last_path_at: Option, @@ -126,16 +127,22 @@ impl MlsPlanner { z_max: bounds_msg.pose.orientation.z as f32, }; + let update_start = Instant::now(); self.planner.update_region(&points, &bounds, &self.config); + let update_ms = update_start.elapsed().as_secs_f64() * 1e3; + let publish_start = Instant::now(); self.publish_graph().await; + let publish_ms = publish_start.elapsed().as_secs_f64() * 1e3; debug!( + update_ms, + publish_ms, local_points = points.len(), - voxels = self.planner.voxel_count(), - nodes = self.planner.graph().nodes.len(), - "local region processed", + "local region processed" ); + + self.maybe_replan().await; } async fn publish_graph(&self) { @@ -157,23 +164,39 @@ impl MlsPlanner { publish_path(&self.node_edges, &edges_path).await; } + /// Store-only: record the latest start pose. Replanning happens on map + /// updates in `maybe_replan`, not here. async fn on_start_pose(&mut self, msg: PoseStamped) { let p = &msg.pose.position; self.latest_start = Some((p.x as f32, p.y as f32, p.z as f32)); - // Drop any previous plan so the visualizer doesn't show a stale path - // rooted at the old start. - info!("canceling any active path, start pose changed"); - publish_path(&self.path, &empty_path(&self.config.world_frame, now())).await; } + /// Arm the active goal, or clear it on a non-finite goal (the cancel + /// signal). Plans once on arrival so a fresh click is acted on immediately + /// rather than waiting for the next map update. Subsequent replanning is + /// map-driven in `maybe_replan`. A goal arrives once per click, so this is + /// not the odometry-rate external trigger the refactor removed. async fn on_goal_pose(&mut self, msg: PoseStamped) { - let Some(start) = self.latest_start else { - tracing::warn!("MLSPlanner received goal before start; skipping"); - return; - }; - let p = &msg.pose.position; let goal = (p.x as f32, p.y as f32, p.z as f32); + self.active_goal = if goal.0.is_finite() && goal.1.is_finite() && goal.2.is_finite() { + Some(goal) + } else { + None + }; + self.maybe_replan().await; + } + + /// Replan from the stored start to the active goal on fresh map data. Pure + /// glue: it gates and does IO, all planning lives in `Planner::plan`. + async fn maybe_replan(&mut self) { + let (Some(start), Some(goal)) = (self.latest_start, self.active_goal) else { + return; + }; + if is_at_goal(start, goal, self.config.goal_tolerance) { + self.active_goal = None; + return; + } let plan_start = Instant::now(); let waypoints = match self.planner.plan(start, goal, &self.config) { @@ -201,6 +224,11 @@ impl MlsPlanner { } } +/// True when start is within `tol` of goal in the ground plane. +fn is_at_goal(start: (f32, f32, f32), goal: (f32, f32, f32), tol: f32) -> bool { + (start.0 - goal.0).hypot(start.1 - goal.1) < tol +} + fn same_stamp(a: &Time, b: &Time) -> bool { a.sec == b.sec && a.nsec == b.nsec } diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs index deb35d1e23..a544bf578b 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs @@ -43,6 +43,9 @@ pub struct Config { #[serde(default = "default_wall_penalty_weight")] #[validate(range(min = 0.0))] pub wall_penalty_weight: f32, + /// Ground-plane distance from goal at which the planner stops replanning. + #[validate(range(exclusive_min = 0.0))] + pub goal_tolerance: f32, } fn default_robot_radius_m() -> f32 { @@ -475,6 +478,7 @@ mod region_tests { node_step_threshold_m: 0.25, robot_radius_m: 0.0, wall_penalty_weight: 1.0, + goal_tolerance: 0.3, } } diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs index 162e6b6e72..ac9ffbd766 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs @@ -554,6 +554,7 @@ mod tests { node_step_threshold_m: 0.25, robot_radius_m: 0.2, wall_penalty_weight: 4.0, + goal_tolerance: 0.3, }; plan(plg, start, goal, &config) } diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/python.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/python.rs index 4f70de39e4..acca843e63 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/python.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/python.rs @@ -56,6 +56,9 @@ impl MLSPlanner { node_step_threshold_m, robot_radius_m, wall_penalty_weight, + // Only the binary's replan loop reads goal_tolerance. This + // in-process binding plans on demand and never consults it. + goal_tolerance: 1.0, }; config .validate() From edc770e3aac447b886f701fb324f70d6bd5c5929 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Mon, 15 Jun 2026 16:42:46 -0700 Subject: [PATCH 42/56] Reduce emissions --- .../unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py index 2f32da9ec0..37e4db5612 100644 --- a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py +++ b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py @@ -103,7 +103,7 @@ def _static_robot_body(rr: Any) -> list[Any]: map_freq=-1.0, ).remappings([(FastLio2, "global_map", "global_map_fastlio")]), RayTracingVoxelMap.blueprint( - voxel_size=voxel_size, emit_every=1, global_emit_every=1500, max_health=5 + voxel_size=voxel_size, emit_every=2, global_emit_every=600, max_health=5 ), # global_map is remapped off so the planner runs purely on the # incremental local_map + region_bounds pair. From defce96dcaf92358f4dcccb5e040baf73ea3db5c Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Mon, 15 Jun 2026 17:08:10 -0700 Subject: [PATCH 43/56] Worker thread --- .../nav_3d/mls_planner/rust/src/main.rs | 317 +++++++++++------- .../mls_planner/rust/src/mls_planner.rs | 2 +- 2 files changed, 200 insertions(+), 119 deletions(-) diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/main.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/main.rs index 98360e8105..42dcef5082 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/main.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/main.rs @@ -1,6 +1,7 @@ // Copyright 2026 Dimensional Inc. // SPDX-License-Identifier: Apache-2.0 +use std::sync::{Arc, Mutex}; use std::time::{Duration, Instant, SystemTime, UNIX_EPOCH}; use dimos_mls_planner::edges::{edges_to_segments, PlannerGraph}; @@ -11,9 +12,30 @@ use lcm_msgs::geometry_msgs::{Point, Pose, PoseStamped, Quaternion}; use lcm_msgs::nav_msgs::Path; use lcm_msgs::sensor_msgs::{PointCloud2, PointField}; use lcm_msgs::std_msgs::{Header, Time}; +use tokio::sync::Notify; use tracing::debug; +/// A point in the planner's world frame. +type Xyz = (f32, f32, f32); + +/// State shared between the handle loop and the worker. The handle loop writes +/// the newest value, the worker reads it. +type Shared = Arc>>; + +/// A coalesced map input handed from the handle loop to the worker. Only the +/// newest is kept, so a dropped intermediate frame is harmless. +enum MapUpdate { + Region { + cloud: PointCloud2, + bounds: PoseStamped, + }, + Global { + cloud: PointCloud2, + }, +} + #[derive(Module)] +#[module(setup = spawn_worker)] struct MlsPlanner { #[input(decode = PointCloud2::decode, handler = on_global_map)] global_map: Input, @@ -47,159 +69,226 @@ struct MlsPlanner { #[config] config: Config, - planner: Planner, - latest_start: Option<(f32, f32, f32)>, - active_goal: Option<(f32, f32, f32)>, + // Handle-loop-local: pair a local map with its stamp-matching bounds before + // handing the matched snapshot to the worker. pending_local: Option, pending_bounds: Option, - last_path_at: Option, + + // Shared with the worker task. The handle loop only writes these and wakes + // the worker, so it never blocks on the heavy map processing. + pending: Shared, + latest_start: Shared, + active_goal: Shared, + wake: Arc, } impl MlsPlanner { - async fn on_global_map(&mut self, msg: PointCloud2) { - let points = match extract_xyz(&msg) { - Ok(p) => p, - Err(e) => { - warn_throttled!( - Duration::from_secs(1), - error = %e, - "Failed to extract lidar points, dropped a cloud.", - ); - return; - } + /// Spawn the worker that owns the planner graph and does all heavy map + /// processing, so the handle loop stays free to drain its inputs. + async fn spawn_worker(&mut self) { + let worker = Worker { + pending: Arc::clone(&self.pending), + latest_start: Arc::clone(&self.latest_start), + active_goal: Arc::clone(&self.active_goal), + wake: Arc::clone(&self.wake), + config: self.config.clone(), + surface_map: self.surface_map.clone(), + nodes: self.nodes.clone(), + node_edges: self.node_edges.clone(), + path: self.path.clone(), }; - if points.is_empty() { - return; - } - - self.planner.update_global_map(&points, &self.config); - - self.publish_graph().await; + tokio::spawn(worker.run()); + } - debug!( - global_map_points = points.len(), - voxels = self.planner.voxel_count(), - surface_cells = self.planner.surface().count(), - nodes = self.planner.graph().nodes.len(), - edges = self.planner.graph().node_edges.len(), - "global_map processed", - ); + async fn on_global_map(&mut self, msg: PointCloud2) { + self.hand_off(MapUpdate::Global { cloud: msg }); } async fn on_local_map(&mut self, msg: PointCloud2) { self.pending_local = Some(msg); - self.try_region_update().await; + self.try_pair(); } async fn on_region_bounds(&mut self, msg: PoseStamped) { self.pending_bounds = Some(msg); - self.try_region_update().await; + self.try_pair(); } - /// Run the incremental update once a local map and its bounds with - /// matching stamps are both in hand. - async fn try_region_update(&mut self) { + /// Hand a local map and its stamp-matching bounds to the worker once both + /// are in hand. Cheap stamp compare. Runs on the handle loop. + fn try_pair(&mut self) { let (Some(bounds_msg), Some(cloud)) = (&self.pending_bounds, &self.pending_local) else { return; }; if !same_stamp(&bounds_msg.header.stamp, &cloud.header.stamp) { return; } - let bounds_msg = self.pending_bounds.take().expect("checked above"); + let bounds = self.pending_bounds.take().expect("checked above"); let cloud = self.pending_local.take().expect("checked above"); + self.hand_off(MapUpdate::Region { cloud, bounds }); + } - let points = match extract_xyz(&cloud) { - Ok(p) => p, - Err(e) => { - warn_throttled!( - Duration::from_secs(1), - error = %e, - "Failed to extract local map points, dropped a region update.", - ); - return; - } - }; - let bounds = RegionBounds { - origin_x: bounds_msg.pose.position.x as f32, - origin_y: bounds_msg.pose.position.y as f32, - radius: bounds_msg.pose.orientation.x as f32, - z_min: bounds_msg.pose.orientation.y as f32, - z_max: bounds_msg.pose.orientation.z as f32, - }; + /// Store the newest map input (coalescing over any unprocessed one) and + /// wake the worker. + fn hand_off(&self, update: MapUpdate) { + *self.pending.lock().expect("pending mutex") = Some(update); + self.wake.notify_one(); + } - let update_start = Instant::now(); - self.planner.update_region(&points, &bounds, &self.config); - let update_ms = update_start.elapsed().as_secs_f64() * 1e3; + /// Store-only: record the latest start pose. The worker reads it when it + /// replans. No wake here, so odometry never drives replanning. + async fn on_start_pose(&mut self, msg: PoseStamped) { + let p = &msg.pose.position; + *self.latest_start.lock().expect("start mutex") = + Some((p.x as f32, p.y as f32, p.z as f32)); + } - let publish_start = Instant::now(); - self.publish_graph().await; - let publish_ms = publish_start.elapsed().as_secs_f64() * 1e3; + /// Arm the active goal, or clear it on a non-finite goal (the cancel + /// signal), then wake the worker so a fresh click replans immediately + /// against the current graph. A goal arrives once per click, so this is not + /// the odometry-rate external trigger the refactor removed. + async fn on_goal_pose(&mut self, msg: PoseStamped) { + let p = &msg.pose.position; + let goal = (p.x as f32, p.y as f32, p.z as f32); + *self.active_goal.lock().expect("goal mutex") = + (goal.0.is_finite() && goal.1.is_finite() && goal.2.is_finite()).then_some(goal); + self.wake.notify_one(); + } +} - debug!( - update_ms, - publish_ms, - local_points = points.len(), - "local region processed" - ); +/// Owns the planner graph and does every map mutation, graph publish, and +/// replan off the handle loop. Woken by the handlers via `wake`. +struct Worker { + pending: Shared, + latest_start: Shared, + active_goal: Shared, + wake: Arc, + config: Config, + surface_map: Output, + nodes: Output, + node_edges: Output, + path: Output, +} + +impl Worker { + async fn run(self) { + let mut planner = Planner::default(); + let mut last_path_at: Option = None; + loop { + self.wake.notified().await; + // Coalesced: take the newest map input, dropping any intermediates. + let update = self.pending.lock().expect("pending mutex").take(); + if let Some(update) = update { + self.apply_update(&mut planner, update).await; + } + self.maybe_replan(&mut planner, &mut last_path_at).await; + } + } + + /// Mutate the graph from a map update, then publish the graph artifacts. + /// The CPU-bound section runs under `block_in_place` so the runtime can + /// still schedule the handle loop on another thread. + async fn apply_update(&self, planner: &mut Planner, update: MapUpdate) { + let messages = tokio::task::block_in_place(|| self.ingest(planner, update)); + if let Some((surface, node_cloud, edges)) = messages { + publish_cloud(&self.surface_map, &surface).await; + publish_cloud(&self.nodes, &node_cloud).await; + publish_path(&self.node_edges, &edges).await; + } + } - self.maybe_replan().await; + /// Pure-CPU half of `apply_update`: extract points, update the graph, and + /// build the artifact messages. Returns `None` if the cloud was unusable. + fn ingest( + &self, + planner: &mut Planner, + update: MapUpdate, + ) -> Option<(PointCloud2, PointCloud2, Path)> { + match update { + MapUpdate::Region { cloud, bounds } => { + let points = match extract_xyz(&cloud) { + Ok(p) => p, + Err(e) => { + warn_throttled!( + Duration::from_secs(1), + error = %e, + "Failed to extract local map points, dropped a region update.", + ); + return None; + } + }; + let bounds = RegionBounds { + origin_x: bounds.pose.position.x as f32, + origin_y: bounds.pose.position.y as f32, + radius: bounds.pose.orientation.x as f32, + z_min: bounds.pose.orientation.y as f32, + z_max: bounds.pose.orientation.z as f32, + }; + + let update_start = Instant::now(); + planner.update_region(&points, &bounds, &self.config); + debug!( + update_ms = update_start.elapsed().as_secs_f64() * 1e3, + local_points = points.len(), + "local region processed" + ); + } + MapUpdate::Global { cloud } => { + let points = match extract_xyz(&cloud) { + Ok(p) => p, + Err(e) => { + warn_throttled!( + Duration::from_secs(1), + error = %e, + "Failed to extract lidar points, dropped a cloud.", + ); + return None; + } + }; + if points.is_empty() { + return None; + } + planner.update_global_map(&points, &self.config); + debug!(global_map_points = points.len(), "global_map processed"); + } + } + Some(self.build_graph_messages(planner)) } - async fn publish_graph(&self) { + fn build_graph_messages(&self, planner: &Planner) -> (PointCloud2, PointCloud2, Path) { let voxel_size = self.config.voxel_size; let frame = &self.config.world_frame; - let graph = self.planner.graph(); + let graph = planner.graph(); - let surface_points: Vec<(f32, f32, f32)> = self - .planner + let surface_points: Vec = planner .surface() .map(|(ix, iy, iz)| surface_point_xyz(ix, iy, iz, voxel_size)) .collect(); - publish_cloud(&self.surface_map, &surface_points, frame, now()).await; + let surface = build_pc2_xyz(&surface_points, frame, now()); - let node_points: Vec<(f32, f32, f32)> = graph.nodes.iter().map(|n| n.pos).collect(); - publish_cloud(&self.nodes, &node_points, frame, now()).await; + let node_points: Vec = graph.nodes.iter().map(|n| n.pos).collect(); + let node_cloud = build_pc2_xyz(&node_points, frame, now()); - let edges_path = build_segments_path(graph, voxel_size, frame, now()); - publish_path(&self.node_edges, &edges_path).await; - } - - /// Store-only: record the latest start pose. Replanning happens on map - /// updates in `maybe_replan`, not here. - async fn on_start_pose(&mut self, msg: PoseStamped) { - let p = &msg.pose.position; - self.latest_start = Some((p.x as f32, p.y as f32, p.z as f32)); - } - - /// Arm the active goal, or clear it on a non-finite goal (the cancel - /// signal). Plans once on arrival so a fresh click is acted on immediately - /// rather than waiting for the next map update. Subsequent replanning is - /// map-driven in `maybe_replan`. A goal arrives once per click, so this is - /// not the odometry-rate external trigger the refactor removed. - async fn on_goal_pose(&mut self, msg: PoseStamped) { - let p = &msg.pose.position; - let goal = (p.x as f32, p.y as f32, p.z as f32); - self.active_goal = if goal.0.is_finite() && goal.1.is_finite() && goal.2.is_finite() { - Some(goal) - } else { - None - }; - self.maybe_replan().await; + let edges = build_segments_path(graph, voxel_size, frame, now()); + (surface, node_cloud, edges) } - /// Replan from the stored start to the active goal on fresh map data. Pure - /// glue: it gates and does IO, all planning lives in `Planner::plan`. - async fn maybe_replan(&mut self) { - let (Some(start), Some(goal)) = (self.latest_start, self.active_goal) else { + /// Replan from the latest start to the active goal. Pure glue: it gates and + /// does IO, all planning lives in `Planner::plan`. + async fn maybe_replan(&self, planner: &mut Planner, last_path_at: &mut Option) { + let start = *self.latest_start.lock().expect("start mutex"); + let goal = *self.active_goal.lock().expect("goal mutex"); + let (Some(start), Some(goal)) = (start, goal) else { return; }; if is_at_goal(start, goal, self.config.goal_tolerance) { - self.active_goal = None; + *self.active_goal.lock().expect("goal mutex") = None; return; } let plan_start = Instant::now(); - let waypoints = match self.planner.plan(start, goal, &self.config) { + let waypoints = tokio::task::block_in_place(|| planner.plan(start, goal, &self.config)); + let waypoints = match waypoints { Some(wp) => wp, None => { tracing::warn!(?start, ?goal, "no path between start and goal"); @@ -209,10 +298,8 @@ impl MlsPlanner { }; let plan_ms = plan_start.elapsed().as_secs_f64() * 1e3; let produced = Instant::now(); - let since_last_ms = self - .last_path_at - .map_or(-1.0, |t| (produced - t).as_secs_f64() * 1e3); - self.last_path_at = Some(produced); + let since_last_ms = last_path_at.map_or(-1.0, |t| (produced - t).as_secs_f64() * 1e3); + *last_path_at = Some(produced); let stamp = now(); let path_msg = build_path_from_waypoints(&waypoints, &self.config.world_frame, stamp); @@ -225,7 +312,7 @@ impl MlsPlanner { } /// True when start is within `tol` of goal in the ground plane. -fn is_at_goal(start: (f32, f32, f32), goal: (f32, f32, f32), tol: f32) -> bool { +fn is_at_goal(start: Xyz, goal: Xyz, tol: f32) -> bool { (start.0 - goal.0).hypot(start.1 - goal.1) < tol } @@ -233,14 +320,8 @@ fn same_stamp(a: &Time, b: &Time) -> bool { a.sec == b.sec && a.nsec == b.nsec } -async fn publish_cloud( - out: &Output, - points: &[(f32, f32, f32)], - frame_id: &str, - stamp: Time, -) { - let cloud = build_pc2_xyz(points, frame_id, stamp); - if let Err(e) = out.publish(&cloud).await { +async fn publish_cloud(out: &Output, cloud: &PointCloud2) { + if let Err(e) = out.publish(cloud).await { error_throttled!( Duration::from_secs(1), error = %e, diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs index a544bf578b..b635ea2768 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs @@ -17,7 +17,7 @@ use crate::surfaces::{ }; use crate::voxel::{voxelize, VoxelKey}; -#[derive(Debug, Deserialize, Validate)] +#[derive(Debug, Clone, Deserialize, Validate)] #[serde(deny_unknown_fields)] pub struct Config { pub world_frame: String, From e45d0d3874b4cdf7f0c013656fa28583135ef4a1 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Mon, 15 Jun 2026 17:30:07 -0700 Subject: [PATCH 44/56] Vis throttle --- .../nav_3d/mls_planner/mls_planner_native.py | 1 + .../nav_3d/mls_planner/rust/src/main.rs | 52 +++++++++++++------ .../mls_planner/rust/src/mls_planner.rs | 5 ++ .../nav_3d/mls_planner/rust/src/planner.rs | 1 + .../nav_3d/mls_planner/rust/src/python.rs | 2 + .../navigation/unitree_go2_nav_3d.py | 3 ++ 6 files changed, 47 insertions(+), 17 deletions(-) diff --git a/dimos/navigation/nav_3d/mls_planner/mls_planner_native.py b/dimos/navigation/nav_3d/mls_planner/mls_planner_native.py index 24a86411ae..b2c75994ef 100644 --- a/dimos/navigation/nav_3d/mls_planner/mls_planner_native.py +++ b/dimos/navigation/nav_3d/mls_planner/mls_planner_native.py @@ -42,6 +42,7 @@ class MLSPlannerNativeConfig(NativeModuleConfig): robot_radius_m: float = 0.2 wall_penalty_weight: float = 4.0 goal_tolerance: float = 0.3 + viz_publish_hz: float = 2.0 class MLSPlannerNative(NativeModule): diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/main.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/main.rs index 42dcef5082..be90e808e0 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/main.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/main.rs @@ -174,36 +174,54 @@ impl Worker { async fn run(self) { let mut planner = Planner::default(); let mut last_path_at: Option = None; + let mut last_viz_at: Option = None; loop { self.wake.notified().await; // Coalesced: take the newest map input, dropping any intermediates. let update = self.pending.lock().expect("pending mutex").take(); if let Some(update) = update { - self.apply_update(&mut planner, update).await; + self.apply_update(&mut planner, update, &mut last_viz_at) + .await; } self.maybe_replan(&mut planner, &mut last_path_at).await; } } - /// Mutate the graph from a map update, then publish the graph artifacts. - /// The CPU-bound section runs under `block_in_place` so the runtime can - /// still schedule the handle loop on another thread. - async fn apply_update(&self, planner: &mut Planner, update: MapUpdate) { - let messages = tokio::task::block_in_place(|| self.ingest(planner, update)); + /// Mutate the graph from a map update, then publish the viz artifacts at no + /// more than `viz_publish_hz`. The graph itself is always updated; only the + /// surface_map / nodes / node_edges rebuild + publish is rate-capped, since + /// rebuilding the whole graph every cycle is the dominant per-cycle cost and + /// nothing on the planning path consumes those outputs. The CPU-bound + /// section runs under `block_in_place` so the runtime can still schedule the + /// handle loop on another thread. + async fn apply_update( + &self, + planner: &mut Planner, + update: MapUpdate, + last_viz_at: &mut Option, + ) { + let now = Instant::now(); + let viz_due = self.config.viz_publish_hz > 0.0 && { + let viz_interval = Duration::from_secs_f32(1.0 / self.config.viz_publish_hz); + last_viz_at.is_none_or(|t| now.duration_since(t) >= viz_interval) + }; + + let messages = tokio::task::block_in_place(|| { + let updated = self.ingest(planner, update); + (updated && viz_due).then(|| self.build_graph_messages(planner)) + }); + if let Some((surface, node_cloud, edges)) = messages { publish_cloud(&self.surface_map, &surface).await; publish_cloud(&self.nodes, &node_cloud).await; publish_path(&self.node_edges, &edges).await; + *last_viz_at = Some(now); } } - /// Pure-CPU half of `apply_update`: extract points, update the graph, and - /// build the artifact messages. Returns `None` if the cloud was unusable. - fn ingest( - &self, - planner: &mut Planner, - update: MapUpdate, - ) -> Option<(PointCloud2, PointCloud2, Path)> { + /// Pure-CPU graph mutation from a map update. Returns whether the graph was + /// updated (false if the cloud was unusable). + fn ingest(&self, planner: &mut Planner, update: MapUpdate) -> bool { match update { MapUpdate::Region { cloud, bounds } => { let points = match extract_xyz(&cloud) { @@ -214,7 +232,7 @@ impl Worker { error = %e, "Failed to extract local map points, dropped a region update.", ); - return None; + return false; } }; let bounds = RegionBounds { @@ -242,17 +260,17 @@ impl Worker { error = %e, "Failed to extract lidar points, dropped a cloud.", ); - return None; + return false; } }; if points.is_empty() { - return None; + return false; } planner.update_global_map(&points, &self.config); debug!(global_map_points = points.len(), "global_map processed"); } } - Some(self.build_graph_messages(planner)) + true } fn build_graph_messages(&self, planner: &Planner) -> (PointCloud2, PointCloud2, Path) { diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs index b635ea2768..741ae6c982 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs @@ -46,6 +46,10 @@ pub struct Config { /// Ground-plane distance from goal at which the planner stops replanning. #[validate(range(exclusive_min = 0.0))] pub goal_tolerance: f32, + /// Rate cap for republishing the surface_map / nodes / node_edges viz + /// artifacts. 0 disables them entirely. The path output is unthrottled. + #[validate(range(min = 0.0))] + pub viz_publish_hz: f32, } fn default_robot_radius_m() -> f32 { @@ -479,6 +483,7 @@ mod region_tests { robot_radius_m: 0.0, wall_penalty_weight: 1.0, goal_tolerance: 0.3, + viz_publish_hz: 2.0, } } diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs index ac9ffbd766..833c5c6fdc 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs @@ -555,6 +555,7 @@ mod tests { robot_radius_m: 0.2, wall_penalty_weight: 4.0, goal_tolerance: 0.3, + viz_publish_hz: 2.0, }; plan(plg, start, goal, &config) } diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/python.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/python.rs index acca843e63..413dd68ae4 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/python.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/python.rs @@ -59,6 +59,8 @@ impl MLSPlanner { // Only the binary's replan loop reads goal_tolerance. This // in-process binding plans on demand and never consults it. goal_tolerance: 1.0, + // Only the binary's worker publishes viz artifacts. Unused here. + viz_publish_hz: 1.0, }; config .validate() diff --git a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py index 37e4db5612..7c70d2f6e9 100644 --- a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py +++ b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py @@ -111,6 +111,9 @@ def _static_robot_body(rr: Any) -> list[Any]: world_frame="odom", voxel_size=voxel_size, robot_height=go2_lidar_height, + # The surface_map / nodes / node_edges viz artifacts are suppressed in + # the viewer below, so don't pay to build and publish them. + viz_publish_hz=0.0, ).remappings([(MLSPlannerNative, "global_map", "global_map_unused")]), GoalRelay.blueprint(), BasicPathFollower.blueprint(lookahead_m=0.5, heading_gain=0.8, max_angular=0.6), From 6865fe9e7b35282d45ca9a0733c188d976cb8faf Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Tue, 16 Jun 2026 17:01:07 -0700 Subject: [PATCH 45/56] Adjust fields --- dimos/core/global_config.py | 3 +++ .../go2/blueprints/navigation/unitree_go2_nav_3d.py | 10 ++++++++-- 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/dimos/core/global_config.py b/dimos/core/global_config.py index 6ea946ebea..d52434d4fd 100644 --- a/dimos/core/global_config.py +++ b/dimos/core/global_config.py @@ -73,6 +73,9 @@ class GlobalConfig(BaseSettings): env_file=".env", env_file_encoding="utf-8", extra="ignore", + # Coerce on assignment so string overrides from the CLI (-o g.field=val) + # and env become real typed values instead of raw strings. + validate_assignment=True, ) def update(self, **kwargs: object) -> None: diff --git a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py index 7c70d2f6e9..397e0987fc 100644 --- a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py +++ b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py @@ -103,7 +103,7 @@ def _static_robot_body(rr: Any) -> list[Any]: map_freq=-1.0, ).remappings([(FastLio2, "global_map", "global_map_fastlio")]), RayTracingVoxelMap.blueprint( - voxel_size=voxel_size, emit_every=2, global_emit_every=600, max_health=5 + voxel_size=voxel_size, emit_every=2, global_emit_every=150, max_health=5 ), # global_map is remapped off so the planner runs purely on the # incremental local_map + region_bounds pair. @@ -111,6 +111,12 @@ def _static_robot_body(rr: Any) -> list[Any]: world_frame="odom", voxel_size=voxel_size, robot_height=go2_lidar_height, + # Hard wall clearance: paths stay at least this far off walls. Raised + # from the 0.2 default so the robot keeps more distance. + robot_radius_m=0.35, + # Soft push toward the open centerline, decaying with wall distance. + # Never blocks a corridor, so safe to raise well above the 4.0 default. + wall_penalty_weight=8.0, # The surface_map / nodes / node_edges viz artifacts are suppressed in # the viewer below, so don't pay to build and publish them. viz_publish_hz=0.0, @@ -118,6 +124,6 @@ def _static_robot_body(rr: Any) -> list[Any]: GoalRelay.blueprint(), BasicPathFollower.blueprint(lookahead_m=0.5, heading_gain=0.8, max_angular=0.6), MovementManager.blueprint(), -).global_config(n_workers=10, robot_model="unitree_go2") +).global_config(n_workers=10, robot_model="unitree_go2", obstacle_avoidance=True) __all__ = ["unitree_go2_nav_3d"] From 0f6e87dd43ed59587ac087cad8d976e59e251f5e Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Tue, 16 Jun 2026 17:29:16 -0700 Subject: [PATCH 46/56] Better plans --- .../nav_3d/mls_planner/rust/src/nodes.rs | 2 +- .../nav_3d/mls_planner/rust/src/planner.rs | 134 ++++++++++++------ 2 files changed, 94 insertions(+), 42 deletions(-) diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/nodes.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/nodes.rs index a6e3160b27..3b5518f9f6 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/nodes.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/nodes.rs @@ -313,7 +313,7 @@ fn scale_edges( /// Cost multiplier at wall distance d. Infinite inside the robot radius, /// then decays from 1 + weight toward 1 with length scale buffer_m. #[inline] -fn penalty_of(d: f32, buffer_m: f32, robot_radius_m: f32, weight: f32) -> f32 { +pub(crate) fn penalty_of(d: f32, buffer_m: f32, robot_radius_m: f32, weight: f32) -> f32 { if d < robot_radius_m { return f32::INFINITY; } diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs index 833c5c6fdc..c0e7d60965 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs @@ -10,6 +10,7 @@ use crate::adjacency::{CellId, SurfaceCells, SurfaceLookup}; use crate::dijkstra::walk_preds; use crate::edges::{NodeEdgeIdx, NodeId, PlannerGraph, NO_NODE}; use crate::mls_planner::Config; +use crate::nodes::penalty_of; use crate::voxel::{surface_point_xyz, VoxelKey}; /// Robot-rooted candidate search radius, in multiples of node spacing. @@ -170,8 +171,14 @@ pub fn plan( // Shortcut height tolerance in cells, tied to the traversable step. let smooth_tol_cells = ((config.node_step_threshold_m / voxel_size).round() as i32).max(1); + let wall_cost = WallCost { + robot_radius_m: config.robot_radius_m, + buffer_m: config.node_wall_buffer_m, + weight: config.wall_penalty_weight, + voxel_size, + }; let cells = assemble_cells(plg, &node_seq, &lead_in, &goal_segment); - let cells = string_pull(plg, &cells, smooth_tol_cells, config.node_wall_buffer_m); + let cells = string_pull(plg, &cells, smooth_tol_cells, &wall_cost); Some(cells_to_waypoints( plg, &cells, start_pose, goal_pose, voxel_size, )) @@ -386,48 +393,77 @@ fn cells_to_waypoints( waypoints } -/// Shortcut runs of cells with straight on-surface segments, keeping the -/// farthest cell in line of sight from each anchor. Shortcuts never pass -/// still account for the wall buffer, so even the new path should be safe. -fn string_pull(plg: &PlannerGraph, cells: &[CellId], tol_cells: i32, buffer_m: f32) -> Vec { +/// Wall-penalty cost model for smoothing, mirroring the node graph's. +struct WallCost { + robot_radius_m: f32, + buffer_m: f32, + weight: f32, + voxel_size: f32, +} + +/// Shortcut runs of cells with straight on-surface segments. A shortcut is only +/// taken when its wall-penalty-weighted cost is no worse than the rough sub-path +/// it replaces, so smoothing never trades away the clearance the node graph +/// earned: it straightens in the open but keeps the centered route near walls. +fn string_pull(plg: &PlannerGraph, cells: &[CellId], tol_cells: i32, wc: &WallCost) -> Vec { if cells.len() <= 2 { return cells.to_vec(); } + let cost = |from: CellId, to: CellId| { + segment_cost( + plg, + plg.cells.coord(from), + plg.cells.coord(to), + tol_cells, + wc, + ) + }; let mut out = vec![cells[0]]; let mut anchor = 0; while anchor + 1 < cells.len() { - let anchor_coord = plg.cells.coord(cells[anchor]); - let mut last_ok = anchor + 1; + // Farthest cell reachable by a straight segment that is at least as safe + // (penalty-weighted) as the rough path between anchor and that cell. + let mut best = anchor + 1; + let mut rough_cost = 0.0_f32; let mut j = anchor + 1; while j < cells.len() { - let coord = plg.cells.coord(cells[j]); - if !los_on_surface(plg, anchor_coord, coord, tol_cells, buffer_m) { - break; + rough_cost += cost(cells[j - 1], cells[j]).unwrap_or(f32::INFINITY); + match cost(cells[anchor], cells[j]) { + // Straight segment is no costlier than the rough path it spans. + Some(shortcut) if shortcut <= rough_cost + 1e-3 => best = j, + // Segment leaves the surface or enters the robot radius — stop. + None => break, + // Valid but less safe than the rough path; keep scanning farther. + _ => {} } - last_ok = j; j += 1; } - out.push(cells[last_ok]); - anchor = last_ok; + out.push(cells[best]); + anchor = best; } out } -/// True if every column the segment crosses has a surface cell near the -/// segment height that is at least buffer_m from the nearest wall. -fn los_on_surface( +/// Wall-penalty-weighted length of the straight on-surface segment a -> b, +/// matching the node graph's cost model (geometric length times the average +/// `penalty_of` along it). None if the segment leaves the surface, jumps more +/// than tol_cells in height, or passes within robot_radius_m of a wall. +fn segment_cost( plg: &PlannerGraph, a: VoxelKey, b: VoxelKey, tol_cells: i32, - buffer_m: f32, -) -> bool { + wc: &WallCost, +) -> Option { let (dx, dy, dz) = (b.0 - a.0, b.1 - a.1, b.2 - a.2); let samples = dx.abs().max(dy.abs()) * 2; if samples == 0 { - return true; + return Some(0.0); } + let length_m = (((dx * dx + dy * dy) as f32).sqrt()) * wc.voxel_size; let (mut last_ix, mut last_iy) = (i32::MIN, i32::MIN); + let mut penalty_sum = 0.0_f32; + let mut counted = 0u32; for k in 0..=samples { let t = k as f32 / samples as f32; let ix = (a.0 as f32 + t * dx as f32).round() as i32; @@ -438,9 +474,7 @@ fn los_on_surface( last_ix = ix; last_iy = iy; let iz_line = a.2 as f32 + t * dz as f32; - let Some(zs) = plg.surface_lookup.get(&(ix, iy)) else { - return false; - }; + let zs = plg.surface_lookup.get(&(ix, iy))?; // Surface cell in this column nearest the interpolated segment height. let mut nearest: Option<(f32, i32)> = None; for &iz in zs { @@ -449,25 +483,30 @@ fn los_on_surface( nearest = Some((d, iz)); } } - let Some((d, iz)) = nearest else { - return false; - }; + let (d, iz) = nearest?; if d > tol_cells as f32 { - return false; + return None; } - if let Some(id) = plg.cells.id((ix, iy, iz)) { - let wall_dist = plg - .wall_state - .dist - .get(id as usize) - .copied() - .unwrap_or(f32::INFINITY); - if wall_dist < buffer_m { - return false; + // Columns on the surface but not in the graph carry no penalty. + let p = match plg.cells.id((ix, iy, iz)) { + Some(id) => { + let wall_dist = plg + .wall_state + .dist + .get(id as usize) + .copied() + .unwrap_or(f32::INFINITY); + penalty_of(wall_dist, wc.buffer_m, wc.robot_radius_m, wc.weight) } + None => 1.0, + }; + if !p.is_finite() { + return None; } + penalty_sum += p; + counted += 1; } - true + Some(length_m * penalty_sum / counted.max(1) as f32) } fn edge_between(plg: &PlannerGraph, a: NodeId, b: NodeId) -> Option { @@ -661,15 +700,22 @@ mod tests { "waypoint {w:?} is off the surface" ); } + let wc = WallCost { + robot_radius_m: 0.2, + buffer_m: 0.3, + weight: 4.0, + voxel_size: VOXEL, + }; for pair in wp[1..wp.len() - 1].windows(2) { assert!( - los_on_surface( + segment_cost( &plg, waypoint_key(&pair[0]), waypoint_key(&pair[1]), tol, - 0.3 - ), + &wc + ) + .is_some(), "segment {:?} -> {:?} leaves the surface", pair[0], pair[1] @@ -706,13 +752,19 @@ mod tests { build_surface_cells(&mut plg.cells, &plg.surface_lookup, VOXEL, 2); let path: Vec = (0..10).map(|x| plg.cells.id((x, 0, 0)).unwrap()).collect(); + let wc = WallCost { + robot_radius_m: 0.2, + buffer_m: 0.3, + weight: 4.0, + voxel_size: VOXEL, + }; plg.wall_state.dist = vec![f32::INFINITY; plg.cells.slot_capacity()]; - let open = string_pull(&plg, &path, 1, 0.3); + let open = string_pull(&plg, &path, 1, &wc); assert_eq!(open.len(), 2, "open strip should collapse to its endpoints"); let mid = plg.cells.id((5, 0, 0)).unwrap(); plg.wall_state.dist[mid as usize] = 0.1; - let guarded = string_pull(&plg, &path, 1, 0.3); + let guarded = string_pull(&plg, &path, 1, &wc); assert!( guarded.len() > 2, "shortcut across a sub-buffer cell must be refused: {guarded:?}" From 383bf60028b6423e00328bd6722396dc2af247b3 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Tue, 16 Jun 2026 17:42:44 -0700 Subject: [PATCH 47/56] Better turns --- dimos/navigation/basic_path_follower/module.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/dimos/navigation/basic_path_follower/module.py b/dimos/navigation/basic_path_follower/module.py index 8915c121e7..b72db091b5 100644 --- a/dimos/navigation/basic_path_follower/module.py +++ b/dimos/navigation/basic_path_follower/module.py @@ -45,8 +45,6 @@ class BasicPathFollowerConfig(ModuleConfig): lookahead_m: float = 0.6 heading_gain: float = 1.5 max_angular: float = 1.0 - # Rotate in place instead of advancing when heading error exceeds this. - rotate_threshold: float = 0.6 class BasicPathFollower(Module): @@ -183,9 +181,11 @@ def _step(self, odom: PoseStamped, waypoints: np.ndarray) -> None: -self.config.max_angular, min(self.config.max_angular, self.config.heading_gain * yaw_error), ) - linear = 0.0 - if abs(yaw_error) <= self.config.rotate_threshold: - linear = self.config.speed * max(0.0, math.cos(yaw_error)) + # Taper forward speed with heading error instead of a hard stop: the + # robot decelerates into a turn and pivots in place only near 90 degrees + # (cos -> 0). Slower speed at sharp corners means a tighter turn radius, + # so it tracks the safe path instead of stopping and lurching. + linear = self.config.speed * max(0.0, math.cos(yaw_error)) self.nav_cmd_vel.publish(Twist(Vector3(linear, 0, 0), Vector3(0, 0, angular))) From 536224941465ea7e20f37f227aed1883343ddfb7 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Tue, 16 Jun 2026 18:08:07 -0700 Subject: [PATCH 48/56] Tuning --- .../unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py index 397e0987fc..1591a2bd36 100644 --- a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py +++ b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py @@ -111,9 +111,8 @@ def _static_robot_body(rr: Any) -> list[Any]: world_frame="odom", voxel_size=voxel_size, robot_height=go2_lidar_height, - # Hard wall clearance: paths stay at least this far off walls. Raised - # from the 0.2 default so the robot keeps more distance. - robot_radius_m=0.35, + # Hard wall clearance radius + robot_radius_m=0.2, # Soft push toward the open centerline, decaying with wall distance. # Never blocks a corridor, so safe to raise well above the 4.0 default. wall_penalty_weight=8.0, From 67e24e77669a99c4f3947bd1fa7c93cb606c3bfd Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Tue, 16 Jun 2026 18:18:33 -0700 Subject: [PATCH 49/56] Debug --- dimos/navigation/basic_path_follower/module.py | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/dimos/navigation/basic_path_follower/module.py b/dimos/navigation/basic_path_follower/module.py index b72db091b5..e1ee012f2a 100644 --- a/dimos/navigation/basic_path_follower/module.py +++ b/dimos/navigation/basic_path_follower/module.py @@ -76,6 +76,7 @@ def __init__(self, **kwargs: Any) -> None: self._stats_last = 0.0 self._last_path_t = 0.0 self._max_gap = 0.0 + self._cmd_log_last = 0.0 @rpc def start(self) -> None: @@ -187,6 +188,20 @@ def _step(self, odom: PoseStamped, waypoints: np.ndarray) -> None: # so it tracks the safe path instead of stopping and lurching. linear = self.config.speed * max(0.0, math.cos(yaw_error)) + # DEBUG: command trace, remove before merge + now = time.perf_counter() + if now - self._cmd_log_last >= 0.2: + self._cmd_log_last = now + logger.debug( + "follower cmd", + lin=round(linear, 3), + ang=round(angular, 3), + yaw_err_deg=round(math.degrees(yaw_error), 1), + target_dx=round(float(target[0] - position[0]), 2), + target_dy=round(float(target[1] - position[1]), 2), + goal_dist=round(float(np.linalg.norm(waypoints[-1] - position)), 2), + ) + self.nav_cmd_vel.publish(Twist(Vector3(linear, 0, 0), Vector3(0, 0, angular))) def _lookahead_point(self, waypoints: np.ndarray, position: np.ndarray) -> np.ndarray: From 68a02fd2439cc4b442e91f1842e6ce2a5a884f67 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Tue, 16 Jun 2026 18:57:39 -0700 Subject: [PATCH 50/56] WIP sport mode --- dimos/robot/unitree/connection.py | 25 +++++++++++++++++++ dimos/robot/unitree/dimsim_connection.py | 3 +++ .../navigation/unitree_go2_nav_3d.py | 7 ++++-- dimos/robot/unitree/go2/connection.py | 12 +++++++++ dimos/robot/unitree/mujoco_connection.py | 3 +++ 5 files changed, 48 insertions(+), 2 deletions(-) diff --git a/dimos/robot/unitree/connection.py b/dimos/robot/unitree/connection.py index 44101cc19d..1afe2638e5 100644 --- a/dimos/robot/unitree/connection.py +++ b/dimos/robot/unitree/connection.py @@ -15,6 +15,7 @@ import asyncio from dataclasses import dataclass import functools +import json import threading import time from typing import Any, TypeAlias, TypeVar @@ -320,6 +321,30 @@ def set_obstacle_avoidance(self, enabled: bool = True) -> None: {"api_id": 1001, "parameter": {"enable": int(enabled)}}, ) + def set_motion_mode(self, name: str) -> None: + """Select the robot's top-level motion mode via the motion switcher. + + 'mcf' is the AI/sport controller (firmware >= 1.1.7) that handles + terrain, including walking up and down stairs. 'normal' is the basic + controller. Stair traversal lives in mcf, so the nav stack selects it. + """ + # api_id 1001 = CheckMode, 1002 = SelectMode, param {"name": }. + current = None + try: + resp = self.publish_request(RTC_TOPIC["MOTION_SWITCHER"], {"api_id": 1001}) + current = json.loads(resp["data"]["data"]).get("name") + except (KeyError, TypeError, ValueError) as e: + print(f"Motion mode check failed: {e}") + print(f"Motion mode: current={current!r} requested={name!r}") + if current == name: + return + self.publish_request( + RTC_TOPIC["MOTION_SWITCHER"], + {"api_id": 1002, "parameter": {"name": name}}, + ) + # Switching controllers makes the robot re-stand; give it a moment. + time.sleep(5) + def free_walk(self) -> bool: """Activate FreeWalk locomotion mode — enables walking and velocity commands.""" return bool(self.publish_request(RTC_TOPIC["SPORT_MOD"], {"api_id": SPORT_CMD["FreeWalk"]})) diff --git a/dimos/robot/unitree/dimsim_connection.py b/dimos/robot/unitree/dimsim_connection.py index 5afcd1fda7..8f5d222f5a 100644 --- a/dimos/robot/unitree/dimsim_connection.py +++ b/dimos/robot/unitree/dimsim_connection.py @@ -94,6 +94,9 @@ def balance_stand(self) -> bool: def set_obstacle_avoidance(self, enabled: bool = True) -> None: pass + def set_motion_mode(self, name: str) -> None: + pass + def enable_rage_mode(self) -> bool: return True diff --git a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py index 1591a2bd36..9da454f61c 100644 --- a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py +++ b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py @@ -91,7 +91,10 @@ def _static_robot_body(rr: Any) -> list[Any]: unitree_go2_nav_3d = autoconnect( vis_module(viewer_backend=global_config.viewer, rerun_config=_nav_rerun_config), - GO2Connection.blueprint(lidar=False, camera=False).remappings( + # motion_mode="mcf" selects the AI/sport controller that walks up and down + # stairs. With it, obstacle_avoidance is turned off below, matching the app + # recipe for stair traversal. + GO2Connection.blueprint(lidar=False, camera=False, motion_mode="mcf").remappings( [ (GO2Connection, "lidar", "lidar_l1"), (GO2Connection, "odom", "odom_go2"), @@ -123,6 +126,6 @@ def _static_robot_body(rr: Any) -> list[Any]: GoalRelay.blueprint(), BasicPathFollower.blueprint(lookahead_m=0.5, heading_gain=0.8, max_angular=0.6), MovementManager.blueprint(), -).global_config(n_workers=10, robot_model="unitree_go2", obstacle_avoidance=True) +).global_config(n_workers=10, robot_model="unitree_go2", obstacle_avoidance=False) __all__ = ["unitree_go2_nav_3d"] diff --git a/dimos/robot/unitree/go2/connection.py b/dimos/robot/unitree/go2/connection.py index 54f84cb3b3..63ed750150 100644 --- a/dimos/robot/unitree/go2/connection.py +++ b/dimos/robot/unitree/go2/connection.py @@ -69,6 +69,9 @@ class ConnectionConfig(ModuleConfig): mode: Go2Mode = Go2Mode.DEFAULT lidar: bool = True camera: bool = True + # Top-level motion controller: "mcf" is the AI/sport mode that traverses + # terrain (stairs); "normal" is basic. None leaves the current mode as-is. + motion_mode: str | None = None class Go2ConnectionProtocol(Protocol): @@ -84,6 +87,7 @@ def standup(self) -> bool: ... def liedown(self) -> bool: ... def balance_stand(self) -> bool: ... def set_obstacle_avoidance(self, enabled: bool = True) -> None: ... + def set_motion_mode(self, name: str) -> None: ... def enable_rage_mode(self) -> bool: ... def publish_request(self, topic: str, data: dict) -> dict: ... # type: ignore[type-arg] @@ -173,6 +177,9 @@ def balance_stand(self) -> bool: def set_obstacle_avoidance(self, enabled: bool = True) -> None: pass + def set_motion_mode(self, name: str) -> None: + pass + def enable_rage_mode(self) -> bool: return True @@ -256,6 +263,11 @@ def onimage(image: Image) -> None: ) self._camera_info_thread.start() + # Select the terrain-capable controller (mcf) before standing, so the + # robot can walk up and down stairs under our velocity commands. + if self.config.motion_mode: + self.connection.set_motion_mode(self.config.motion_mode) + self.standup() time.sleep(3) self.connection.balance_stand() diff --git a/dimos/robot/unitree/mujoco_connection.py b/dimos/robot/unitree/mujoco_connection.py index 4c455899e8..9ce7d41de5 100644 --- a/dimos/robot/unitree/mujoco_connection.py +++ b/dimos/robot/unitree/mujoco_connection.py @@ -237,6 +237,9 @@ def balance_stand(self) -> bool: def set_obstacle_avoidance(self, enabled: bool = True) -> None: pass + def set_motion_mode(self, name: str) -> None: + pass + def enable_rage_mode(self) -> bool: return True From 13294b5c2c6a748ee6418867304528662101f268 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Tue, 16 Jun 2026 19:31:53 -0700 Subject: [PATCH 51/56] Aggressive settings --- .../go2/blueprints/navigation/unitree_go2_nav_3d.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py index 9da454f61c..ad6ede5ac4 100644 --- a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py +++ b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py @@ -114,13 +114,13 @@ def _static_robot_body(rr: Any) -> list[Any]: world_frame="odom", voxel_size=voxel_size, robot_height=go2_lidar_height, - # Hard wall clearance radius + # Hard wall clearance radius and node-placement wall buffer, lowered so + # nodes can sit on the narrow stair treads. robot_radius_m=0.2, - # Soft push toward the open centerline, decaying with wall distance. - # Never blocks a corridor, so safe to raise well above the 4.0 default. + node_wall_buffer_m=0.15, + # Max step the planner will connect across (robot step height). + node_step_threshold_m=0.25, wall_penalty_weight=8.0, - # The surface_map / nodes / node_edges viz artifacts are suppressed in - # the viewer below, so don't pay to build and publish them. viz_publish_hz=0.0, ).remappings([(MLSPlannerNative, "global_map", "global_map_unused")]), GoalRelay.blueprint(), From 70ae32ed1b8740d43e6731f1132392a20012de1f Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Wed, 17 Jun 2026 13:30:04 -0700 Subject: [PATCH 52/56] Better safety configs and multi run --- .../nav_3d/mls_planner/mls_planner.pyi | 9 +- .../nav_3d/mls_planner/mls_planner_native.py | 9 +- .../nav_3d/mls_planner/rust/src/adjacency.rs | 13 +- .../mls_planner/rust/src/mls_planner.rs | 237 +++++++++++++++--- .../nav_3d/mls_planner/rust/src/nodes.rs | 175 +++++++++---- .../nav_3d/mls_planner/rust/src/planner.rs | 161 +++++++----- .../nav_3d/mls_planner/rust/src/python.rs | 27 +- .../nav_3d/mls_planner/utils/plan_rrd.py | 200 ++++++++++----- .../navigation/unitree_go2_nav_3d.py | 12 +- 9 files changed, 598 insertions(+), 245 deletions(-) diff --git a/dimos/navigation/nav_3d/mls_planner/mls_planner.pyi b/dimos/navigation/nav_3d/mls_planner/mls_planner.pyi index f0d589e3dd..0565924a9f 100644 --- a/dimos/navigation/nav_3d/mls_planner/mls_planner.pyi +++ b/dimos/navigation/nav_3d/mls_planner/mls_planner.pyi @@ -26,10 +26,11 @@ class MLSPlanner: surface_dilation_passes: int = 3, surface_erosion_passes: int = 3, node_spacing_m: float = 1.0, - node_wall_buffer_m: float = 0.3, - node_step_threshold_m: float = 0.25, - robot_radius_m: float = 0.2, - wall_penalty_weight: float = 4.0, + wall_clearance_m: float = 0.3, + wall_buffer_m: float = 0.75, + wall_buffer_weight: float = 100.0, + step_threshold_m: float = 0.25, + step_penalty_weight: float = 4.0, ) -> None: ... def update_global_map(self, points: NDArray[np.float32]) -> None: """Voxelize the map and rebuild surfaces, nodes, and edges. Shape (N, 3) float32.""" diff --git a/dimos/navigation/nav_3d/mls_planner/mls_planner_native.py b/dimos/navigation/nav_3d/mls_planner/mls_planner_native.py index b2c75994ef..0b427590a2 100644 --- a/dimos/navigation/nav_3d/mls_planner/mls_planner_native.py +++ b/dimos/navigation/nav_3d/mls_planner/mls_planner_native.py @@ -37,10 +37,11 @@ class MLSPlannerNativeConfig(NativeModuleConfig): surface_dilation_passes: int = 3 surface_erosion_passes: int = 3 node_spacing_m: float = 1.0 - node_wall_buffer_m: float = 0.3 - node_step_threshold_m: float = 0.25 - robot_radius_m: float = 0.2 - wall_penalty_weight: float = 4.0 + wall_clearance_m: float = 0.3 + wall_buffer_m: float = 0.75 + wall_buffer_weight: float = 100.0 + step_threshold_m: float = 0.25 + step_penalty_weight: float = 4.0 goal_tolerance: float = 0.3 viz_publish_hz: float = 2.0 diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/adjacency.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/adjacency.rs index 2a6d1c84a8..65897c06b6 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/adjacency.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/adjacency.rs @@ -21,12 +21,20 @@ pub const NO_CELL: CellId = u32::MAX; const TOMBSTONE: VoxelKey = (i32::MIN, i32::MIN, i32::MIN); const NEIGHBORS_4: [(i32, i32); 4] = [(-1, 0), (1, 0), (0, -1), (0, 1)]; +/// Vertical extent of a `dz`-cell change in meters, the step-penalty input. +#[inline] +pub fn rise(dz: i32, voxel_size: f32) -> f32 { + dz.unsigned_abs() as f32 * voxel_size +} + #[derive(Clone, Copy, Debug)] pub struct Edge { pub dest: CellId, /// Geometric cost, set at build time and never mutated. pub base_cost: f32, - /// base_cost scaled by the wall-safe penalty, recomputed each update. + /// Vertical change of the edge in meters, for the step penalty. + pub rise: f32, + /// base_cost scaled by the wall penalty plus the step penalty. pub cost: f32, } @@ -127,6 +135,7 @@ impl SurfaceCells { self.edges[src as usize].push(Edge { dest, base_cost: cost, + rise: 0.0, cost, }); } @@ -224,6 +233,7 @@ pub fn build_surface_cells( local.push(Edge { dest, base_cost: cost, + rise: rise(dz, voxel_size), cost, }); } @@ -281,6 +291,7 @@ pub fn rebuild_edges_around( edges.push(Edge { dest, base_cost: cost, + rise: rise(dz, voxel_size), cost, }); } diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs index 741ae6c982..520a447956 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs @@ -6,7 +6,7 @@ use ahash::AHashSet; use rayon::prelude::*; use serde::Deserialize; -use validator::Validate; +use validator::{Validate, ValidationError}; use crate::adjacency::{build_surface_cells, build_surface_lookup, rebuild_edges_around, CellId}; use crate::edges::{build_node_edges, build_node_edges_region, PlannerGraph}; @@ -18,6 +18,7 @@ use crate::surfaces::{ use crate::voxel::{voxelize, VoxelKey}; #[derive(Debug, Clone, Deserialize, Validate)] +#[validate(schema(function = "validate_wall_buffer"))] #[serde(deny_unknown_fields)] pub struct Config { pub world_frame: String, @@ -31,18 +32,23 @@ pub struct Config { pub surface_erosion_passes: u32, #[validate(range(exclusive_min = 0.0))] pub node_spacing_m: f32, + /// Hard clearance. Cells closer than this to a wall or edge are impassable. #[validate(range(min = 0.0))] - pub node_wall_buffer_m: f32, + pub wall_clearance_m: f32, + /// Width of the soft standoff zone beyond the clearance. Paths prefer to stay + /// clearance + buffer from walls. #[validate(range(min = 0.0))] - pub node_step_threshold_m: f32, - /// Hard clearance floor: cells closer than this to a wall are impassable. - #[serde(default = "default_robot_radius_m")] + pub wall_buffer_m: f32, + /// Peak soft wall penalty at the clearance edge: the cost multiplier there is + /// 1 + this, decaying to 1 at the outer edge of the buffer zone. #[validate(range(min = 0.0))] - pub robot_radius_m: f32, - /// Strength of the soft wall penalty at the radius, decaying with distance. - #[serde(default = "default_wall_penalty_weight")] + pub wall_buffer_weight: f32, + /// Max traversable vertical step. Taller steps are impassable. #[validate(range(min = 0.0))] - pub wall_penalty_weight: f32, + pub step_threshold_m: f32, + /// Soft cost added per meter of vertical climb. + #[validate(range(min = 0.0))] + pub step_penalty_weight: f32, /// Ground-plane distance from goal at which the planner stops replanning. #[validate(range(exclusive_min = 0.0))] pub goal_tolerance: f32, @@ -52,12 +58,14 @@ pub struct Config { pub viz_publish_hz: f32, } -fn default_robot_radius_m() -> f32 { - 0.2 -} - -fn default_wall_penalty_weight() -> f32 { - 4.0 +/// The soft wall penalty needs a non-zero zone to act in. +fn validate_wall_buffer(config: &Config) -> Result<(), ValidationError> { + if config.wall_buffer_weight > 0.0 && config.wall_buffer_m == 0.0 { + return Err(ValidationError::new( + "wall_buffer_weight requires wall_buffer_m > 0", + )); + } + Ok(()) } /// Cylindrical region the planner re-derives from a local map slice. @@ -165,7 +173,7 @@ impl Planner { removed: Vec, config: &Config, ) { - let step = (config.node_step_threshold_m / config.voxel_size).floor() as i32; + let step = (config.step_threshold_m / config.voxel_size).floor() as i32; for &c in &removed { self.graph.cells.remove(c); } @@ -191,9 +199,10 @@ impl Planner { &window, config.voxel_size, config.node_spacing_m, - config.node_wall_buffer_m, - config.robot_radius_m, - config.wall_penalty_weight, + config.wall_clearance_m, + config.wall_buffer_m, + config.wall_buffer_weight, + config.step_penalty_weight, &mut self.graph.wall_state, &mut self.graph.nodes, ); @@ -305,7 +314,7 @@ impl Planner { /// Rebuild all cells from surface_lookup, then nodes and edges. fn rebuild_graph(&mut self, config: &Config) { let voxel_size = config.voxel_size; - let step = (config.node_step_threshold_m / voxel_size).floor() as i32; + let step = (config.step_threshold_m / voxel_size).floor() as i32; build_surface_cells( &mut self.graph.cells, @@ -323,7 +332,8 @@ impl Planner { const SLACK_CELLS: i32 = 2; let voxel_size = config.voxel_size; let pad = (config.surface_dilation_passes + config.surface_erosion_passes) as i32; - let buffer_cells = (config.node_wall_buffer_m / voxel_size).ceil() as i32; + let buffer_cells = + ((config.wall_clearance_m + config.wall_buffer_m) / voxel_size).ceil() as i32; let spacing_cells = (config.node_spacing_m / voxel_size).ceil() as i32; let margin = pad + buffer_cells + spacing_cells + SLACK_CELLS; @@ -369,9 +379,10 @@ impl Planner { &mut self.graph.cells, config.voxel_size, config.node_spacing_m, - config.node_wall_buffer_m, - config.robot_radius_m, - config.wall_penalty_weight, + config.wall_clearance_m, + config.wall_buffer_m, + config.wall_buffer_weight, + config.step_penalty_weight, &mut self.graph.wall_state, &mut self.graph.nodes, ); @@ -478,10 +489,11 @@ mod region_tests { surface_dilation_passes: 3, surface_erosion_passes: 3, node_spacing_m: 1.0, - node_wall_buffer_m: 0.3, - node_step_threshold_m: 0.25, - robot_radius_m: 0.0, - wall_penalty_weight: 1.0, + wall_clearance_m: 0.0, + wall_buffer_m: 0.3, + wall_buffer_weight: 1.0, + step_threshold_m: 0.25, + step_penalty_weight: 0.0, goal_tolerance: 0.3, viz_publish_hz: 2.0, } @@ -816,14 +828,14 @@ mod region_tests { let goal = (1.0, 3.5, 0.05); let max_x = |w: &[(f32, f32, f32)]| w.iter().map(|p| p.0).fold(f32::MIN, f32::max); - // No floor: the shortest route slips straight through the narrow gap. - cfg.robot_radius_m = 0.0; + // No clearance: the shortest route slips straight through the narrow gap. + cfg.wall_clearance_m = 0.0; let mut open = Planner::default(); open.update_global_map(&pts, &cfg); let wp_open = open.plan(start, goal, &cfg).expect("open plan exists"); - // Floor wider than the narrow gap: it is impassable, so detour wide. - cfg.robot_radius_m = 0.2; + // Clearance wider than the narrow gap: it is impassable, so detour wide. + cfg.wall_clearance_m = 0.2; let mut safe = Planner::default(); safe.update_global_map(&pts, &cfg); let wp_safe = safe.plan(start, goal, &cfg).expect("safe plan exists"); @@ -841,4 +853,165 @@ mod region_tests { path_len(&wp_open) ); } + + /// Every cell the smoothed path crosses, between waypoints included, must + /// clear the hard wall distance. + #[test] + fn final_path_clears_wall_distance() { + let mut cfg = test_config(); + cfg.wall_clearance_m = 0.2; + cfg.wall_buffer_m = 0.5; + let all = big_world(); + let mut p = Planner::default(); + p.update_global_map(&all, &cfg); + + let wp = p + .plan((0.7, 4.0, 0.05), (7.3, 4.0, 0.05), &cfg) + .expect("plan exists"); + let clearance: std::collections::HashMap = + p.surface_clearance().into_iter().collect(); + let vs = cfg.voxel_size; + let key = |x: f32, y: f32, z: f32| { + ( + (x / vs).floor() as i32, + (y / vs).floor() as i32, + (z / vs).round() as i32 - 1, + ) + }; + + // Interior waypoints are exact cell centers; sample between them too. + let interior = &wp[1..wp.len() - 1]; + assert!(interior.len() >= 2, "expected a multi-cell path"); + for pair in interior.windows(2) { + let (a, b) = (pair[0], pair[1]); + for k in 0..=24 { + let t = k as f32 / 24.0; + let x = a.0 + t * (b.0 - a.0); + let y = a.1 + t * (b.1 - a.1); + let z = a.2 + t * (b.2 - a.2); + if let Some(&c) = clearance.get(&key(x, y, z)) { + assert!( + c >= cfg.wall_clearance_m - 1e-4, + "path point ({x:.2},{y:.2}) sits {c:.3} from a wall, under the {} clearance", + cfg.wall_clearance_m + ); + } + } + } + } + + /// Solid 0.3 m block, taller than the step threshold; the path must route + /// around it and never climb on. + fn block_world() -> Vec<(f32, f32, f32)> { + let vs = 0.1_f32; + let half = vs * 0.5; + let mut pts = Vec::new(); + for ix in 0..40 { + for iy in 0..12 { + pts.push((ix as f32 * vs + half, iy as f32 * vs + half, half)); + } + } + // A solid block, 0.3 m tall, blocking the iy 0..6 lane around ix 18..22. + for ix in 18..22 { + for iy in 0..6 { + for iz in 0..4 { + pts.push(( + ix as f32 * vs + half, + iy as f32 * vs + half, + iz as f32 * vs + half, + )); + } + } + } + pts + } + + #[test] + fn final_path_never_climbs_over_threshold_step() { + let mut cfg = test_config(); + cfg.surface_dilation_passes = 0; + cfg.surface_erosion_passes = 0; + cfg.wall_clearance_m = 0.0; + cfg.wall_buffer_m = 0.0; + cfg.node_spacing_m = 0.5; + let pts = block_world(); + let mut p = Planner::default(); + p.update_global_map(&pts, &cfg); + + let wp = p + .plan((1.0, 0.5, 0.05), (3.9, 0.5, 0.05), &cfg) + .expect("plan exists"); + + // The block top is at z = 0.4; the floor surface point is z = 0.1. No + // interior waypoint may land on the block. + for w in &wp[1..wp.len() - 1] { + assert!( + w.2 < 0.25, + "path climbed onto the 0.3 m block at {w:?}, exceeding the step threshold" + ); + } + // It had to detour out of the blocked lane (iy < 0.6). + let max_y = wp.iter().map(|p| p.1).fold(f32::MIN, f32::max); + assert!( + max_y > 0.6, + "path did not detour around the block: max_y={max_y}" + ); + } + + /// Flat floor with a crossable 0.2 m ridge blocking ix 15 except a flat gap + /// at iy 10..12. Crossing is short but climbs two steps; the detour is flat. + /// Route choice is read from the xy lane, since smoothing flattens the ridge + /// waypoints away. + fn ridge_world() -> Vec<(f32, f32, f32)> { + let vs = 0.1_f32; + let half = vs * 0.5; + let mut pts = Vec::new(); + for ix in 0..40 { + for iy in 0..12 { + pts.push((ix as f32 * vs + half, iy as f32 * vs + half, half)); + } + } + // A 0.2 m ridge cap at ix 15, iy 0..10: a 2-cell step up and back down. + for iy in 0..10 { + pts.push((15.0 * vs + half, iy as f32 * vs + half, 2.0 * vs + half)); + } + pts + } + + #[test] + fn step_penalty_diverts_path_around_ridge() { + let mut cfg = test_config(); + cfg.surface_dilation_passes = 0; + cfg.surface_erosion_passes = 0; + cfg.wall_clearance_m = 0.0; + cfg.wall_buffer_m = 0.0; + cfg.node_spacing_m = 0.5; + let pts = ridge_world(); + let start = (1.0, 0.5, 0.05); + let goal = (2.9, 0.5, 0.05); + let max_y = |w: &[(f32, f32, f32)]| w.iter().map(|p| p.1).fold(f32::MIN, f32::max); + + // No step penalty: the short route crosses the ridge low. + cfg.step_penalty_weight = 0.0; + let mut cheap = Planner::default(); + cheap.update_global_map(&pts, &cfg); + let wp_cheap = cheap.plan(start, goal, &cfg).expect("plan exists"); + + // Heavy step penalty: the flat detour to the iy 10 gap wins. + cfg.step_penalty_weight = 30.0; + let mut avoid = Planner::default(); + avoid.update_global_map(&pts, &cfg); + let wp_avoid = avoid.plan(start, goal, &cfg).expect("plan exists"); + + assert!( + max_y(&wp_cheap) < 0.6, + "with no step penalty the path should cross the ridge low: max_y={}", + max_y(&wp_cheap) + ); + assert!( + max_y(&wp_avoid) > 0.9, + "with a heavy step penalty the path should detour to the flat gap: max_y={}", + max_y(&wp_avoid) + ); + } } diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/nodes.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/nodes.rs index 3b5518f9f6..dad28dd1a6 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/nodes.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/nodes.rs @@ -27,9 +27,10 @@ pub fn place_nodes( cells: &mut SurfaceCells, voxel_size: f32, node_spacing_m: f32, - node_wall_buffer_m: f32, - robot_radius_m: f32, - wall_penalty_weight: f32, + wall_clearance_m: f32, + wall_buffer_m: f32, + wall_buffer_weight: f32, + step_penalty_weight: f32, state: &mut DijkstraState, out_nodes: &mut Vec, ) { @@ -42,7 +43,8 @@ pub fn place_nodes( collect_wall_adjacent_cells(cells, &mut wall_seeds); dijkstra(cells, &wall_seeds, state, Weight::Base); - let node_floor = node_wall_buffer_m.max(robot_radius_m); + // Floor is the hard clearance; NMS already prefers the clearest cells. + let node_floor = wall_clearance_m; let candidates: Vec = cells .ids() .filter(|&id| state.dist[id as usize] >= node_floor) @@ -60,9 +62,10 @@ pub fn place_nodes( apply_wall_safe_penalty( cells, &state.dist, - node_wall_buffer_m, - robot_radius_m, - wall_penalty_weight, + wall_clearance_m, + wall_buffer_m, + wall_buffer_weight, + step_penalty_weight, ); } @@ -102,9 +105,10 @@ pub fn place_nodes_region( window: &AHashSet, voxel_size: f32, node_spacing_m: f32, - node_wall_buffer_m: f32, - robot_radius_m: f32, - wall_penalty_weight: f32, + wall_clearance_m: f32, + wall_buffer_m: f32, + wall_buffer_weight: f32, + step_penalty_weight: f32, wall_state: &mut DijkstraState, nodes: &mut Vec, ) { @@ -115,7 +119,7 @@ pub fn place_nodes_region( nodes.retain(|n| cells.is_live(n.cell_id) && !window.contains(&n.cell_id)); let kept: Vec = nodes.iter().map(|n| n.cell_id).collect(); - let node_floor = node_wall_buffer_m.max(robot_radius_m); + let node_floor = wall_clearance_m; let candidates: Vec = window .iter() .copied() @@ -134,9 +138,10 @@ pub fn place_nodes_region( apply_wall_safe_penalty_region( cells, &wall_state.dist, - node_wall_buffer_m, - robot_radius_m, - wall_penalty_weight, + wall_clearance_m, + wall_buffer_m, + wall_buffer_weight, + step_penalty_weight, window, ); } @@ -179,9 +184,10 @@ fn is_wall_adjacent(cells: &SurfaceCells, id: CellId) -> bool { fn apply_wall_safe_penalty_region( cells: &mut SurfaceCells, dist: &[f32], + clearance_m: f32, buffer_m: f32, - robot_radius_m: f32, - weight: f32, + buffer_weight: f32, + step_weight: f32, window: &AHashSet, ) { let mut affected: AHashSet = AHashSet::with_capacity(window.len() * 2); @@ -196,9 +202,10 @@ fn apply_wall_safe_penalty_region( cells.edges_mut(id), id, dist, + clearance_m, buffer_m, - robot_radius_m, - weight, + buffer_weight, + step_weight, ); } } @@ -276,19 +283,27 @@ fn nms_grid( survivors } -/// Scale every edge cost by the average of its endpoint penalties, which -/// pushes shortest paths away from walls and forbids sub-radius cells. -/// Unreached cells have dist == +INFINITY which collapses to penalty 1.0. +/// Scale each edge by its endpoints' average wall penalty and add the step +/// penalty. Unreached cells (dist +INFINITY) collapse the wall penalty to 1.0. fn apply_wall_safe_penalty( cells: &mut SurfaceCells, dist: &[f32], + clearance_m: f32, buffer_m: f32, - robot_radius_m: f32, - weight: f32, + buffer_weight: f32, + step_weight: f32, ) { let mut edge_lists: Vec<(CellId, &mut Vec)> = cells.iter_edges_mut().collect(); edge_lists.par_iter_mut().for_each(|(src, edges)| { - scale_edges(edges, *src, dist, buffer_m, robot_radius_m, weight); + scale_edges( + edges, + *src, + dist, + clearance_m, + buffer_m, + buffer_weight, + step_weight, + ); }); } @@ -299,26 +314,38 @@ fn scale_edges( edges: &mut [Edge], src: CellId, dist: &[f32], + clearance_m: f32, buffer_m: f32, - robot_radius_m: f32, - weight: f32, + buffer_weight: f32, + step_weight: f32, ) { - let pu = penalty_of(dist[src as usize], buffer_m, robot_radius_m, weight); + let pu = penalty_of(dist[src as usize], clearance_m, buffer_m, buffer_weight); for edge in edges.iter_mut() { - let pv = penalty_of(dist[edge.dest as usize], buffer_m, robot_radius_m, weight); - edge.cost = edge.base_cost * (pu + pv) / 2.0; + let pv = penalty_of( + dist[edge.dest as usize], + clearance_m, + buffer_m, + buffer_weight, + ); + edge.cost = edge.base_cost * (pu + pv) / 2.0 + step_weight * edge.rise; } } -/// Cost multiplier at wall distance d. Infinite inside the robot radius, -/// then decays from 1 + weight toward 1 with length scale buffer_m. +/// Lateral wall multiplier at wall distance d. Infinite inside the clearance, +/// then 1 + weight at the clearance edge decaying convexly to 1 at +/// clearance_m + buffer_m, and 1 beyond. #[inline] -pub(crate) fn penalty_of(d: f32, buffer_m: f32, robot_radius_m: f32, weight: f32) -> f32 { - if d < robot_radius_m { +pub(crate) fn penalty_of(d: f32, clearance_m: f32, buffer_m: f32, weight: f32) -> f32 { + if d < clearance_m { return f32::INFINITY; } - let scale = buffer_m.max(1e-3); - 1.0 + weight * (-(d - robot_radius_m) / scale).exp() + let outer = clearance_m + buffer_m; + if d >= outer { + return 1.0; + } + let band = buffer_m.max(1e-3); + let t = (outer - d) / band; // 0 at the outer edge, 1 at the clearance edge + 1.0 + weight * t * t } #[cfg(test)] @@ -351,7 +378,9 @@ mod tests { let mut sc = build_cells(&open_patch(0, 0, 10), 2); let mut state = DijkstraState::default(); let mut nodes = Vec::new(); - place_nodes(&mut sc, VOXEL, 1.0, 0.3, 0.0, 1.0, &mut state, &mut nodes); + place_nodes( + &mut sc, VOXEL, 1.0, 0.0, 0.3, 1.0, 0.0, &mut state, &mut nodes, + ); assert!(!nodes.is_empty()); for n in &nodes { let (ix, iy, _) = sc.coord(n.cell_id); @@ -370,7 +399,9 @@ mod tests { let mut sc = build_cells(&cells_in, 2); let mut state = DijkstraState::default(); let mut nodes = Vec::new(); - place_nodes(&mut sc, VOXEL, 1.0, 0.3, 0.0, 1.0, &mut state, &mut nodes); + place_nodes( + &mut sc, VOXEL, 1.0, 0.0, 0.3, 1.0, 0.0, &mut state, &mut nodes, + ); assert!(!nodes.is_empty()); } @@ -381,7 +412,9 @@ mod tests { let mut sc = build_cells(&cells_in, 2); let mut state = DijkstraState::default(); let mut nodes = Vec::new(); - place_nodes(&mut sc, VOXEL, 1.0, 0.3, 0.0, 1.0, &mut state, &mut nodes); + place_nodes( + &mut sc, VOXEL, 1.0, 0.0, 0.3, 1.0, 0.0, &mut state, &mut nodes, + ); assert!(nodes.len() >= 2); for i in 0..nodes.len() { for j in (i + 1)..nodes.len() { @@ -397,30 +430,62 @@ mod tests { } #[test] - fn wall_penalty_weight_scales_edge_costs() { - // On a 1-wide strip every cell is wall-adjacent, so the penalty - // multiplier is exactly 1 + weight and edge cost is base times it. + fn penalty_ramps_across_buffer_zone() { + // clearance 0.1, soft zone 0.4 wide, so the outer edge is at 0.5. + let (clearance, buffer, w) = (0.1, 0.4, 4.0); + assert!(penalty_of(0.05, clearance, buffer, w).is_infinite()); + assert!((penalty_of(0.1, clearance, buffer, w) - 5.0).abs() < 1e-6); + assert!((penalty_of(0.5, clearance, buffer, w) - 1.0).abs() < 1e-6); + assert!((penalty_of(1.0, clearance, buffer, w) - 1.0).abs() < 1e-6); + assert!((penalty_of(0.3, clearance, buffer, w) - 2.0).abs() < 1e-6); + } + + #[test] + fn wall_penalty_doubles_cost_at_the_wall() { + // On a 1-wide strip every cell is wall-adjacent (d = 0), so with zero + // clearance the ramp peaks at 2 and edge cost is twice the geometric. let cells_in: Vec = (0..10).map(|ix| (ix, 0, 0)).collect(); - let cost_with = |weight: f32| { + let mut sc = build_cells(&cells_in, 2); + let mut state = DijkstraState::default(); + let mut nodes = Vec::new(); + place_nodes( + &mut sc, VOXEL, 1.0, 0.0, 0.3, 1.0, 0.0, &mut state, &mut nodes, + ); + let id = sc.id((5, 0, 0)).unwrap(); + assert!((sc.neighbors(id)[0].cost - 2.0 * VOXEL).abs() < 1e-5); + } + + #[test] + fn step_penalty_adds_to_vertical_edges() { + // A 2-cell rise (0.2 m) between adjacent cells. With weight 10 the edge + // gains 10 * 0.2 = 2.0 on top of its geometric and wall cost. + let cells_in: Vec = vec![(0, 0, 0), (1, 0, 2), (2, 0, 2)]; + let cost_with = |step_weight: f32| { let mut sc = build_cells(&cells_in, 2); let mut state = DijkstraState::default(); let mut nodes = Vec::new(); place_nodes( - &mut sc, VOXEL, 1.0, 0.3, 0.0, weight, &mut state, &mut nodes, + &mut sc, + VOXEL, + 1.0, + 0.0, + 0.3, + 1.0, + step_weight, + &mut state, + &mut nodes, ); - let id = sc.id((5, 0, 0)).unwrap(); - sc.neighbors(id)[0].cost + let id = sc.id((0, 0, 0)).unwrap(); + sc.neighbors(id) + .iter() + .find(|e| sc.coord(e.dest) == (1, 0, 2)) + .unwrap() + .cost }; - let unweighted = cost_with(0.0); assert!( - (unweighted - VOXEL).abs() < 1e-5, - "zero weight must leave the geometric cost, got {unweighted}" + (cost_with(10.0) - cost_with(0.0) - 10.0 * 0.2).abs() < 1e-4, + "step penalty must add weight * rise" ); - assert!( - (cost_with(4.0) - 5.0 * VOXEL).abs() < 1e-5, - "weight 4 at the wall must scale cost by 5" - ); - assert!(cost_with(4.0) > cost_with(1.0)); } #[test] @@ -429,7 +494,9 @@ mod tests { let mut sc = build_cells(&cells_in, 2); let mut state = DijkstraState::default(); let mut nodes = Vec::new(); - place_nodes(&mut sc, VOXEL, 1.0, 0.3, 0.0, 1.0, &mut state, &mut nodes); + place_nodes( + &mut sc, VOXEL, 1.0, 0.0, 0.3, 1.0, 0.0, &mut state, &mut nodes, + ); let id0 = sc.id((0, 0, 0)).unwrap(); let outbound = sc.neighbors(id0); assert!(!outbound.is_empty()); diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs index c0e7d60965..d56916edf4 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs @@ -6,7 +6,7 @@ use std::collections::BinaryHeap; use ahash::{AHashMap, AHashSet}; -use crate::adjacency::{CellId, SurfaceCells, SurfaceLookup}; +use crate::adjacency::{rise, CellId, SurfaceCells, SurfaceLookup}; use crate::dijkstra::walk_preds; use crate::edges::{NodeEdgeIdx, NodeId, PlannerGraph, NO_NODE}; use crate::mls_planner::Config; @@ -16,17 +16,13 @@ use crate::voxel::{surface_point_xyz, VoxelKey}; /// Robot-rooted candidate search radius, in multiples of node spacing. const CANDIDATE_RADIUS_FACTOR: f32 = 3.0; -/// How far to search horizontally when snapping a pose to the surface. -/// A downward-pitched lidar cannot see the floor under the robot, so the -/// nearest mapped surface can start well outside the robot footprint. +/// Horizontal search radius when snapping a pose to the surface. const SNAP_SEARCH_RADIUS_M: f32 = 1.5; -/// How many nearby snap candidates to try when connecting the start. +/// Max snap candidates tried when connecting the start. const MAX_SNAP_ATTEMPTS: usize = 64; /// Surface cells near the pose, nearest first in xy. -/// The nearest cell can sit on a small fragment disconnected from the main -/// surface, so callers that need connectivity should try candidates in order. pub fn snap_candidates( surface_lookup: &SurfaceLookup, pose: (f32, f32, f32), @@ -168,17 +164,17 @@ pub fn plan( return None; }; - // Shortcut height tolerance in cells, tied to the traversable step. - let smooth_tol_cells = ((config.node_step_threshold_m / voxel_size).round() as i32).max(1); + // Max traversable step in cells, floored to match the graph adjacency. + let step_cells = (config.step_threshold_m / voxel_size).floor() as i32; let wall_cost = WallCost { - robot_radius_m: config.robot_radius_m, - buffer_m: config.node_wall_buffer_m, - weight: config.wall_penalty_weight, + clearance_m: config.wall_clearance_m, + buffer_m: config.wall_buffer_m, + buffer_weight: config.wall_buffer_weight, voxel_size, }; let cells = assemble_cells(plg, &node_seq, &lead_in, &goal_segment); - let cells = string_pull(plg, &cells, smooth_tol_cells, &wall_cost); + let cells = string_pull(plg, &cells, step_cells, &wall_cost); Some(cells_to_waypoints( plg, &cells, start_pose, goal_pose, voxel_size, )) @@ -393,48 +389,51 @@ fn cells_to_waypoints( waypoints } -/// Wall-penalty cost model for smoothing, mirroring the node graph's. +/// Clearance and step limits the smoother holds the path to. struct WallCost { - robot_radius_m: f32, + clearance_m: f32, buffer_m: f32, - weight: f32, + buffer_weight: f32, voxel_size: f32, } -/// Shortcut runs of cells with straight on-surface segments. A shortcut is only -/// taken when its wall-penalty-weighted cost is no worse than the rough sub-path -/// it replaces, so smoothing never trades away the clearance the node graph -/// earned: it straightens in the open but keeps the centered route near walls. -fn string_pull(plg: &PlannerGraph, cells: &[CellId], tol_cells: i32, wc: &WallCost) -> Vec { +/// Replace runs of cells with straight chords that come no closer to a wall and +/// climb no more than the run they replace. +fn string_pull( + plg: &PlannerGraph, + cells: &[CellId], + step_cells: i32, + wc: &WallCost, +) -> Vec { if cells.len() <= 2 { return cells.to_vec(); } - let cost = |from: CellId, to: CellId| { - segment_cost( + let metrics = |from: CellId, to: CellId| { + segment_metrics( plg, plg.cells.coord(from), plg.cells.coord(to), - tol_cells, + step_cells, wc, ) }; let mut out = vec![cells[0]]; let mut anchor = 0; while anchor + 1 < cells.len() { - // Farthest cell reachable by a straight segment that is at least as safe - // (penalty-weighted) as the rough path between anchor and that cell. let mut best = anchor + 1; - let mut rough_cost = 0.0_f32; + let mut rough_pen = 1.0_f32; + let mut rough_rise = 0.0_f32; let mut j = anchor + 1; while j < cells.len() { - rough_cost += cost(cells[j - 1], cells[j]).unwrap_or(f32::INFINITY); - match cost(cells[anchor], cells[j]) { - // Straight segment is no costlier than the rough path it spans. - Some(shortcut) if shortcut <= rough_cost + 1e-3 => best = j, - // Segment leaves the surface or enters the robot radius — stop. - None => break, - // Valid but less safe than the rough path; keep scanning farther. - _ => {} + if let Some((pen, rise)) = metrics(cells[j - 1], cells[j]) { + rough_pen = rough_pen.max(pen); + rough_rise += rise; + } + match metrics(cells[anchor], cells[j]) { + Some((pen, rise)) if pen <= rough_pen + 1e-3 && rise <= rough_rise + 1e-3 => { + best = j + } + _ => break, } j += 1; } @@ -444,26 +443,25 @@ fn string_pull(plg: &PlannerGraph, cells: &[CellId], tol_cells: i32, wc: &WallCo out } -/// Wall-penalty-weighted length of the straight on-surface segment a -> b, -/// matching the node graph's cost model (geometric length times the average -/// `penalty_of` along it). None if the segment leaves the surface, jumps more -/// than tol_cells in height, or passes within robot_radius_m of a wall. -fn segment_cost( +/// Worst wall penalty and total climb along the straight segment a -> b. None if +/// it leaves the surface, exceeds step_cells, or enters the hard clearance. +fn segment_metrics( plg: &PlannerGraph, a: VoxelKey, b: VoxelKey, - tol_cells: i32, + step_cells: i32, wc: &WallCost, -) -> Option { +) -> Option<(f32, f32)> { let (dx, dy, dz) = (b.0 - a.0, b.1 - a.1, b.2 - a.2); let samples = dx.abs().max(dy.abs()) * 2; if samples == 0 { - return Some(0.0); + // A same-column vertical chord is not traversable. + return (dz == 0).then_some((1.0, 0.0)); } - let length_m = (((dx * dx + dy * dy) as f32).sqrt()) * wc.voxel_size; let (mut last_ix, mut last_iy) = (i32::MIN, i32::MIN); - let mut penalty_sum = 0.0_f32; - let mut counted = 0u32; + let mut prev_iz: Option = None; + let mut max_pen = 1.0_f32; + let mut rise_cells = 0i32; for k in 0..=samples { let t = k as f32 / samples as f32; let ix = (a.0 as f32 + t * dx as f32).round() as i32; @@ -484,10 +482,19 @@ fn segment_cost( } } let (d, iz) = nearest?; - if d > tol_cells as f32 { + if d > step_cells as f32 { return None; } - // Columns on the surface but not in the graph carry no penalty. + // Tally climb and reject an untraversable step between columns. + if let Some(p) = prev_iz { + let step = (iz - p).abs(); + if step > step_cells { + return None; + } + rise_cells += step; + } + prev_iz = Some(iz); + // Columns on the surface but not in the graph carry no wall penalty. let p = match plg.cells.id((ix, iy, iz)) { Some(id) => { let wall_dist = plg @@ -496,17 +503,16 @@ fn segment_cost( .get(id as usize) .copied() .unwrap_or(f32::INFINITY); - penalty_of(wall_dist, wc.buffer_m, wc.robot_radius_m, wc.weight) + penalty_of(wall_dist, wc.clearance_m, wc.buffer_m, wc.buffer_weight) } None => 1.0, }; if !p.is_finite() { return None; } - penalty_sum += p; - counted += 1; + max_pen = max_pen.max(p); } - Some(length_m * penalty_sum / counted.max(1) as f32) + Some((max_pen, rise(rise_cells, wc.voxel_size))) } fn edge_between(plg: &PlannerGraph, a: NodeId, b: NodeId) -> Option { @@ -589,10 +595,11 @@ mod tests { surface_dilation_passes: 0, surface_erosion_passes: 0, node_spacing_m: 1.0, - node_wall_buffer_m: 0.3, - node_step_threshold_m: 0.25, - robot_radius_m: 0.2, - wall_penalty_weight: 4.0, + wall_clearance_m: 0.2, + wall_buffer_m: 0.5, + wall_buffer_weight: 4.0, + step_threshold_m: 0.25, + step_penalty_weight: 4.0, goal_tolerance: 0.3, viz_publish_hz: 2.0, }; @@ -693,7 +700,7 @@ mod tests { let wp = plan_simple(&plg, (0.2, 0.0, 0.05), (1.9, 0.0, 0.05)).unwrap(); // Smoothed waypoints are no longer cell-adjacent, but each segment // between them must still stay on the surface. - let tol = ((0.25f32 / VOXEL).round() as i32).max(1); + let step_cells = (0.25f32 / VOXEL).floor() as i32; for w in &wp[1..wp.len() - 1] { assert!( plg.cells.id(waypoint_key(w)).is_some(), @@ -701,18 +708,18 @@ mod tests { ); } let wc = WallCost { - robot_radius_m: 0.2, - buffer_m: 0.3, - weight: 4.0, + clearance_m: 0.2, + buffer_m: 0.5, + buffer_weight: 4.0, voxel_size: VOXEL, }; for pair in wp[1..wp.len() - 1].windows(2) { assert!( - segment_cost( + segment_metrics( &plg, waypoint_key(&pair[0]), waypoint_key(&pair[1]), - tol, + step_cells, &wc ) .is_some(), @@ -743,9 +750,25 @@ mod tests { } #[test] - fn string_pull_refuses_shortcut_through_sub_buffer_cell() { + fn segment_metrics_rejects_vertical_chord() { + let plg = PlannerGraph::new(); + let wc = WallCost { + clearance_m: 0.2, + buffer_m: 0.3, + buffer_weight: 4.0, + voxel_size: VOXEL, + }; + assert!(segment_metrics(&plg, (5, 5, 0), (5, 5, 4), 2, &wc).is_none()); + assert_eq!( + segment_metrics(&plg, (5, 5, 0), (5, 5, 0), 2, &wc), + Some((1.0, 0.0)) + ); + } + + #[test] + fn string_pull_refuses_shortcut_through_sub_clearance_cell() { // Straight strip: with open clearance the run collapses to its - // endpoints. Drop one mid cell below the buffer and the shortcut + // endpoints. Drop one mid cell below the hard clearance and the shortcut // spanning it is refused, so the smoothed path retains that cell. let mut plg = PlannerGraph::new(); build_surface_lookup(&strip(10), &mut plg.surface_lookup); @@ -753,9 +776,9 @@ mod tests { let path: Vec = (0..10).map(|x| plg.cells.id((x, 0, 0)).unwrap()).collect(); let wc = WallCost { - robot_radius_m: 0.2, - buffer_m: 0.3, - weight: 4.0, + clearance_m: 0.2, + buffer_m: 0.5, + buffer_weight: 4.0, voxel_size: VOXEL, }; plg.wall_state.dist = vec![f32::INFINITY; plg.cells.slot_capacity()]; @@ -763,11 +786,11 @@ mod tests { assert_eq!(open.len(), 2, "open strip should collapse to its endpoints"); let mid = plg.cells.id((5, 0, 0)).unwrap(); - plg.wall_state.dist[mid as usize] = 0.1; + plg.wall_state.dist[mid as usize] = 0.1; // below the 0.2 clearance let guarded = string_pull(&plg, &path, 1, &wc); assert!( guarded.len() > 2, - "shortcut across a sub-buffer cell must be refused: {guarded:?}" + "shortcut across a sub-clearance cell must be refused: {guarded:?}" ); assert!( guarded.contains(&mid), diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/python.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/python.rs index 413dd68ae4..ca4e8f054b 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/python.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/python.rs @@ -29,10 +29,11 @@ impl MLSPlanner { surface_dilation_passes = 3, surface_erosion_passes = 3, node_spacing_m = 1.0, - node_wall_buffer_m = 0.3, - node_step_threshold_m = 0.25, - robot_radius_m = 0.2, - wall_penalty_weight = 4.0, + wall_clearance_m = 0.3, + wall_buffer_m = 0.75, + wall_buffer_weight = 100.0, + step_threshold_m = 0.25, + step_penalty_weight = 4.0, ))] fn new( voxel_size: f32, @@ -40,10 +41,11 @@ impl MLSPlanner { surface_dilation_passes: u32, surface_erosion_passes: u32, node_spacing_m: f32, - node_wall_buffer_m: f32, - node_step_threshold_m: f32, - robot_radius_m: f32, - wall_penalty_weight: f32, + wall_clearance_m: f32, + wall_buffer_m: f32, + wall_buffer_weight: f32, + step_threshold_m: f32, + step_penalty_weight: f32, ) -> PyResult { let config = Config { world_frame: String::new(), @@ -52,10 +54,11 @@ impl MLSPlanner { surface_dilation_passes, surface_erosion_passes, node_spacing_m, - node_wall_buffer_m, - node_step_threshold_m, - robot_radius_m, - wall_penalty_weight, + wall_clearance_m, + wall_buffer_m, + wall_buffer_weight, + step_threshold_m, + step_penalty_weight, // Only the binary's replan loop reads goal_tolerance. This // in-process binding plans on demand and never consults it. goal_tolerance: 1.0, diff --git a/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py b/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py index 70fb38ae36..b2d5324274 100644 --- a/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py +++ b/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py @@ -12,11 +12,15 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Replay a lidar+odometry .db through RayTraceMap and MLSPlan into rerun.""" +"""Replay a lidar+odometry .db through RayTraceMap and the MLS planner into rerun. + +Pass one or more --config clearance,buffer,weight to overlay each as a colored path. +""" from __future__ import annotations from pathlib import Path as FsPath +from time import perf_counter import numpy as np from numpy.typing import NDArray @@ -30,9 +34,8 @@ from dimos.memory2.transform import FnTransformer from dimos.memory2.type.observation import Observation from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.msgs.nav_msgs.Path import Path from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2, register_colormap_annotation -from dimos.navigation.nav_3d.mls_planner.transformer import MLSPlan +from dimos.navigation.nav_3d.mls_planner.mls_planner import MLSPlanner from dimos.utils.data import resolve_named_path TIMELINE = "ts" @@ -40,6 +43,37 @@ TIMING_KEYS = ["update_ms", "plan_ms", "total_ms"] SIZE_KEYS = ["voxels", "surface_cells", "nodes", "edges"] +# Distinct path colors for overlaid configurations, config 0 first. +PATH_PALETTE = [ + [0, 255, 0], + [255, 0, 255], + [0, 200, 255], + [255, 180, 0], + [255, 80, 80], + [160, 120, 255], + [120, 255, 200], + [255, 255, 120], +] + + +def _parse_configs( + specs: list[str] | None, + clearance: float, + buffer: float, + weight: float, +) -> list[tuple[float, float, float]]: + """Each spec is 'clearance,buffer,weight'. Falls back to the single flags.""" + if not specs: + return [(clearance, buffer, weight)] + out: list[tuple[float, float, float]] = [] + for spec in specs: + parts = spec.replace(" ", "").split(",") + if len(parts) != 3: + raise typer.BadParameter(f"--config must be 'clearance,buffer,weight'; got {spec!r}") + c, b, w = (float(p) for p in parts) + out.append((c, b, w)) + return out + def _print_summary(streams: dict[str, dict[str, Stream[float]]]) -> None: print("\nper-frame summary (mean / p50 / p95 / max):") @@ -87,12 +121,12 @@ def _log_edges(edges: NDArray[np.float32], entity: str) -> None: rr.log(entity, rr.LineStrips3D(segments)) -def _log_path(path: Path, entity: str) -> None: - if not path.poses: +def _log_path_wp(waypoints: NDArray[np.float32] | None, entity: str, color: list[int]) -> None: + if waypoints is None or len(waypoints) == 0: rr.log(entity, rr.LineStrips3D([])) return - points = [(float(p.position.x), float(p.position.y), float(p.position.z)) for p in path.poses] - rr.log(entity, rr.LineStrips3D([points], colors=[[0, 255, 0]], radii=0.05)) + points = [(float(p[0]), float(p[1]), float(p[2])) for p in waypoints] + rr.log(entity, rr.LineStrips3D([points], colors=[color], radii=0.05)) def _clearance_colors(clearance: NDArray[np.float32], clamp_m: float) -> NDArray[np.uint8]: @@ -104,26 +138,26 @@ def _clearance_colors(clearance: NDArray[np.float32], clamp_m: float) -> NDArray return rgb.astype(np.uint8) -def _log_frame( - obs: Observation[Path], +def _log_shared( + start: tuple[float, float, float], + planner: MLSPlanner, render_voxel: float, clearance_clamp: float, - timing_streams: dict[str, Stream[float]], - size_streams: dict[str, Stream[float]], -) -> None: - rr.set_time(TIMELINE, timestamp=obs.ts) +) -> tuple[NDArray[np.float32], NDArray[np.float32], NDArray[np.float32]]: + """Log the map artifacts shared by every config from a reference planner. - start = obs.tags["start"] + Returns (surface, nodes, edges) for metric sizing. + """ rr.log("world/start", rr.Points3D([start], colors=[[0, 255, 0]], radii=0.1)) - voxel_map = obs.tags["voxel_map"] + voxel_map = planner.voxel_map() if voxel_map.size: rr.log( "world/voxel_map", rr.Points3D(voxel_map, colors=[[180, 125, 125]], radii=render_voxel / 2), ) - surface = obs.tags["surface_clearance"] + surface = planner.surface_clearance_map() if surface.size: rr.log( "world/surface_map", @@ -134,38 +168,13 @@ def _log_frame( ), ) - nodes = obs.tags["nodes"] + nodes = planner.nodes() if nodes.size: rr.log("world/nodes", rr.Points3D(nodes, colors=[[255, 200, 0]], radii=0.05)) - edges = obs.tags["node_edges"] + edges = planner.node_edges() _log_edges(edges, "world/node_edges") - _log_path(obs.data, "world/path") - - timings = obs.tags["timings"] - sizes = { - "voxels": obs.tags["voxels"], - "surface_cells": len(surface), - "nodes": len(nodes), - "edges": len(edges), - } - for key, value in timings.items(): - timing_streams[key].append(float(value), ts=obs.ts) - rr.log(f"metrics/timing/{key}", rr.Scalars(value)) - for key, value in sizes.items(): - size_streams[key].append(float(value), ts=obs.ts) - rr.log(f"metrics/size/{key}", rr.Scalars(value)) - - count = obs.tags.get("frame_count", "?") - planned = obs.tags.get("planned", False) - print( - f"frame_count={count} planned={planned} " - f"waypoints={len(obs.data.poses)} " - f"rebuild={timings['total_ms'] - timings['plan_ms']:.1f}ms " - f"plan={timings['plan_ms']:.1f}ms", - end="\r", - flush=True, - ) + return surface, nodes, edges def main( @@ -186,16 +195,25 @@ def main( emit_every: int = typer.Option(1, "--emit-every", help="Replan every N lidar frames"), robot_height: float = typer.Option(1.0, "--robot-height", help="Robot height (m)"), node_spacing: float = typer.Option(1.0, "--node-spacing", help="Graph node spacing (m)"), - node_wall_buffer: float = typer.Option( - 0.3, "--node-wall-buffer", help="Min wall clearance for nodes and smoothing (m)" + wall_clearance: float = typer.Option( + 0.3, + "--wall-clearance", + help="Hard clearance; cells closer to a wall or edge are impassable (m)", + ), + wall_buffer: float = typer.Option( + 0.75, "--wall-buffer", help="Width of the soft standoff zone beyond clearance (m)" ), - robot_radius: float = typer.Option( - 0.2, - "--robot-radius", - help="Hard clearance floor; cells closer to a wall are impassable (m)", + wall_buffer_weight: float = typer.Option( + 100.0, "--wall-buffer-weight", help="Peak soft wall penalty at the clearance edge" ), - wall_penalty_weight: float = typer.Option( - 4.0, "--wall-penalty-weight", help="Soft wall-penalty strength at the robot radius" + step_penalty_weight: float = typer.Option( + 4.0, "--step-penalty-weight", help="Soft cost per meter of vertical climb" + ), + config: list[str] = typer.Option( + None, + "--config", + help="Repeatable 'clearance,buffer,weight' to overlay as colored paths; " + "overrides the single --wall-* flags", ), goal: tuple[float, float, float] = typer.Option( (0.0, 0.0, 0.0), "--goal", help="Planner goal xyz; override per recording" @@ -239,7 +257,7 @@ def main( pose_tagged = lidar.align(odom, tolerance=align_tol).transform( FnTransformer(_attach_pose_from_odom) ) - pipeline = pose_tagged.transform( + ray_pipeline = pose_tagged.transform( RayTraceMap( voxel_size=voxel_size, max_range=max_range, @@ -247,17 +265,24 @@ def main( emit_every=emit_every, emit_local=True, ) - ).transform( - MLSPlan( - goal=goal, + ) + + configs = _parse_configs(config, wall_clearance, wall_buffer, wall_buffer_weight) + planners: list[tuple[str, list[int], MLSPlanner]] = [] + for i, (clr, buf, wgt) in enumerate(configs): + planner = MLSPlanner( voxel_size=voxel_size, robot_height=robot_height, node_spacing_m=node_spacing, - node_wall_buffer_m=node_wall_buffer, - robot_radius_m=robot_radius, - wall_penalty_weight=wall_penalty_weight, + wall_clearance_m=clr, + wall_buffer_m=buf, + wall_buffer_weight=wgt, + step_penalty_weight=step_penalty_weight, ) - ) + color = PATH_PALETTE[i % len(PATH_PALETTE)] + label = f"cfg{i}_c{clr:g}_b{buf:g}_w{wgt:g}" + planners.append((label, color, planner)) + print(f"config {i}: clearance={clr} buffer={buf} weight={wgt} color={color} -> {label}") rr.log("world/goal", rr.Points3D([goal], colors=[[255, 0, 0]], radii=0.1), static=True) @@ -266,8 +291,59 @@ def main( size_streams = {k: metrics.stream(f"size_{k}", float) for k in SIZE_KEYS} try: - for obs in pipeline: - _log_frame(obs, render_voxel, clearance_clamp, timing_streams, size_streams) + frame = 0 + for ray_obs in ray_pipeline: + if ray_obs.pose_tuple is None: + continue + bounds = ray_obs.tags.get("region_bounds") + if bounds is None: + raise ValueError("plan_rrd needs RayTraceMap(emit_local=True)") + px, py, pz, *_ = ray_obs.pose_tuple + start = (float(px), float(py), float(pz) - robot_height) + ox, oy, radius, z_min, z_max = bounds + pts = ray_obs.data.points_f32() + rr.set_time(TIMELINE, timestamp=ray_obs.ts) + + ref_timing: dict[str, float] = {} + surface = nodes = edges = np.empty((0,), dtype=np.float32) + for j, (label, color, planner) in enumerate(planners): + t0 = perf_counter() + planner.update_region(pts, (ox, oy), radius, z_min, z_max) + t1 = perf_counter() + waypoints = planner.plan(start, goal) + t2 = perf_counter() + _log_path_wp(waypoints, f"world/paths/{label}", color) + if j == 0: + ref_timing = { + "update_ms": (t1 - t0) * 1000, + "plan_ms": (t2 - t1) * 1000, + "total_ms": (t2 - t0) * 1000, + } + surface, nodes, edges = _log_shared( + start, planner, render_voxel, clearance_clamp + ) + + for key, value in ref_timing.items(): + timing_streams[key].append(float(value), ts=ray_obs.ts) + rr.log(f"metrics/timing/{key}", rr.Scalars(value)) + sizes = { + "voxels": planners[0][2].voxel_count(), + "surface_cells": len(surface), + "nodes": len(nodes), + "edges": len(edges), + } + for key, value in sizes.items(): + size_streams[key].append(float(value), ts=ray_obs.ts) + rr.log(f"metrics/size/{key}", rr.Scalars(value)) + + frame += 1 + print( + f"frame={frame} configs={len(planners)} " + f"rebuild(ref)={ref_timing['total_ms'] - ref_timing['plan_ms']:.1f}ms " + f"plan(ref)={ref_timing['plan_ms']:.1f}ms", + end="\r", + flush=True, + ) except KeyboardInterrupt: print("\ninterrupted; reporting metrics for completed frames") finally: diff --git a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py index ad6ede5ac4..3be82ce12e 100644 --- a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py +++ b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py @@ -114,13 +114,11 @@ def _static_robot_body(rr: Any) -> list[Any]: world_frame="odom", voxel_size=voxel_size, robot_height=go2_lidar_height, - # Hard wall clearance radius and node-placement wall buffer, lowered so - # nodes can sit on the narrow stair treads. - robot_radius_m=0.2, - node_wall_buffer_m=0.15, - # Max step the planner will connect across (robot step height). - node_step_threshold_m=0.25, - wall_penalty_weight=8.0, + wall_clearance_m=0.3, + wall_buffer_m=0.75, + wall_buffer_weight=100.0, + step_threshold_m=0.25, + step_penalty_weight=1.0, viz_publish_hz=0.0, ).remappings([(MLSPlannerNative, "global_map", "global_map_unused")]), GoalRelay.blueprint(), From c3df161f2ef848f73ff9b9cafb62edba739fd1d2 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Wed, 17 Jun 2026 13:58:47 -0700 Subject: [PATCH 53/56] Improved follower --- .../navigation/basic_path_follower/module.py | 23 +++++++++++--- .../basic_path_follower/test_module.py | 31 +++++++++++++++++++ .../navigation/unitree_go2_nav_3d.py | 4 +-- 3 files changed, 51 insertions(+), 7 deletions(-) create mode 100644 dimos/navigation/basic_path_follower/test_module.py diff --git a/dimos/navigation/basic_path_follower/module.py b/dimos/navigation/basic_path_follower/module.py index e1ee012f2a..c0cbb44ed9 100644 --- a/dimos/navigation/basic_path_follower/module.py +++ b/dimos/navigation/basic_path_follower/module.py @@ -42,11 +42,19 @@ class BasicPathFollowerConfig(ModuleConfig): speed: float = 0.5 control_frequency: float = 10.0 goal_tolerance: float = 0.3 - lookahead_m: float = 0.6 + # Lookahead grows with speed (time_s * speed) within these bounds, so the + # robot tracks tightly when slow and stays stable when fast. + lookahead_time_s: float = 1.5 + min_lookahead_m: float = 0.4 + max_lookahead_m: float = 1.5 heading_gain: float = 1.5 max_angular: float = 1.0 +def lookahead_distance(speed: float, time_s: float, lo: float, hi: float) -> float: + return min(hi, max(lo, time_s * speed)) + + class BasicPathFollower(Module): """Follow a planned path by chasing a lookahead point with P-controlled heading. @@ -208,11 +216,16 @@ def _lookahead_point(self, waypoints: np.ndarray, position: np.ndarray) -> np.nd if len(waypoints) == 1: return np.asarray(waypoints[0]) - # Project onto the path, then march lookahead_m along it and interpolate. - # Returning an actual waypoint would let the target jump to a distant - # vertex and cut the corner. + # Project onto the path, then march the lookahead along it and + # interpolate. Returning an actual waypoint would let the target jump to + # a distant vertex and cut the corner. seg_idx, start = self._project_onto_path(waypoints, position) - remaining = self.config.lookahead_m + remaining = lookahead_distance( + self.config.speed, + self.config.lookahead_time_s, + self.config.min_lookahead_m, + self.config.max_lookahead_m, + ) for i in range(seg_idx, len(waypoints) - 1): end = waypoints[i + 1] seg = end - start diff --git a/dimos/navigation/basic_path_follower/test_module.py b/dimos/navigation/basic_path_follower/test_module.py new file mode 100644 index 0000000000..a5ad8d5782 --- /dev/null +++ b/dimos/navigation/basic_path_follower/test_module.py @@ -0,0 +1,31 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from dimos.navigation.basic_path_follower.module import lookahead_distance + + +def test_lookahead_floor_at_low_speed(): + assert lookahead_distance(0.1, 1.5, 0.4, 1.5) == 0.4 + + +def test_lookahead_scales_in_linear_region(): + assert lookahead_distance(0.5, 1.5, 0.4, 1.5) == 0.75 + + +def test_lookahead_clamped_at_ceiling(): + assert lookahead_distance(2.0, 1.5, 0.4, 1.5) == 1.5 + + +def test_lookahead_monotonic_in_speed(): + assert lookahead_distance(0.8, 1.5, 0.4, 1.5) > lookahead_distance(0.45, 1.5, 0.4, 1.5) diff --git a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py index 3be82ce12e..d9d0ee2bac 100644 --- a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py +++ b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py @@ -106,7 +106,7 @@ def _static_robot_body(rr: Any) -> list[Any]: map_freq=-1.0, ).remappings([(FastLio2, "global_map", "global_map_fastlio")]), RayTracingVoxelMap.blueprint( - voxel_size=voxel_size, emit_every=2, global_emit_every=150, max_health=5 + voxel_size=voxel_size, emit_every=2, global_emit_every=50, max_health=5 ), # global_map is remapped off so the planner runs purely on the # incremental local_map + region_bounds pair. @@ -122,7 +122,7 @@ def _static_robot_body(rr: Any) -> list[Any]: viz_publish_hz=0.0, ).remappings([(MLSPlannerNative, "global_map", "global_map_unused")]), GoalRelay.blueprint(), - BasicPathFollower.blueprint(lookahead_m=0.5, heading_gain=0.8, max_angular=0.6), + BasicPathFollower.blueprint(speed=0.5, heading_gain=0.8, max_angular=0.6), MovementManager.blueprint(), ).global_config(n_workers=10, robot_model="unitree_go2", obstacle_avoidance=False) From 2f532e1a42ef98d979a69f872de9bf128dcaf15d Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Wed, 17 Jun 2026 17:12:04 -0700 Subject: [PATCH 54/56] FastLIO config change --- dimos/hardware/sensors/lidar/fastlio2/cpp/config/mid360.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/config/mid360.json b/dimos/hardware/sensors/lidar/fastlio2/cpp/config/mid360.json index ff6cc6dbf6..806d110ee2 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/config/mid360.json +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/config/mid360.json @@ -11,7 +11,7 @@ "blind": 1 }, "mapping": { - "acc_cov": 0.1, + "acc_cov": 1.0, "gyr_cov": 0.1, "b_acc_cov": 0.0001, "b_gyr_cov": 0.0001, From 2443b88c9aefe0f09a2eac3eeb14ff8059e3c4da Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Wed, 17 Jun 2026 17:57:55 -0700 Subject: [PATCH 55/56] Refactor morphological operations --- .../nav_3d/mls_planner/mls_planner.pyi | 3 +- .../nav_3d/mls_planner/mls_planner_native.py | 3 +- .../mls_planner/rust/src/mls_planner.rs | 40 +++++------ .../nav_3d/mls_planner/rust/src/planner.rs | 3 +- .../nav_3d/mls_planner/rust/src/python.rs | 9 +-- .../nav_3d/mls_planner/rust/src/surfaces.rs | 70 +++++++------------ 6 files changed, 50 insertions(+), 78 deletions(-) diff --git a/dimos/navigation/nav_3d/mls_planner/mls_planner.pyi b/dimos/navigation/nav_3d/mls_planner/mls_planner.pyi index 0565924a9f..07e0d2ea3b 100644 --- a/dimos/navigation/nav_3d/mls_planner/mls_planner.pyi +++ b/dimos/navigation/nav_3d/mls_planner/mls_planner.pyi @@ -23,8 +23,7 @@ class MLSPlanner: *, voxel_size: float, robot_height: float, - surface_dilation_passes: int = 3, - surface_erosion_passes: int = 3, + surface_closing_radius: float = 0.15, node_spacing_m: float = 1.0, wall_clearance_m: float = 0.3, wall_buffer_m: float = 0.75, diff --git a/dimos/navigation/nav_3d/mls_planner/mls_planner_native.py b/dimos/navigation/nav_3d/mls_planner/mls_planner_native.py index 0b427590a2..edc8bc4c1b 100644 --- a/dimos/navigation/nav_3d/mls_planner/mls_planner_native.py +++ b/dimos/navigation/nav_3d/mls_planner/mls_planner_native.py @@ -34,8 +34,7 @@ class MLSPlannerNativeConfig(NativeModuleConfig): voxel_size: float = 0.1 robot_height: float = 1.5 - surface_dilation_passes: int = 3 - surface_erosion_passes: int = 3 + surface_closing_radius: float = 0.3 node_spacing_m: float = 1.0 wall_clearance_m: float = 0.3 wall_buffer_m: float = 0.75 diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs index 520a447956..cd23d9913a 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs @@ -26,10 +26,10 @@ pub struct Config { pub voxel_size: f32, #[validate(range(exclusive_min = 0.0))] pub robot_height: f32, - #[validate(range(min = 0))] - pub surface_dilation_passes: u32, - #[validate(range(min = 0))] - pub surface_erosion_passes: u32, + /// Radius in meters of the morphological closing that fills small holes in + /// the extracted surface. Fills holes up to twice this wide. + #[validate(range(min = 0.0))] + pub surface_closing_radius: f32, #[validate(range(exclusive_min = 0.0))] pub node_spacing_m: f32, /// Hard clearance. Cells closer than this to a wall or edge are impassable. @@ -68,6 +68,13 @@ fn validate_wall_buffer(config: &Config) -> Result<(), ValidationError> { Ok(()) } +impl Config { + /// Compute number of dilations and erosions to do based on closing radius + pub fn closing_passes(&self) -> u32 { + (self.surface_closing_radius / self.voxel_size).ceil() as u32 + } +} + /// Cylindrical region the planner re-derives from a local map slice. pub struct RegionBounds { pub origin_x: f32, @@ -121,8 +128,7 @@ impl Planner { extract_surfaces( &self.voxel_map, clearance, - config.surface_dilation_passes, - config.surface_erosion_passes, + config.closing_passes(), &mut self.by_col, &mut surface, ); @@ -141,7 +147,7 @@ impl Planner { ) { let voxel_size = config.voxel_size; let clearance = (config.robot_height / voxel_size).ceil() as i32; - let pad = (config.surface_dilation_passes + config.surface_erosion_passes) as i32; + let pad = (2 * config.closing_passes()) as i32; let changed = self.replace_region_voxels(local_points, bounds, voxel_size); @@ -153,13 +159,8 @@ impl Planner { // A changed voxel column shifts surfaces only within pad of it, so the // write-back box is the changed-column bbox grown by pad. let write = (bx0 - pad, bx1 + pad, by0 - pad, by1 + pad); - let new_cells = extract_surfaces_region( - &self.by_col, - clearance, - config.surface_dilation_passes, - config.surface_erosion_passes, - write, - ); + let new_cells = + extract_surfaces_region(&self.by_col, clearance, config.closing_passes(), write); let (added, removed) = self.replace_surface_region(write, &new_cells); self.rebuild_region_graph(added, removed, config); @@ -331,7 +332,7 @@ impl Planner { // A few extra cells beyond the morphology, wall-buffer, and spacing reach. const SLACK_CELLS: i32 = 2; let voxel_size = config.voxel_size; - let pad = (config.surface_dilation_passes + config.surface_erosion_passes) as i32; + let pad = (2 * config.closing_passes()) as i32; let buffer_cells = ((config.wall_clearance_m + config.wall_buffer_m) / voxel_size).ceil() as i32; let spacing_cells = (config.node_spacing_m / voxel_size).ceil() as i32; @@ -486,8 +487,7 @@ mod region_tests { world_frame: String::new(), voxel_size: 0.1, robot_height: 0.5, - surface_dilation_passes: 3, - surface_erosion_passes: 3, + surface_closing_radius: 0.3, node_spacing_m: 1.0, wall_clearance_m: 0.0, wall_buffer_m: 0.3, @@ -929,8 +929,7 @@ mod region_tests { #[test] fn final_path_never_climbs_over_threshold_step() { let mut cfg = test_config(); - cfg.surface_dilation_passes = 0; - cfg.surface_erosion_passes = 0; + cfg.surface_closing_radius = 0.0; cfg.wall_clearance_m = 0.0; cfg.wall_buffer_m = 0.0; cfg.node_spacing_m = 0.5; @@ -981,8 +980,7 @@ mod region_tests { #[test] fn step_penalty_diverts_path_around_ridge() { let mut cfg = test_config(); - cfg.surface_dilation_passes = 0; - cfg.surface_erosion_passes = 0; + cfg.surface_closing_radius = 0.0; cfg.wall_clearance_m = 0.0; cfg.wall_buffer_m = 0.0; cfg.node_spacing_m = 0.5; diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs index d56916edf4..2d603bec48 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs @@ -592,8 +592,7 @@ mod tests { world_frame: "world".into(), voxel_size: VOXEL, robot_height: Z_TOL, - surface_dilation_passes: 0, - surface_erosion_passes: 0, + surface_closing_radius: 0.0, node_spacing_m: 1.0, wall_clearance_m: 0.2, wall_buffer_m: 0.5, diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/python.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/python.rs index ca4e8f054b..b92b9ef46e 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/python.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/python.rs @@ -26,8 +26,7 @@ impl MLSPlanner { *, voxel_size, robot_height, - surface_dilation_passes = 3, - surface_erosion_passes = 3, + surface_closing_radius = 0.3, node_spacing_m = 1.0, wall_clearance_m = 0.3, wall_buffer_m = 0.75, @@ -38,8 +37,7 @@ impl MLSPlanner { fn new( voxel_size: f32, robot_height: f32, - surface_dilation_passes: u32, - surface_erosion_passes: u32, + surface_closing_radius: f32, node_spacing_m: f32, wall_clearance_m: f32, wall_buffer_m: f32, @@ -51,8 +49,7 @@ impl MLSPlanner { world_frame: String::new(), voxel_size, robot_height, - surface_dilation_passes, - surface_erosion_passes, + surface_closing_radius, node_spacing_m, wall_clearance_m, wall_buffer_m, diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/surfaces.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/surfaces.rs index 2455c668d3..19e72de8c2 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/surfaces.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/surfaces.rs @@ -36,8 +36,7 @@ fn is_standable(ix: i32, iy: i32, iz: i32, by_col: &ColumnIz, clearance_cells: i pub fn extract_surfaces( voxel_map: &AHashSet, clearance_cells: i32, - dilation_passes: u32, - erosion_passes: u32, + closing_passes: u32, by_col: &mut ColumnIz, out: &mut Vec, ) { @@ -67,14 +66,7 @@ pub fn extract_surfaces( .collect(); drop(entries); - close_surface_holes( - standable, - by_col, - dilation_passes, - erosion_passes, - clearance_cells, - out, - ); + close_surface_holes(standable, by_col, closing_passes, clearance_cells, out); } /// Standable cells in one column: any cell with robot clearance above, plus @@ -123,12 +115,11 @@ pub fn remove_from_by_col(by_col: &mut ColumnIz, (ix, iy, iz): VoxelKey) { pub fn extract_surfaces_region( by_col: &ColumnIz, clearance_cells: i32, - dilation_passes: u32, - erosion_passes: u32, + closing_passes: u32, write: (i32, i32, i32, i32), ) -> Vec { let (wx0, wx1, wy0, wy1) = write; - let pad = (dilation_passes + erosion_passes) as i32; + let pad = (2 * closing_passes) as i32; let standable: Vec = ((wx0 - pad)..(wx1 + pad + 1)) .into_par_iter() @@ -147,8 +138,7 @@ pub fn extract_surfaces_region( close_surface_holes( standable, by_col, - dilation_passes, - erosion_passes, + closing_passes, clearance_cells, &mut closed, ); @@ -163,12 +153,11 @@ pub fn extract_surfaces_region( fn close_surface_holes( standable: Vec, by_col: &ColumnIz, - dilation_passes: u32, - erosion_passes: u32, + closing_passes: u32, clearance_cells: i32, out: &mut Vec, ) { - if standable.is_empty() || (dilation_passes == 0 && erosion_passes == 0) { + if standable.is_empty() || closing_passes == 0 { out.extend(standable); return; } @@ -179,16 +168,11 @@ fn close_surface_holes( } let slices: Vec<(i32, Vec<(i32, i32)>)> = by_z.into_iter().collect(); - out.par_extend(slices.par_iter().flat_map_iter(|(iz, xys)| { - close_at_z( - xys, - *iz, - by_col, - dilation_passes, - erosion_passes, - clearance_cells, - ) - })); + out.par_extend( + slices.par_iter().flat_map_iter(|(iz, xys)| { + close_at_z(xys, *iz, by_col, closing_passes, clearance_cells) + }), + ); } /// Close holes on an xy slice of the surfaces. @@ -196,11 +180,10 @@ fn close_at_z( xys: &[(i32, i32)], iz: i32, by_col: &ColumnIz, - dilation_passes: u32, - erosion_passes: u32, + closing_passes: u32, clearance_cells: i32, ) -> Vec { - let pad = (dilation_passes + erosion_passes) as i32; + let pad = (2 * closing_passes) as i32; let mut min_x = i32::MAX; let mut max_x = i32::MIN; let mut min_y = i32::MAX; @@ -222,12 +205,9 @@ fn close_at_z( img.put_pixel((ix - x0) as u32, (iy - y0) as u32, ON); } - if dilation_passes > 0 { - img = dilate(&img, Norm::L1, dilation_passes.min(u8::MAX as u32) as u8); - } - if erosion_passes > 0 { - img = erode(&img, Norm::L1, erosion_passes.min(u8::MAX as u32) as u8); - } + let k = closing_passes.min(u8::MAX as u32) as u8; + img = dilate(&img, Norm::L1, k); + img = erode(&img, Norm::L1, k); let mut out = Vec::new(); for py in 0..h { @@ -255,35 +235,35 @@ mod tests { cells.iter().copied().collect() } - fn run(cells: &[VoxelKey], clearance: i32, dil: u32, ero: u32) -> Vec { + fn run(cells: &[VoxelKey], clearance: i32, closing: u32) -> Vec { let map = voxel_map(cells); let mut by_col = ColumnIz::new(); let mut out = Vec::new(); - extract_surfaces(&map, clearance, dil, ero, &mut by_col, &mut out); + extract_surfaces(&map, clearance, closing, &mut by_col, &mut out); out } #[test] fn empty_input() { - assert!(run(&[], 5, 0, 0).is_empty()); + assert!(run(&[], 5, 0).is_empty()); } #[test] fn single_cell_is_topmost_surface() { - let s = run(&[(0, 0, 0)], 5, 0, 0); + let s = run(&[(0, 0, 0)], 5, 0); assert_eq!(s, vec![(0, 0, 0)]); } #[test] fn stacked_cells_within_headroom_only_topmost_is_surface() { let cells: Vec = (0..5).map(|z| (0, 0, z)).collect(); - let s = run(&cells, 5, 0, 0); + let s = run(&cells, 5, 0); assert_eq!(s, vec![(0, 0, 4)]); } #[test] fn gap_larger_than_headroom_makes_lower_cell_standable() { - let mut s = run(&[(0, 0, 0), (0, 0, 10)], 5, 0, 0); + let mut s = run(&[(0, 0, 0), (0, 0, 10)], 5, 0); s.sort(); assert_eq!(s, vec![(0, 0, 0), (0, 0, 10)]); } @@ -303,7 +283,7 @@ mod tests { .into_iter() .map(|(dx, dy)| (dx, dy, 0)) .collect(); - let s = run(&cells, 5, 3, 3); + let s = run(&cells, 5, 3); assert!( s.contains(&(0, 0, 0)), "closing should fill the center hole" @@ -326,7 +306,7 @@ mod tests { .map(|(dx, dy)| (dx, dy, 0)) .collect(); cells.push((0, 0, 1)); - let s = run(&cells, 5, 3, 3); + let s = run(&cells, 5, 3); assert!(!s.contains(&(0, 0, 0))); } } From 251dee49748362e6c7d973513d228d7035534079 Mon Sep 17 00:00:00 2001 From: Andrew Lauer Date: Wed, 17 Jun 2026 22:06:24 -0700 Subject: [PATCH 56/56] More configs --- .../navigation/nav_3d/mls_planner/utils/plan_rrd.py | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py b/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py index b2d5324274..ff3301f466 100644 --- a/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py +++ b/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py @@ -194,6 +194,11 @@ def main( ray_subsample: int = typer.Option(1, "--ray-subsample", help="Keep every Nth ray"), emit_every: int = typer.Option(1, "--emit-every", help="Replan every N lidar frames"), robot_height: float = typer.Option(1.0, "--robot-height", help="Robot height (m)"), + surface_closing_radius: float = typer.Option( + 0.3, + "--surface-closing-radius", + help="Hole-fill radius (m); morphological closing fills holes up to twice this wide", + ), node_spacing: float = typer.Option(1.0, "--node-spacing", help="Graph node spacing (m)"), wall_clearance: float = typer.Option( 0.3, @@ -206,6 +211,11 @@ def main( wall_buffer_weight: float = typer.Option( 100.0, "--wall-buffer-weight", help="Peak soft wall penalty at the clearance edge" ), + step_height: float = typer.Option( + 0.25, + "--step-height", + help="Max traversable vertical step (m); taller steps are impassable", + ), step_penalty_weight: float = typer.Option( 4.0, "--step-penalty-weight", help="Soft cost per meter of vertical climb" ), @@ -273,10 +283,12 @@ def main( planner = MLSPlanner( voxel_size=voxel_size, robot_height=robot_height, + surface_closing_radius=surface_closing_radius, node_spacing_m=node_spacing, wall_clearance_m=clr, wall_buffer_m=buf, wall_buffer_weight=wgt, + step_threshold_m=step_height, step_penalty_weight=step_penalty_weight, ) color = PATH_PALETTE[i % len(PATH_PALETTE)]