feat: multi level navigation#2570
Open
aclauer wants to merge 24 commits into
Open
Conversation
aclauer
requested review from
arkluc,
leshy,
mustafab0,
paul-nechifor and
spomichter
as code owners
Codecov Report❌ Patch coverage is @@ Coverage Diff @@
## main #2570 +/- ##
==========================================
+ Coverage 69.61% 70.79% +1.18%
==========================================
Files 878 870 -8
Lines 79326 77499 -1827
Branches 7126 6884 -242
==========================================
- Hits 55220 54864 -356
+ Misses 22301 20833 -1468
+ Partials 1805 1802 -3
Flags with carried forward coverage won't be shown. Click here to find out more.
... and 14 files with indirect coverage changes 🚀 New features to boost your workflow:
|
Contributor
Greptile SummaryThis PR wires together 3D click-to-navigate on the Unitree Go2: PointLIO provides odometry-registered point clouds, the ray tracer builds a voxel map with incremental region updates, the MLS planner runs string-pulled Dijkstra paths with wall-penalized edges, and a new
Confidence Score: 5/5Safe to merge. The core algorithmic changes are well-tested and the previously flagged robot_search radius and string_pull baseline issues have been correctly fixed. All three previously identified logic bugs (robot_search radius compared against penalized costs, string_pull stale baseline on infeasible steps, and the stale empty-path guard) are correctly fixed in this revision. The new incremental dijkstra_region, multi-candidate start snapping, truncate_to_safe, and the BasicPathFollower look sound after tracing through the data flow. The remaining comments are non-blocking style and performance suggestions. No files require special attention for merge safety. Important Files Changed
Sequence Diagram%%{init: {'theme': 'neutral'}}%%
sequenceDiagram
participant HW as Go2 Hardware
participant PL as PointLIO
participant RT as RayTracingVoxelMap
participant MP as MLSPlannerNative
participant GR as GoalRelay
participant BF as BasicPathFollower
participant MM as MovementManager
HW->>PL: lidar scan + odom
PL->>RT: local point cloud (body frame)
RT->>RT: rotate points to world frame
RT->>MP: local_map (PointCloud2)
RT->>MP: region_bounds (PoseStamped)
MP->>MP: "stamps_paired? pair & hand_off"
MP->>MP: update_region (Dijkstra incremental)
HW->>GR: odometry
GR->>MP: start_pose
HW-->>GR: goal click (PointStamped)
GR->>MP: goal_pose
MP->>MP: plan_or_truncate
MP->>BF: path (Path)
BF->>BF: lookahead control loop
BF->>MM: nav_cmd_vel (Twist)
MM->>HW: velocity command
%%{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 HW as Go2 Hardware
participant PL as PointLIO
participant RT as RayTracingVoxelMap
participant MP as MLSPlannerNative
participant GR as GoalRelay
participant BF as BasicPathFollower
participant MM as MovementManager
HW->>PL: lidar scan + odom
PL->>RT: local point cloud (body frame)
RT->>RT: rotate points to world frame
RT->>MP: local_map (PointCloud2)
RT->>MP: region_bounds (PoseStamped)
MP->>MP: "stamps_paired? pair & hand_off"
MP->>MP: update_region (Dijkstra incremental)
HW->>GR: odometry
GR->>MP: start_pose
HW-->>GR: goal click (PointStamped)
GR->>MP: goal_pose
MP->>MP: plan_or_truncate
MP->>BF: path (Path)
BF->>BF: lookahead control loop
BF->>MM: nav_cmd_vel (Twist)
MM->>HW: velocity command
Reviews (7): Last reviewed commit: "Merge branch 'main' into andrew/feat/kro..." | Re-trigger Greptile |
![greptile-apps[bot]](https://avatars.githubusercontent.com/in/867647?s=60&v=4){kind=small}
github-actions
Bot
added
the
ready-to-merge
github-actions
Bot
removed
the
ready-to-merge
github-actions
Bot
added
the
ready-to-merge
github-actions
Bot
added
ready-to-merge
github-actions
Bot
removed
the
ready-to-merge
github-actions
Bot
added
the
ready-to-merge
leshy
reviewed
Jun 24, 2026
| {"api_id": 1001, "parameter": {"enable": int(enabled)}}, | ||
| ) | ||
|
|
||
| def set_motion_mode(self, name: str) -> None: |
Member
There was a problem hiding this comment.
How come this was introduced? generally we never run in "normal" mode - only way we ever used go2 is in sport mode
leshy
approved these changes
Jun 24, 2026
leshy
left a comment
leshy
left a comment
Member
There was a problem hiding this comment.
just one question around AI / normal mode switcher
leshy
reviewed
Jun 24, 2026
| class ConnectionConfig(ModuleConfig): | ||
| ip: str = Field(default_factory=lambda m: m["g"].robot_ip) | ||
| mode: Go2Mode = Go2Mode.DEFAULT | ||
| lidar: bool = True |
Member
There was a problem hiding this comment.
we can actually turn off lidar when this is false, no need to implement here and can merge, but just noting
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
2 participants
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
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:
Planner:
Other things:
How to Test
build native modules:
run (on jetson on go2)!
Contributor License Agreement