feat: go2 3d nav

[aclauer]

merge #2483 first

Problem

Closes DIM-XXX

Solution

How to Test

Contributor License Agreement

• I have read and approved the CLA.

[codecov]

❌ 1 Tests Failed:

Tests completed	Failed	Passed	Skipped
2185	1	2184	70

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

dimos.e2e_tests.test_dimsim_walk_forward::test_walk_forward

Flake rate in main: 42.86% (Passed 8 times, Failed 6 times)

Stack Traces | 208s run time

lcm_spy = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x7eaa114d41d0>
start_blueprint = <function start_blueprint.<locals>.set_name_and_start at 0x7eaa0f3c5620>
human_input = <function human_input.<locals>.send_human_input at 0x7eaa0f3c6f20>
dim_sim = <dimos.e2e_tests.dim_sim_client.DimSimClient object at 0x7eaa113270b0>

    @pytest.mark.self_hosted_large
    def test_walk_forward(lcm_spy, start_blueprint, human_input, dim_sim) -> None:
        start_blueprint(
            "run",
            "--disable",
            "spatial-memory",
            "--disable",
            "security-module",
            "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)
    
        origin_x, origin_y = 1, 2
        dim_sim.set_agent_position(origin_x, origin_y)
    
        human_input("move forward 3 meter")
    
>       lcm_spy.wait_until_odom_position(origin_x + 3, origin_y, threshold=0.4, timeout=120)

dim_sim    = <dimos.e2e_tests.dim_sim_client.DimSimClient object at 0x7eaa113270b0>
human_input = <function human_input.<locals>.send_human_input at 0x7eaa0f3c6f20>
lcm_spy    = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x7eaa114d41d0>
origin_x   = 1
origin_y   = 2
start_blueprint = <function start_blueprint.<locals>.set_name_and_start at 0x7eaa0f3c5620>

dimos/e2e_tests/test_dimsim_walk_forward.py:37: 
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 
dimos/e2e_tests/lcm_spy.py:182: in wait_until_odom_position
    self.wait_for_message_result(
        predicate  = <function LcmSpy.wait_until_odom_position.<locals>.predicate at 0x7eaa0f3c6ca0>
        self       = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x7eaa114d41d0>
        threshold  = 0.4
        timeout    = 120
        x          = 4
        y          = 2
dimos/e2e_tests/lcm_spy.py:168: in wait_for_message_result
    self.wait_until(
        event      = <threading.Event at 0x7eaa11348a70: unset>
        fail_message = 'Failed to get to position x=4, y=2'
        listener   = <function LcmSpy.wait_for_message_result.<locals>.listener at 0x7eaa0f3c6d40>
        predicate  = <function LcmSpy.wait_until_odom_position.<locals>.predicate at 0x7eaa0f3c6ca0>
        self       = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x7eaa114d41d0>
        timeout    = 120
        topic      = '/odom#geometry_msgs.PoseStamped'
        type       = <class 'dimos.msgs.geometry_msgs.PoseStamped.PoseStamped'>
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 

self = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x7eaa114d41d0>

    def wait_until(
        self,
        *,
        condition: Callable[[], bool],
        timeout: float,
        error_message: str,
        poll_interval: float = 0.1,
    ) -> None:
        start_time = time.time()
        while time.time() - start_time < timeout:
            if condition():
                return
            time.sleep(poll_interval)
>       raise TimeoutError(error_message)
E       TimeoutError: Failed to get to position x=4, y=2

condition  = <bound method Event.is_set of <threading.Event at 0x7eaa11348a70: unset>>
error_message = 'Failed to get to position x=4, y=2'
poll_interval = 0.1
self       = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x7eaa114d41d0>
start_time = 1781664374.6884544
timeout    = 120

dimos/e2e_tests/lcm_spy.py:105: 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.}