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
2167	1	2166	70

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: 26.67% (Passed 11 times, Failed 4 times)

Stack Traces | 578s run time

lcm_spy = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x766fcac13fb0>
start_blueprint = <function start_blueprint.<locals>.set_name_and_start at 0x766fc87877e0>
human_input = <function human_input.<locals>.send_human_input at 0x766fc87a0180>
dim_sim = <dimos.e2e_tests.dim_sim_client.DimSimClient object at 0x766fca42cc50>
explore_house = <function explore_house.<locals>.explore at 0x766fc87a07c0>

    @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 0x766fca42cc50>
explore_house = <function explore_house.<locals>.explore at 0x766fc87a07c0>
human_input = <function human_input.<locals>.send_human_input at 0x766fc87a0180>
lcm_spy    = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x766fcac13fb0>
start_blueprint = <function start_blueprint.<locals>.set_name_and_start at 0x766fc87877e0>

dimos/e2e_tests/test_dimsim_spatial_memory.py:32: 
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 
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 0x766fc8787d80>
        self       = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x766fcac13fb0>
        threshold  = 2
        timeout    = 180
        x          = -3.567
        y          = -1.332
dimos/e2e_tests/lcm_spy.py:168: in wait_for_message_result
    self.wait_until(
        event      = <threading.Event at 0x766fca42e4b0: unset>
        fail_message = 'Failed to get to position x=-3.567, y=-1.332'
        listener   = <function LcmSpy.wait_for_message_result.<locals>.listener at 0x766fc87a09a0>
        predicate  = <function LcmSpy.wait_until_odom_position.<locals>.predicate at 0x766fc8787d80>
        self       = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x766fcac13fb0>
        timeout    = 180
        topic      = '/odom#geometry_msgs.PoseStamped'
        type       = <class 'dimos.msgs.geometry_msgs.PoseStamped.PoseStamped'>
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 

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

    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=-3.567, y=-1.332

condition  = <bound method Event.is_set of <threading.Event at 0x766fca42e4b0: unset>>
error_message = 'Failed to get to position x=-3.567, y=-1.332'
poll_interval = 0.1
self       = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x766fcac13fb0>
start_time = 1781571894.575893
timeout    = 180

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.}