feat: MLS planner improvements

[aclauer]

Problem

3d nav improvements

Closes DIM-XXX

Solution

• uses LoGlo PC and local region boundaries to avoid recomputing planner artifacts of entire map
• optimize path with "string pulling" to make less jagged routes

How to Test

uv run maturin develop --release -m dimos/mapping/ray_tracing/rust/Cargo.toml
uv run maturin develop --release -m dimos/navigation/nav_3d/mls_planner/rust/Cargo.toml

uv run python -m dimos.navigation.nav_3d.mls_planner.utils.plan_rrd ~/recordings/mem2-003.db --goal 0.75 0.45 -0.5 --robot-height 0.3

large_plan.mp4

Contributor License Agreement

• I have read and approved the CLA.

[codecov]

Codecov Report

❌ Patch coverage is 13.84615% with 56 lines in your changes missing coverage. Please review.
✅ All tests successful. No failed tests found.

Files with missing lines	Patch %	Lines
dimos/mapping/ray_tracing/test_transformer.py	0.00%	29 Missing ⚠️
.../navigation/nav_3d/mls_planner/test_transformer.py	0.00%	27 Missing ⚠️

Flag	Coverage Δ
OS-ubuntu-24.04-arm	63.83% <13.84%> (-0.03%)	⬇️
OS-ubuntu-latest	64.67% <13.84%> (-0.03%)	⬇️
Py-3.10	64.67% <13.84%> (-0.03%)	⬇️
Py-3.11	64.66% <13.84%> (-0.03%)	⬇️
Py-3.12	64.66% <13.84%> (-0.03%)	⬇️
Py-3.13	64.66% <13.84%> (-0.03%)	⬇️
Py-3.14	64.68% <13.84%> (-0.03%)	⬇️
Py-3.14t	64.66% <13.84%> (-0.03%)	⬇️
SelfHosted-Large	30.34% <0.00%> (?)
SelfHosted-Linux	38.26% <0.00%> (-0.02%)	⬇️
SelfHosted-macOS	36.98% <0.00%> (-0.06%)	⬇️

Flags with carried forward coverage won't be shown. Click here to find out more.

Files with missing lines	Coverage Δ
dimos/mapping/ray_tracing/module.py	100.00% <100.00%> (ø)
...avigation/nav_3d/mls_planner/mls_planner_native.py	100.00% <100.00%> (ø)
.../navigation/nav_3d/mls_planner/test_transformer.py	11.36% <0.00%> (-2.93%)	⬇️
dimos/mapping/ray_tracing/test_transformer.py	9.43% <0.00%> (-9.09%)	⬇️

... and 18 files with indirect coverage changes

🚀 New features to boost your workflow:

• 📦 JS Bundle Analysis: Save yourself from yourself by tracking and limiting bundle sizes in JS merges.

[greptile-apps]

Greptile Summary

This PR adds two significant improvements to the MLS 3-D planner: incremental regional map updates (avoiding full graph rebuilds on every LiDAR frame) and a string-pulling path smoother that collapses jagged cell-by-cell routes into straight-line segments where clearance allows.

• Regional incremental updates: Planner::update_region replaces a cylindrical slice of the voxel map, re-extracts only the affected surface columns (with morphology halo padding), and rebuilds only the graph subgraph inside a node_window. dijkstra_region seeds from in-window sources plus cached frontier cells, keeping far-field Voronoi assignments stable. NodeId changes from a dense index to the stable CellId so cached edges survive region patching.
• String-pull smoother: After assembling the raw cell sequence, string_pull greedily extends from each anchor to the farthest cell in LoS with clearance ≥ max(node_wall_buffer_m, robot_radius_m). Shortcuts through unobserved or close-to-wall columns are refused.
• Penalty hardening: robot_radius_m adds a hard-impassability floor; scale_edges is idempotent (scales from base_cost), preventing penalty compounding on regional repasses.

Confidence Score: 5/5

Safe to merge — the incremental regional update pipeline maintains the surface_lookup/cells invariant correctly, the string puller is conservative, and comprehensive round-trip tests verify that streaming-only regional builds produce equivalent plans to full global rebuilds.

All findings are non-blocking. The robot_search penalized-cost vs. geometric-radius mismatch degrades path quality near dense wall regions but the fallback via walk_preds always produces a valid path. The MLSPlan breaking change is intentional and tested. No correctness bugs found in the incremental Dijkstra, node placement, edge caching, or path assembly logic.

planner.rs (robot_search cost-budget semantics and los_on_surface gap handling) and transformer.py (breaking region_bounds requirement) are worth a second read before merge if any downstream callers outside this repo feed MLSPlan directly.

Important Files Changed

Filename	Overview
dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs	Major refactor: replaces single-pass global Dijkstra with goal-rooted node Dijkstra + bounded robot_search; adds string-pull smoother. robot_search uses penalized edge costs with a geometric-radius budget (minor mismatch); los_on_surface is conservatively safe but can block shortcuts across gap columns.
dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs	Adds update_region for incremental cylindrical-region updates. The replace_region_voxels, replace_surface_region, and rebuild_region_graph pipeline maintains the surface_lookup/cells invariant correctly. Comprehensive round-trip tests verify region-only streaming produces equivalent plans to a full build.
dimos/navigation/nav_3d/mls_planner/rust/src/dijkstra.rs	Adds dijkstra_region for incremental window-scoped Dijkstra seeded from in-window sources and cached frontier cells; adds Weight enum. walk_preds gains cycle detection. Tie-breaking changed from CellId to VoxelKey for reproducibility.
dimos/navigation/nav_3d/mls_planner/rust/src/nodes.rs	Adds robot_radius_m hard-impassability floor and exponential wall_penalty_weight; adds place_nodes_region and apply_wall_safe_penalty_region. scale_edges is idempotent (scales from base_cost), preventing compounding penalties on regional repasses.
dimos/navigation/nav_3d/mls_planner/rust/src/edges.rs	Changes NodeId from dense index to CellId (stable across incremental updates). Adds build_node_edges_region which retains cached edges outside the change window. node_adj changes from Vec to AHashMap to handle sparse node sets.
dimos/navigation/nav_3d/mls_planner/transformer.py	Switches from update_global_map to update_region, requiring region_bounds tag. Adds timing telemetry. The mandatory region_bounds check is a breaking change for existing callers not using emit_local=True.
dimos/mapping/ray_tracing/transformer.py	Adds emit_local flag and _local_bounds helper that computes a percentile-based cylinder over a batch of frames; emits region_bounds tag when enabled and resets batch accumulators after each emit.

Flowchart

%%{init: {'theme': 'neutral'}}%%
flowchart TD
    LiDAR["LiDAR frame (PointCloud2)"] --> RT["RayTraceMap\n(emit_local=True)"]
    RT -->|"local_map + region_bounds"| MP["MlsPlanner\ntry_region_update"]
    RT -->|global_map| MG["update_global_map\n(full rebuild)"]
    MP --> URV["replace_region_voxels"]
    URV --> ESR["extract_surfaces_region"]
    ESR --> RSS["replace_surface_region"]
    RSS --> RRG["rebuild_region_graph"]
    RRG --> PNR["place_nodes_region\n(dijkstra_region wall dist + NMS + penalty)"]
    PNR --> BNER["build_node_edges_region\n(dijkstra_region Voronoi + merge cached edges)"]
    MG --> RN["rebuild_nodes\n(full wall dijkstra + penalty)"]
    RN --> BNE["build_node_edges\n(full Voronoi dijkstra)"]
    BNER --> PLAN["plan()"]
    BNE --> PLAN
    PLAN --> SC["snap_candidates"]
    SC --> ND["node_dijkstra\n(goal-rooted cost-to-go)"]
    ND --> SE["select_entry\n(robot_search + best connect+ctg)"]
    SE --> AC["assemble_cells"]
    AC --> SP["string_pull\n(los_on_surface clearance check)"]
    SP --> WP["cells_to_waypoints"]

Loading

_{Reviews (3): Last reviewed commit: "Clean up" | Re-trigger Greptile}