feat(g1-groot): navigation on the real G1 (Point-LIO + raytracing costmap)

[Nabla7]

Nav for the real G1 in the groot WBC blueprint: MID-360 -> Point-LIO → raytracing voxel map -> height costmap -> replanning A*, driven through the coordinator's twist_command (no sport mode). Same front-end and mapper as go2 nav_3d.

Split out of #2477 so it can land on its own; that branch rebases after.

Two small fixes are included because nav is unusable on the robot without them:

• mode_machine: adopt the firmware-reported value at first LowState. It varies per boot, and a mismatch means the firmware silently rejects every command.
• 100 Hz tick / decimation 2 and n_workers=10: the Orin can't hold a 500 Hz python tick and the policy starves.

Voxel size is 0.08 to match go2 — at 0.05 the raytracer pegged the Orin and fell ~60 s behind its inputs, registering scans against stale odometry (the "map rotates with the robot" issue). Verified on the robot: map stays correct over long runs, goal nav works. Sim path untouched.

Point-LIO IPs default to the G1's onboard NIC; override with LIDAR_HOST_IP / LIDAR_IP.

[codecov]

❌ 1 Tests Failed:

Tests completed	Failed	Passed	Skipped
2813	1	2812	91

View the full list of 1 ❄️ flaky test(s)

dimos.e2e_tests.test_dimsim_spatial_memory::test_go_to_the_bed

Flake rate in main: 18.32% (Passed 107 times, Failed 24 times)

Stack Traces | 550s run time

lcm_spy = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x7b0947ed8ad0>
start_blueprint = <function start_blueprint.<locals>.set_name_and_start at 0x7b0945919120>
human_input = <function human_input.<locals>.send_human_input at 0x7b09459191c0>
dim_sim = <dimos.e2e_tests.dim_sim_client.DimSimClient object at 0x7b094748fdd0>
explore_house = <function explore_house.<locals>.explore at 0x7b09459196c0>

    @pytest.mark.self_hosted_large
    def test_go_to_the_bed(lcm_spy, start_blueprint, human_input, dim_sim, explore_house) -> None:
        start_blueprint(
            "run",
            "unitree-go2-agentic",
            simulator="dimsim",
        )
        lcm_spy.save_topic(".../McpClient/on_system_modules/res")
        lcm_spy.wait_for_saved_topic(".../McpClient/on_system_modules/res", timeout=1200.0)
    
        explore_house()
    
        human_input("go to the bed")
    
>       lcm_spy.wait_until_odom_position(-3.567, -1.332, threshold=2, timeout=180)

dim_sim    = <dimos.e2e_tests.dim_sim_client.DimSimClient object at 0x7b094748fdd0>
explore_house = <function explore_house.<locals>.explore at 0x7b09459196c0>
human_input = <function human_input.<locals>.send_human_input at 0x7b09459191c0>
lcm_spy    = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x7b0947ed8ad0>
start_blueprint = <function start_blueprint.<locals>.set_name_and_start at 0x7b0945919120>

dimos/e2e_tests/test_dimsim_spatial_memory.py:32: 
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 
dimos/e2e_tests/lcm_spy.py:167: in wait_until_odom_position
    self.wait_for_message_result(
        predicate  = <function LcmSpy.wait_until_odom_position.<locals>.predicate at 0x7b09459198a0>
        self       = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x7b0947ed8ad0>
        threshold  = 2
        timeout    = 180
        x          = -3.567
        y          = -1.332
dimos/e2e_tests/lcm_spy.py:153: in wait_for_message_result
    wait_until(
        event      = <threading.Event at 0x7b0947498050: unset>
        fail_message = 'Failed to get to position x=-3.567, y=-1.332'
        listener   = <function LcmSpy.wait_for_message_result.<locals>.listener at 0x7b0945919760>
        predicate  = <function LcmSpy.wait_until_odom_position.<locals>.predicate at 0x7b09459198a0>
        self       = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x7b0947ed8ad0>
        timeout    = 180
        topic      = '/odom#geometry_msgs.PoseStamped'
        type       = <class 'dimos.msgs.geometry_msgs.PoseStamped.PoseStamped'>
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 

predicate = <bound method Event.is_set of <threading.Event at 0x7b0947498050: unset>>

    def wait_until(
        predicate: Callable[[], bool],
        *,
        timeout: float,
        interval: float = 0.1,
        message: str | None = None,
    ) -> None:
        """Poll ``predicate`` until it returns truthy or ``timeout`` elapses."""
        deadline = time.monotonic() + timeout
        while time.monotonic() < deadline:
            if predicate():
                return
            time.sleep(interval)
>       raise TimeoutError(message or f"Timed out after {timeout}s waiting for condition")
E       TimeoutError: Failed to get to position x=-3.567, y=-1.332

deadline   = 177844.219087307
interval   = 0.1
message    = 'Failed to get to position x=-3.567, y=-1.332'
predicate  = <bound method Event.is_set of <threading.Event at 0x7b0947498050: unset>>
timeout    = 180

.../utils/testing/waiting.py:35: TimeoutError

To view more test analytics, go to the Test Analytics Dashboard
_{📋 Got 3 mins? Take this short survey to help us improve Test Analytics.}

[greptile-apps]

Greptile Summary

This PR adds real-hardware G1 navigation through the existing WBC coordinator. The main changes are:

• Point-LIO, raytracing voxel mapping, height costmaps, and replanning A* for real hardware.
• Lower real-hardware tick rate, decimation, and worker settings for the Orin.
• Rerun frame and costmap rendering updates for the real MID-360 odometry frame.
• Firmware-reported mode_machine adoption in the whole-body connection.

Confidence Score: 4/5

This is close, but the command mode handling should be fixed before merging.

• The Point-LIO configuration path no longer appears to force the hardcoded lidar addresses.
• The whole-body connection can still keep a stale startup mode_machine value.
• When that stale value is used, firmware can silently reject motion commands.

dimos/robot/unitree/g1/wholebody_connection.py

Important Files Changed

Filename	Overview
dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py	Adds the real-hardware navigation stack and real-hardware Rerun overrides.
dimos/robot/unitree/g1/g1_rerun.py	Makes the G1 costmap render height configurable while keeping the existing default.
dimos/robot/unitree/g1/wholebody_connection.py	Adopts firmware-reported mode_machine, but still latches the first observed value permanently.

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

[greptile-apps]

First LowState Becomes Permanent

If the first LowState after startup reports a transient or not-yet-set mode_machine, this one-shot path stores that value and never checks later samples. Subsequent commands are stamped with the stale value and can be silently rejected for the rest of the run, leaving the robot unresponsive until restart.

[jeff-hykin]

The URDF should contain the flip, but also the flip shouldn't matter cause we gravity align by default.

If its tested and working though I think ignore this comment by me

[jeff-hykin]

LGTM so long as its working on hardware.

[greptile-apps]

First sample still sticks

_mode_machine_verified is set on the first LowState before the reported value is known to be stable. If the first frame carries a transient/default value, or briefly matches the configured default before firmware reports the real boot-specific value, every later LowState returns early and commands keep using the stale mode. The robot can still receive commands stamped with a mode the firmware silently rejects.