G1 raytracing navigation stack (unitree-g1-nav-simple)

[jeff-hykin]

How to Test

on g1:

dimos run unitree-g1-nav-simple

Contributor License Agreement

• I have read and approved the CLA.

[greptile-apps]

Greptile Summary

Introduces the unitree-g1-nav-simple navigation stack for the Unitree G1, wiring FastLio2 → RayTracingVoxelMap → CostMapper → ReplanningAStarPlanner with a nix-built Rust ray-tracer. The PR also extracts reusable sub-blueprints (_unitree_g1_onboard, unitree_g1_vis, unitree_g1_primitive_no_nav), adds deadzone compensation to the G1 DDS SDK, and propagates OccupancyGrid.frame_id throughout the A* path builder.

• New nav stack (unitree_g1_nav_simple): composes onboard sensors, ray-tracing voxel map, height-cost occupancy grid, and replanning A* planner with G1-specific clearance and rotation parameters.
• Blueprint refactor: _unitree_g1_onboard, unitree_g1_vis, and unitree_g1_primitive_no_nav are extracted into standalone files; the nav_onboard blueprint is updated to reuse them.
• DDS SDK deadzone boost: sub-threshold linear/angular velocity commands are snapped to a configurable minimum so the G1's motor deadzone doesn't silently drop small motion commands.

Confidence Score: 5/5

Safe to merge; the new nav stack is additive and existing stacks are unaffected.

All changes are either new files (new blueprint, sub-blueprints, Rust nix build) or targeted fixes (frame_id propagation, deadzone compensation). No existing navigation paths are broken. The one asymmetry in the FastLio2 TF publish only manifests when frame_id_prefix is explicitly set, which no current blueprint does.

dimos/hardware/sensors/lidar/fastlio2/module.py for the frame_id_prefix asymmetry introduced by the TF refactor.

Important Files Changed

Filename	Overview
dimos/robot/unitree/g1/blueprints/navigation/unitree_g1_nav_simple.py	New runnable blueprint composing _unitree_g1_onboard + RayTracingVoxelMap + CostMapper + ReplanningAStarPlanner + MovementManager + unitree_g1_vis; unitree_g1_vis is also already included inside _unitree_g1_onboard (flagged in prior thread).
dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_onboard.py	New sub-blueprint bundling FastLio2 (with global_map remapping), G1HighLevelDdsSdk, and unitree_g1_vis; cleanly extracted from unitree_g1_nav_onboard.
dimos/mapping/costmapper.py	Adds initial_safe_radius_meters config, costmap_to_rerun helper, and numpy-based disc clearing; the disc is intentionally anchored to world origin (confirmed by developer) and the LUT covers the full 0–101 index range correctly.
dimos/hardware/sensors/lidar/fastlio2/module.py	_on_odom_for_tf now reads frame_id through the Module property (applies frame_id_prefix) but child_frame_id directly from config (no prefix), creating a TF asymmetry when frame_id_prefix is set.
dimos/navigation/replanning_a_star/module.py	Adds Odometry port and ReplanningAStarPlannerConfig with robot_width/robot_rotation_diameter overrides; both odom and odometry are subscribed unconditionally (flagged in prior thread).
dimos/navigation/replanning_a_star/min_cost_astar.py	All hardcoded "world" frame_id strings in path construction replaced with costmap.frame_id; clean and consistent change.
dimos/robot/unitree/g1/effectors/high_level/dds_sdk.py	Adds deadzone compensation via _boost_above_deadzone; configurable min_effective_linear/angular_velocity defaults (0.05 m/s, 0.2 rad/s) applied in move().
dimos/mapping/ray_tracing/module.py	Switches build from cargo to nix and executable path from target/release to result/bin; aligns with FastLio2 pattern.
dimos/msgs/nav_msgs/Odometry.py	Adds to_pose_stamped() conversion helper preserving ts, frame_id, position, and orientation.
dimos/robot/all_blueprints.py	Registers unitree-g1-nav-simple and corrects the typo in unitree-g1-primitive-no-nav import path.

Sequence Diagram

sequenceDiagram
    participant HW as G1 Hardware
    participant FL as FastLio2
    participant RT as RayTracingVoxelMap
    participant CM as CostMapper
    participant RA as ReplanningAStarPlanner
    participant MM as MovementManager
    participant DDS as G1HighLevelDdsSdk

    HW->>FL: LiDAR scan
    FL->>RT: lidar (PointCloud2)
    FL->>RT: odometry (Odometry)
    RT->>CM: global_map (PointCloud2)
    CM->>RA: global_costmap (OccupancyGrid)
    FL->>RA: odometry → to_pose_stamped()
    RA->>MM: nav_cmd_vel (Twist)
    MM->>DDS: cmd_vel (Twist)
    DDS->>DDS: _boost_above_deadzone()
    DDS->>HW: motion command

Loading

_{Reviews (6): Last reviewed commit: "Merge branch 'jeff/feat/g1_raycast' of g..." | Re-trigger Greptile}

[codecov]

Codecov Report

❌ Patch coverage is 54.47154% 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/costmapper.py	45.45%	18 Missing ⚠️
...s/robot/unitree/g1/effectors/high_level/dds_sdk.py	0.00%	14 Missing ⚠️
.../g1/blueprints/navigation/unitree_g1_nav_simple.py	38.88%	11 Missing ⚠️
dimos/navigation/replanning_a_star/module.py	63.63%	4 Missing ⚠️
.../unitree/g1/blueprints/primitive/unitree_g1_vis.py	78.94%	4 Missing ⚠️
dimos/msgs/nav_msgs/OccupancyGrid.py	0.00%	3 Missing ⚠️
dimos/msgs/nav_msgs/Odometry.py	66.66%	1 Missing ⚠️
...tree/g1/blueprints/primitive/unitree_g1_onboard.py	87.50%	1 Missing ⚠️

📢 Thoughts on this report? Let us know!

[jeff-hykin]

@paul-nechifor (sorry needs re-approval after merge with main)