feat: 3d path planning

[aclauer]

Problem

3d multi level path planning

Closes DIM-XXX

Solution

Added the MLS planner (multi level surface). The idea is to place high level nodes on the surfaces of the map and connect neighboring nodes. Then we can do planning on these nodes which is much faster and has cached paths than planning on the surface directly.

Not included yet:

• incremental building (update the surfaces only in local region visible by sensor)
• path optimization
• path replanning

How to Test

cargo test
dimos run path-planner-eval, then click start and goal positions on the surface

Artifact generation on entire input voxel map (2M voxels) takes ~200ms. Planning takes 0.28ms.

Graph that planning actually happens on ^

Contributor License Agreement

• I have read and approved the CLA.

[codecov]

Codecov Report

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

Files with missing lines	Patch %	Lines
dimos/navigation/nav_3d/evaluator/blueprints.py	50.00%	23 Missing ⚠️
...click_start_goal_router/click_start_goal_router.py	60.71%	11 Missing ⚠️

📢 Thoughts on this report? Let us know!

[greptile-apps]

Greptile Summary

This PR introduces a 3D multi-level surface (MLS) path planner implemented in Rust with a Python wrapper and evaluation harness. The planner voxelizes an incoming point cloud, extracts standable surfaces with morphological closing, places graph nodes via wall-distance Dijkstra + NMS, builds Voronoi-based node edges, and plans paths using A*/Dijkstra on the node graph — achieving ~0.28ms planning on a 2M-voxel map.

• Rust core (surfaces.rs, adjacency.rs, nodes.rs, edges.rs, dijkstra.rs, planner.rs): fully self-contained with extensive unit tests covering disconnected graphs, morphological closing, NMS spacing, and end-to-end path assembly.
• Python wrapper (mls_planner_native.py, blueprints.py): thin NativeModule declaration and rerun visualization wiring.
• Evaluator (evaluator.py, scenarios.py, mesh_loader.py): scenario driver with synthetic wall/floor test cases and optional real-mesh scenarios gated behind MESH_PATH.

Confidence Score: 5/5

Safe to merge. The Rust planner core is well-tested and logically sound; the only findings are in the dev-tool evaluator and are cosmetic/timing edge cases.

The Rust core handles all correctness-critical paths (surface extraction, Dijkstra, node placement, path assembly) with extensive unit tests and careful edge-case handling. Issues flagged are limited to the Python evaluator: a narrow timing window where a delayed empty-path response from an earlier start_pose could be miscounted as a goal response, and a possible no-op .ts assignment on PointCloud2 objects. Neither affects the planner itself or production code.

dimos/navigation/nav_3d/evaluator/evaluator.py — state reset ordering and timestamp attribute correctness.

Important Files Changed

Filename	Overview
dimos/navigation/nav_3d/mls_planner/rust/src/main.rs	Module wiring and protocol layer for the MLS planner. Config validation, global-map handler, start/goal handlers, and LCM message building. The now() function correctly caps the Unix timestamp with .min(i32::MAX as u64) before casting to i32. No new issues found.
dimos/navigation/nav_3d/mls_planner/rust/src/surfaces.rs	Surface extraction: voxelization → standability filter → morphological close per Z-slice. Dilation/erosion passes are now correctly clamped with .min(u8::MAX as u32) as u8. Thorough test coverage. No new issues found.
dimos/navigation/nav_3d/mls_planner/rust/src/dijkstra.rs	Multi-source Dijkstra with reusable state. Binary heap ordering uses total_cmp for NaN safety. Well-tested including multi-source labeling, disconnected graphs, and buffer reuse. No issues found.
dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs	End-to-end path planning: pose snapping, Voronoi-based node-graph search, and waypoint assembly. Gracefully returns None for unreachable snaps or disconnected graphs. Good test coverage. No issues found.
dimos/navigation/nav_3d/mls_planner/rust/src/edges.rs	Node-graph edge construction via parallel Voronoi boundary scanning. Boundary cell convention is maintained consistently across fold and reduce. No issues found.
dimos/navigation/nav_3d/mls_planner/rust/src/nodes.rs	Node placement via wall-distance Dijkstra + NMS grid. Wall-safe penalty applied before node-edge Dijkstra so penalized costs flow into edge construction. No issues found.
dimos/navigation/nav_3d/mls_planner/rust/src/adjacency.rs	Slot-map backed SurfaceCells with tombstone reuse. The clear()+resize_with(n) pattern correctly reuses old (now-empty) Vec allocations. Parallel edge build is sound. No issues found.
dimos/navigation/nav_3d/evaluator/evaluator.py	Scenario driver. Two P2 issues: state reset happens after goal_pose publish (narrow timing window for false failures), and .ts attribute on PointCloud2 may silently create an unused attribute.
dimos/navigation/nav_3d/evaluator/scenarios.py	Synthetic and mesh-based evaluation scenarios. Wall geometry is correctly computed. blocked_wall correctly expects no path. Mesh scenarios gated behind MESH_PATH. No issues.
dimos/navigation/nav_3d/mls_planner/mls_planner_native.py	Thin Python wrapper declaring the Rust-backed native module, config defaults, and stream types. No issues.
dimos/navigation/nav_3d/evaluator/blueprints.py	Wires the evaluator blueprint and renders clouds, nodes, and edge segments into rerun. No new issues beyond those noted in previous review thread.

Sequence Diagram

sequenceDiagram
    participant PC as PointCloud Source
    participant MLS as MLS Planner (Rust)
    participant UI as ClickStartGoalRouter
    participant Viz as RerunBridge

    PC->>MLS: PointCloud2 (global_map)
    MLS->>MLS: voxelize → extract_surfaces
    MLS->>MLS: place_nodes (wall-dist Dijkstra + NMS)
    MLS->>MLS: build_node_edges (Voronoi Dijkstra)
    MLS->>Viz: surface_map (PointCloud2)
    MLS->>Viz: nodes (PointCloud2)
    MLS->>Viz: node_edges (Path / LineSegments3D)

    UI->>MLS: PoseStamped (start_pose)
    MLS->>Viz: empty path (clears stale display)

    UI->>MLS: PoseStamped (goal_pose)
    MLS->>MLS: snap poses → Dijkstra path on node graph
    MLS->>Viz: path (Path)

Loading

_{Reviews (3): Last reviewed commit: "Merge branch 'main' into andrew/feat/dev..." | Re-trigger Greptile}