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 adds a multi-level surface (MLS) 3D path planner implemented as a Rust native module. It extracts walkable surfaces from a voxelized point cloud, places graph nodes at local wall-distance maxima via NMS, builds Voronoi-boundary edges between nodes, and plans paths with Dijkstra over the resulting sparse node graph.

• Rust planner (mls_planner/rust/): full pipeline from surface extraction (morphological close per z-slice), 4-connected adjacency building, Dijkstra-based node placement and edge construction, and a plan() function that snaps poses to the surface graph and assembles waypoints via predecessor walks.
• Python wrappers (mls_planner_native.py, evaluator/): a NativeModule shim for the Rust binary, a scenario-based Evaluator that drives start/goal sequences and checks for expected path/no-path, and a ClickStartGoalRouter that alternates user clicks between start and goal.
• Blueprint update (all_blueprints.py): re-routes path-planner-eval from the old nav_stack evaluator to the new nav_3d evaluator and registers mls-planner-native and click-start-goal-router as named modules.

Confidence Score: 5/5

The planner itself is safe to merge; the two findings are in the evaluator (test harness) and a node-placement edge case for narrow parallel surfaces.

The core Rust pipeline — surface extraction, 4-connected adjacency, multi-source Dijkstra, Voronoi edge construction, and waypoint assembly — is correct and covered by unit tests. The two observations are a stale-response acceptance window in the Python evaluator (only affects correctness of the automated evaluation scores, not the planner itself) and a 3D NMS suppression concern for closely-stacked parallel surfaces (a known limitation for the initial release, which explicitly defers incremental building and path optimization).

nodes.rs (NMS suppression of parallel-surface nodes) and evaluator.py (stale path accepted in subsequent scenario's wait window).

Important Files Changed

Filename	Overview
dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs	Core planning logic: pose snapping, Dijkstra on the node graph, and waypoint assembly via predecessor walks. Logic is sound and well-tested.
dimos/navigation/nav_3d/mls_planner/rust/src/nodes.rs	Node placement with wall-distance Dijkstra + 3D NMS. The NMS uses full 3D Euclidean distance, which can suppress nodes on surfaces that are vertically close (within node_spacing_m) even when they are on separate navigable levels.
dimos/navigation/nav_3d/mls_planner/rust/src/surfaces.rs	Surface extraction with per-z morphological closing. Topmost voxel in each column is unconditionally standable, which is correct for outdoor/staircase maps. Well-tested.
dimos/navigation/nav_3d/mls_planner/rust/src/adjacency.rs	Slot-map surface cell store with tombstone recycling and 4-connected 3D adjacency. Correct implementation with thorough tests.
dimos/navigation/nav_3d/mls_planner/rust/src/dijkstra.rs	Multi-source Dijkstra with reusable state buffer. NaN-safe via total_cmp, predecessor reconstruction correct, well-tested.
dimos/navigation/nav_3d/mls_planner/rust/src/edges.rs	Voronoi-boundary edge construction via parallel Dijkstra fold-reduce. Parallel merge correctly picks minimum-cost boundary edge per node pair.
dimos/navigation/nav_3d/mls_planner/rust/src/main.rs	Module wiring, PointCloud2 serialization, and timestamp handling. The i32::MAX clamp on timestamp seconds is present.
dimos/navigation/nav_3d/evaluator/evaluator.py	Scenario orchestrator using asyncio. The path-received event is cleared after goal publication, meaning a delayed response from a prior scenario arriving during the current scenario's wait can produce an incorrect result.
dimos/navigation/nav_3d/evaluator/blueprints.py	Wires all modules together and maps stream keys to rerun renderers. Accesses private _segments/_traversability attributes of LineSegments3D (noted in prior review thread).
dimos/navigation/nav_3d/evaluator/scenarios.py	Synthetic scenario definitions (floor, wall, two-room, optional mesh). Clean and correct.
dimos/navigation/nav_stack/modules/click_start_goal_router/click_start_goal_router.py	Simple alternating click router. Correct and straightforward.
dimos/navigation/nav_3d/mls_planner/mls_planner_native.py	Python shim for the Rust MLS planner binary. Correct config forwarding and stream declarations.

Sequence Diagram

sequenceDiagram
    participant PC2 as PointCloud2 (global map)
    participant Rust as MlsPlanner (Rust)
    participant Py as MLSPlannerNative (Python)
    participant Click as ClickStartGoalRouter

    PC2->>Rust: on_global_map(PointCloud2)
    Note over Rust: voxelize → extract_surfaces<br/>(morphological close per z-slice)
    Note over Rust: build_surface_cells (4-connected, step-threshold)
    Note over Rust: place_nodes (wall Dijkstra + 3D NMS)
    Note over Rust: build_node_edges (Voronoi Dijkstra)
    Rust-->>Py: surface_map (PointCloud2)
    Rust-->>Py: nodes (PointCloud2)
    Rust-->>Py: node_edges (Path / LineSegments3D)

    Click->>Rust: on_start_pose(PoseStamped)
    Rust-->>Py: path (empty, to clear stale viz)

    Click->>Rust: on_goal_pose(PoseStamped)
    Note over Rust: snap start+goal to surface<br/>walk_preds → nearest node<br/>Dijkstra on node graph<br/>assemble_waypoints
    Rust-->>Py: path (nav_msgs/Path)

Loading

_{Reviews (2): Last reviewed commit: "Fix" | Re-trigger Greptile}