feat: multi level navigation

[aclauer]

Problem

3d navigation and ray tracing integration

Closes DIM-XXX

Solution

Creates a blueprint using ray tracing and mls planner, plus improvements to other modules. Click and point to navigate to goals.

Ray tracer:

• register point clouds with odometry (breaks on old fastlio recordings!)
• outputs region bounds for robust updates

Planner:

• incremental builds of planner artifacts in separate task so ingest isn't blocked
• path replan on each frame
• string pulled paths with tunable cost metrics
• levers for path safety

Other things:

• add set_motion_mode() to go2 connection, so we can turn off obstacle avoidance and things when instantiating the blueprint
• add a simple path follower. Replace this with better trajectory controllers for better results.

How to Test

build native modules:

cd ~/dimos   && ( cd dimos/hardware/sensors/lidar/pointlio/cpp && nix build .#pointlio_native )   && ( cd dimos/mapping/ray_tracing/rust && nix build path:. )   && ( cd dimos/navigation/nav_3d/mls_planner/rust && cargo build --release )

run! (on jetson on go2)

dimos --dtop --rerun-host 0.0.0.0 --rerun-open none run unitree-go2-nav-3d

other things not tested but would be awesome to try:

1. running with go2 stock lidar
2. running directly on go2 compute

Contributor License Agreement

• I have read and approved the CLA.

[codecov]

Codecov Report

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

Files with missing lines	Patch %	Lines
dimos/navigation/basic_path_follower/module.py	37.77%	84 Missing ⚠️
dimos/mapping/ray_tracing/test_transformer.py	0.00%	31 Missing ⚠️
.../navigation/nav_3d/mls_planner/test_transformer.py	0.00%	23 Missing ⚠️
dimos/robot/unitree/connection.py	21.42%	11 Missing ⚠️
dimos/robot/unitree/go2/connection.py	30.76%	9 Missing ⚠️
dimos/navigation/nav_3d/mls_planner/goal_relay.py	80.00%	5 Missing ⚠️
...ee/go2/blueprints/navigation/unitree_go2_nav_3d.py	79.16%	5 Missing ⚠️

@@            Coverage Diff             @@
##             main    #2570      +/-   ##
==========================================
- Coverage   70.77%   70.68%   -0.09%     
==========================================
  Files         873      877       +4     
  Lines       77628    77885     +257     
  Branches     6894     6912      +18     
==========================================
+ Hits        54940    55052     +112     
- Misses      20887    21030     +143     
- Partials     1801     1803       +2     

Flag	Coverage Δ
OS-ubuntu-24.04-arm	62.85% <41.66%> (-0.07%)	⬇️
OS-ubuntu-latest	65.66% <41.66%> (-0.08%)	⬇️
Py-3.10	65.66% <41.66%> (-0.07%)	⬇️
Py-3.11	65.66% <41.66%> (-0.07%)	⬇️
Py-3.12	65.65% <41.66%> (-0.08%)	⬇️
Py-3.13	65.66% <41.66%> (-0.07%)	⬇️
Py-3.14	65.66% <41.66%> (-0.08%)	⬇️
Py-3.14t	65.66% <41.66%> (-0.07%)	⬇️
SelfHosted-Large	30.13% <27.67%> (+<0.01%)	⬆️
SelfHosted-Linux	37.78% <27.67%> (-0.03%)	⬇️
SelfHosted-macOS	36.61% <27.67%> (+0.02%)	⬆️

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%> (ø)
...imos/navigation/basic_path_follower/test_module.py	100.00% <100.00%> (ø)
...avigation/nav_3d/mls_planner/mls_planner_native.py	100.00% <100.00%> (ø)
dimos/robot/all_blueprints.py	100.00% <ø> (ø)
.../unitree/go2/blueprints/basic/unitree_go2_basic.py	73.07% <100.00%> (ø)
dimos/navigation/nav_3d/mls_planner/goal_relay.py	80.00% <80.00%> (ø)
...ee/go2/blueprints/navigation/unitree_go2_nav_3d.py	79.16% <79.16%> (ø)
dimos/robot/unitree/go2/connection.py	55.92% <30.76%> (-0.52%)	⬇️
dimos/robot/unitree/connection.py	45.49% <21.42%> (-1.33%)	⬇️
.../navigation/nav_3d/mls_planner/test_transformer.py	12.50% <0.00%> (-1.79%)	⬇️
... and 2 more

... and 2 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 wires together a full 3-D click-to-navigate stack on the Unitree Go2: PointLio odometry → odometry-registered ray-traced voxel map → incremental MLS planner → pure-pursuit path follower. The Rust planner is substantially rewritten with multi-candidate start snapping, a string-pull smoother, a cached-path safe-truncation fallback, and a separate worker task so map ingestion never blocks replanning.

• Ray tracer: sensor-frame clouds are now rotated into the world frame by the odometry quaternion before voxelisation; a batched region_bounds output (encoded as a PoseStamped with cylindrical data packed into quaternion fields) feeds the planner's incremental update path.
• MLS planner: incremental update_region replaces full rebuilds on each local frame; plan_or_truncate falls back to safe-path truncation when no full route to the goal exists; Dijkstra tie-breaking is upgraded to VoxelKey for spatial determinism; prior P1 fixes (robot_search radius, string-pull baseline, stale NodeId boundary edges) are all present in the new code.
• Blueprint / Go2 integration: new set_motion_mode() RTC call switches the motion controller at startup; lidar and camera streams can now be individually disabled at construction time.

Confidence Score: 3/5

Navigation-critical code: the path follower's stop_movement handler is momentarily effective but gets overridden by the next planner publish, so any runtime stop signal is functionally ignored.

The path follower's _on_stop clears waypoints and sends zero velocity, but the planner worker continues replanning toward the active goal and publishes a new path within milliseconds. An operator or supervisor sending stop_movement — for example on obstacle detection — would see the robot resume navigation immediately. This is the main concern in an otherwise well-structured PR where the Rust planning core is thoroughly reworked and the prior blocking issues have been fixed.

dimos/navigation/basic_path_follower/module.py (stop_movement latch) and dimos/mapping/ray_tracing/rust/src/main.rs (region_bounds quaternion encoding convention).

Important Files Changed

Filename	Overview
dimos/navigation/basic_path_follower/module.py	New pure-pursuit path follower with lookahead interpolation; stop_movement handler clears waypoints but the planner immediately republishes a new path, making the stop momentary rather than persistent.
dimos/mapping/ray_tracing/rust/src/main.rs	Adds odometry-registered point cloud transform, batched local-map emission, and region_bounds output; packs cylindrical bounds into Quaternion x/y/z fields (w=0), a semantic misuse that could break any quaternion-aware tooling.
dimos/navigation/nav_3d/mls_planner/rust/src/mls_planner.rs	Major addition of incremental region update path (update_region, replace_region_voxels, rebuild_region_graph) alongside cached-path truncation; removed-key seeds are passed to downstream functions that must gracefully skip missing cells.
dimos/navigation/nav_3d/mls_planner/rust/src/planner.rs	Substantial rewrite: multi-candidate start snapping, node_dijkstra, string_pull smoother, truncate_to_safe for cached-path fallback; logic is well-structured and prior P1s (robot_search radius, string_pull baseline) are addressed.
dimos/navigation/nav_3d/mls_planner/rust/src/main.rs	Decouples map ingestion from planning via a worker task; pairs local_map + region_bounds by identical timestamps; replans on every wake; goal-mutex locking has a benign one-cycle race window on goal switch.
dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py	New nav-3D blueprint wiring PointLio → RayTracingVoxelMap → MLSPlannerNative → GoalRelay → BasicPathFollower; correctly remaps global_map away and sets mcf motion mode for stair traversal.
dimos/robot/unitree/connection.py	Adds set_motion_mode() with CheckMode/SelectMode RTC calls and a 5-second stabilisation sleep; print replaced with logger.warning as previously requested.
dimos/navigation/nav_3d/mls_planner/rust/src/dijkstra.rs	Adds dijkstra_region for incremental re-labelling, Weight enum to switch between base and penalized costs, and a cycle guard in walk_preds; Scored tie-break upgraded to VoxelKey for deterministic spatial ordering.
dimos/navigation/nav_3d/mls_planner/rust/src/nodes.rs	Splits place_nodes into place_nodes_region for windowed re-placement with kept-node NMS seeds; apply_wall_safe_penalty and ensure_node_per_component added to guarantee connectivity.
dimos/mapping/ray_tracing/transformer.py	Registers sensor-frame clouds into world frame via odometry quaternion; batches points for local bounds computation; switches obs.pose_tuple destructuring from *_ to explicit 7-element form.
dimos/navigation/nav_3d/mls_planner/goal_relay.py	Thin adapter converting Odometry→PoseStamped (start) and PointStamped→PoseStamped (goal) for the planner; clean and minimal.

Sequence Diagram

%%{init: {'theme': 'neutral'}}%%
sequenceDiagram
    participant Go2 as GO2Connection
    participant PL as PointLio
    participant RT as RayTracingVoxelMap
    participant MLS as MLSPlannerNative (Worker)
    participant GR as GoalRelay
    participant BPF as BasicPathFollower
    participant MM as MovementManager

    Go2->>PL: lidar_l1 (PointCloud2)
    Go2->>PL: odom_go2 (Odometry)
    PL->>RT: lidar (sensor-frame cloud)
    PL->>RT: odometry (pose)
    RT->>RT: rotate cloud into world frame
    RT->>MLS: local_map + region_bounds (paired by stamp)
    RT->>MLS: global_map (every 50 frames)

    Go2->>GR: odometry
    GR->>MLS: start_pose (PoseStamped)

    Note over GR: User clicks goal in Rerun
    GR->>MLS: goal_pose (PoseStamped)

    MLS->>MLS: update_region / update_global_map
    MLS->>MLS: plan_or_truncate (plan + string_pull OR truncate_to_safe)
    MLS->>BPF: path (Path)

    Go2->>GR: odometry
    GR->>BPF: odometry (for position)
    BPF->>MM: nav_cmd_vel (Twist)
    MM->>Go2: cmd_vel

Loading

%%{init: {'theme': 'base', 'themeVariables': {"darkMode": true, "background": "#0d1117", "primaryColor": "#21262d", "primaryTextColor": "#e6edf3", "primaryBorderColor": "#8b949e", "lineColor": "#8b949e", "textColor": "#e6edf3", "edgeLabelBackground": "#161b22", "actorBkg": "#21262d", "actorBorder": "#8b949e", "actorTextColor": "#e6edf3", "actorLineColor": "#8b949e", "signalColor": "#8b949e", "signalTextColor": "#e6edf3", "noteBkgColor": "#373320", "noteBorderColor": "#d4a72c", "noteTextColor": "#f0e6c0", "labelBoxBkgColor": "#21262d", "labelBoxBorderColor": "#8b949e", "labelTextColor": "#e6edf3", "loopTextColor": "#e6edf3", "activationBkgColor": "#30363d", "activationBorderColor": "#8b949e"}}}%%
sequenceDiagram
    participant Go2 as GO2Connection
    participant PL as PointLio
    participant RT as RayTracingVoxelMap
    participant MLS as MLSPlannerNative (Worker)
    participant GR as GoalRelay
    participant BPF as BasicPathFollower
    participant MM as MovementManager

    Go2->>PL: lidar_l1 (PointCloud2)
    Go2->>PL: odom_go2 (Odometry)
    PL->>RT: lidar (sensor-frame cloud)
    PL->>RT: odometry (pose)
    RT->>RT: rotate cloud into world frame
    RT->>MLS: local_map + region_bounds (paired by stamp)
    RT->>MLS: global_map (every 50 frames)

    Go2->>GR: odometry
    GR->>MLS: start_pose (PoseStamped)

    Note over GR: User clicks goal in Rerun
    GR->>MLS: goal_pose (PoseStamped)

    MLS->>MLS: update_region / update_global_map
    MLS->>MLS: plan_or_truncate (plan + string_pull OR truncate_to_safe)
    MLS->>BPF: path (Path)

    Go2->>GR: odometry
    GR->>BPF: odometry (for position)
    BPF->>MM: nav_cmd_vel (Twist)
    MM->>Go2: cmd_vel

Loading

_{Reviews (11): Last reviewed commit: "Merge remote-tracking branch 'origin/mai..." | Re-trigger Greptile}

[leshy]

just one question around AI / normal mode switcher

[aclauer]

How come this was introduced? generally we never run in "normal" mode - only way we ever used go2 is in sport mode

Yeah it isn't strictly necessary, but probably good to have a way to configure. For example, "AI" mode is actually deprecated as of firmware < 1.1.6 and "MCF" is the new string.