From 5bec749afbc28383d8533c938b25fc2633371db2 Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Fri, 20 Feb 2026 03:01:09 +0200 Subject: [PATCH 1/4] fix(unitree-go2): minor issues --- dimos/e2e_tests/test_person_follow.py | 6 ++---- dimos/e2e_tests/test_spatial_memory.py | 6 ++---- .../wavefront_frontier_goal_selector.py | 10 ++++++++++ .../unitree/go2/blueprints/basic/unitree_go2_basic.py | 2 +- 4 files changed, 15 insertions(+), 9 deletions(-) diff --git a/dimos/e2e_tests/test_person_follow.py b/dimos/e2e_tests/test_person_follow.py index 709f4e4511..abb9cfb4fa 100644 --- a/dimos/e2e_tests/test_person_follow.py +++ b/dimos/e2e_tests/test_person_follow.py @@ -64,10 +64,8 @@ def test_person_follow( ) -> None: start_blueprint("--mujoco-start-pos", "-6.18 0.96", "run", "unitree-go2-agentic") - lcm_spy.save_topic("/rpc/HumanInput/start/res") - lcm_spy.wait_for_saved_topic("/rpc/HumanInput/start/res", timeout=120.0) - lcm_spy.save_topic("/agent") - lcm_spy.wait_for_saved_topic_content("/agent", b"AIMessage", timeout=120.0) + lcm_spy.save_topic("/rpc/Agent/on_system_modules/res") + lcm_spy.wait_for_saved_topic("/rpc/Agent/on_system_modules/res", timeout=120.0) time.sleep(5) diff --git a/dimos/e2e_tests/test_spatial_memory.py b/dimos/e2e_tests/test_spatial_memory.py index 7c08800a6f..8b03a9915c 100644 --- a/dimos/e2e_tests/test_spatial_memory.py +++ b/dimos/e2e_tests/test_spatial_memory.py @@ -34,10 +34,8 @@ def test_spatial_memory_navigation( ) -> None: start_blueprint("run", "unitree-go2-agentic") - lcm_spy.save_topic("/rpc/HumanInput/start/res") - lcm_spy.wait_for_saved_topic("/rpc/HumanInput/start/res", timeout=120.0) - lcm_spy.save_topic("/agent") - lcm_spy.wait_for_saved_topic_content("/agent", b"AIMessage", timeout=120.0) + lcm_spy.save_topic("/rpc/Agent/on_system_modules/res") + lcm_spy.wait_for_saved_topic("/rpc/Agent/on_system_modules/res", timeout=120.0) time.sleep(5) diff --git a/dimos/navigation/frontier_exploration/wavefront_frontier_goal_selector.py b/dimos/navigation/frontier_exploration/wavefront_frontier_goal_selector.py index 71a082de27..3adfc1c598 100644 --- a/dimos/navigation/frontier_exploration/wavefront_frontier_goal_selector.py +++ b/dimos/navigation/frontier_exploration/wavefront_frontier_goal_selector.py @@ -735,6 +735,16 @@ def stop_exploration(self) -> bool: ): self.exploration_thread.join(timeout=2.0) + # Publish current location as goal to stop the robot. + if self.latest_odometry is not None: + goal = PoseStamped( + position=self.latest_odometry.position, + orientation=self.latest_odometry.orientation, + frame_id="world", + ts=self.latest_odometry.ts, + ) + self.goal_request.publish(goal) + logger.info("Stopped autonomous frontier exploration") return True diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py index 3cb84dfd9f..cfd53abe51 100644 --- a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py @@ -52,7 +52,7 @@ image_topic="/world/color_image", optical_frame="camera_optical", ), - "world/global_map": lambda grid: grid.to_rerun(voxel_size=0.1), + "world/global_map": lambda grid: grid.to_rerun(voxel_size=0.1, mode="boxes"), "world/navigation_costmap": lambda grid: grid.to_rerun( colormap="Accent", z_offset=0.015, From c64b048a946980264fb7705be2a8fc2f79b0ba83 Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Fri, 20 Feb 2026 03:01:09 +0200 Subject: [PATCH 2/4] fix(unitree-go2): minor issues --- dimos/agents/skills/navigation.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/dimos/agents/skills/navigation.py b/dimos/agents/skills/navigation.py index 481ee448af..322a09c2bb 100644 --- a/dimos/agents/skills/navigation.py +++ b/dimos/agents/skills/navigation.py @@ -63,6 +63,7 @@ def __init__(self) -> None: @rpc def start(self) -> None: + super().start() self._disposables.add(Disposable(self.color_image.subscribe(self._on_color_image))) self._disposables.add(Disposable(self.odom.subscribe(self._on_odom))) self._skill_started = True @@ -92,12 +93,12 @@ def tag_location(self, location_name: str) -> str: if not self._skill_started: raise ValueError(f"{self} has not been started.") - tf = self.tf.get("map", "base_link", time_tolerance=2.0) - if not tf: - return "Could not get the robot's current transform." - position = tf.translation - rotation = tf.rotation.to_euler() + if not self._latest_odom: + return "No odometry data received yet, cannot tag location." + + position = self._latest_odom.position + rotation = self._latest_odom.orientation location = RobotLocation( name=location_name, From 5e424f5546629fed397887a4e784db19dfc611e1 Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Fri, 20 Feb 2026 06:06:24 +0200 Subject: [PATCH 3/4] fix stand down/up --- dimos/core/__init__.py | 26 -------------------------- dimos/robot/unitree/connection.py | 24 ++++-------------------- 2 files changed, 4 insertions(+), 46 deletions(-) diff --git a/dimos/core/__init__.py b/dimos/core/__init__.py index 7586407d36..2b6296b623 100644 --- a/dimos/core/__init__.py +++ b/dimos/core/__init__.py @@ -1,7 +1,6 @@ from __future__ import annotations import multiprocessing as mp -import signal import time from typing import TYPE_CHECKING, cast @@ -266,31 +265,6 @@ def start(n: int | None = None, memory_limit: str = "auto") -> DimosCluster: ) patched_client = patchdask(client, cluster) - patched_client._shutting_down = False # type: ignore[attr-defined] - - # Signal handler with proper exit handling - def signal_handler(sig, frame) -> None: # type: ignore[no-untyped-def] - # If already shutting down, force exit - if patched_client._shutting_down: # type: ignore[attr-defined] - import os - - console.print("[red]Force exit!") - os._exit(1) - - patched_client._shutting_down = True # type: ignore[attr-defined] - console.print(f"[yellow]Shutting down (signal {sig})...") - - try: - patched_client.close_all() # type: ignore[attr-defined] - except Exception: - pass - - import sys - - sys.exit(0) - - signal.signal(signal.SIGINT, signal_handler) - signal.signal(signal.SIGTERM, signal_handler) return patched_client diff --git a/dimos/robot/unitree/connection.py b/dimos/robot/unitree/connection.py index 217c7e72b0..401cd71901 100644 --- a/dimos/robot/unitree/connection.py +++ b/dimos/robot/unitree/connection.py @@ -17,7 +17,7 @@ import functools import threading import time -from typing import TypeAlias +from typing import Any, TypeAlias import numpy as np from numpy.typing import NDArray @@ -34,7 +34,6 @@ WebRTCConnectionMethod, ) -from dimos.core import rpc from dimos.core.resource import Resource from dimos.msgs.geometry_msgs import Pose, Transform, Twist from dimos.msgs.sensor_msgs import Image, PointCloud2 @@ -228,7 +227,7 @@ def run_unsubscription() -> None: ) # Generic sync API call (we jump into the client thread) - def publish_request(self, topic: str, data: dict): # type: ignore[no-untyped-def, type-arg] + def publish_request(self, topic: str, data: dict[Any, Any]) -> Any: future = asyncio.run_coroutine_threadsafe( self.conn.datachannel.pub_sub.publish_request_new(topic, data), self.loop ) @@ -275,25 +274,11 @@ def video_stream(self) -> Observable[Image]: def lowstate_stream(self) -> Observable[LowStateMsg]: return backpressure(self.unitree_sub_stream(RTC_TOPIC["LOW_STATE"])) - def standup_ai(self) -> bool: - return self.publish_request(RTC_TOPIC["SPORT_MOD"], {"api_id": SPORT_CMD["BalanceStand"]}) # type: ignore[no-any-return] - - def standup_normal(self) -> bool: - self.publish_request(RTC_TOPIC["SPORT_MOD"], {"api_id": SPORT_CMD["StandUp"]}) - time.sleep(0.5) - self.publish_request(RTC_TOPIC["SPORT_MOD"], {"api_id": SPORT_CMD["RecoveryStand"]}) - return True - - @rpc def standup(self) -> bool: - if self.mode == "ai": - return self.standup_ai() - else: - return self.standup_normal() + return bool(self.publish_request(RTC_TOPIC["SPORT_MOD"], {"api_id": SPORT_CMD["StandUp"]})) - @rpc def liedown(self) -> bool: - return self.publish_request(RTC_TOPIC["SPORT_MOD"], {"api_id": SPORT_CMD["StandDown"]}) # type: ignore[no-any-return] + return bool(self.publish_request(RTC_TOPIC["SPORT_MOD"], {"api_id": SPORT_CMD["StandDown"]})) async def handstand(self): # type: ignore[no-untyped-def] return self.publish_request( @@ -301,7 +286,6 @@ async def handstand(self): # type: ignore[no-untyped-def] {"api_id": SPORT_CMD["Standup"], "parameter": {"data": True}}, ) - @rpc def color(self, color: VUI_COLOR = VUI_COLOR.RED, colortime: int = 60) -> bool: return self.publish_request( # type: ignore[no-any-return] RTC_TOPIC["VUI"], From e0ee3e65c21440665f5b10ce81325125a38d6766 Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Fri, 20 Feb 2026 07:09:44 +0200 Subject: [PATCH 4/4] fix mypy --- dimos/robot/unitree/connection.py | 4 +++- dimos/simulation/engines/mujoco_engine.py | 2 +- pyproject.toml | 2 +- 3 files changed, 5 insertions(+), 3 deletions(-) diff --git a/dimos/robot/unitree/connection.py b/dimos/robot/unitree/connection.py index 401cd71901..f3f8ffaafb 100644 --- a/dimos/robot/unitree/connection.py +++ b/dimos/robot/unitree/connection.py @@ -278,7 +278,9 @@ def standup(self) -> bool: return bool(self.publish_request(RTC_TOPIC["SPORT_MOD"], {"api_id": SPORT_CMD["StandUp"]})) def liedown(self) -> bool: - return bool(self.publish_request(RTC_TOPIC["SPORT_MOD"], {"api_id": SPORT_CMD["StandDown"]})) + return bool( + self.publish_request(RTC_TOPIC["SPORT_MOD"], {"api_id": SPORT_CMD["StandDown"]}) + ) async def handstand(self): # type: ignore[no-untyped-def] return self.publish_request( diff --git a/dimos/simulation/engines/mujoco_engine.py b/dimos/simulation/engines/mujoco_engine.py index 6dc8e46595..ddaaa25ad3 100644 --- a/dimos/simulation/engines/mujoco_engine.py +++ b/dimos/simulation/engines/mujoco_engine.py @@ -21,7 +21,7 @@ from typing import TYPE_CHECKING import mujoco -import mujoco.viewer as viewer # type: ignore[import-untyped] +import mujoco.viewer as viewer # type: ignore[import-untyped,import-not-found] from dimos.simulation.engines.base import SimulationEngine from dimos.simulation.utils.xml_parser import JointMapping, build_joint_mappings diff --git a/pyproject.toml b/pyproject.toml index 1b0c40acd3..b810dd2ee4 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -360,6 +360,7 @@ module = [ "mujoco_playground.*", "nav_msgs.*", "open_clip", + "pinocchio", "piper_sdk.*", "plotext", "plum.*", @@ -375,7 +376,6 @@ module = [ "torchreid", "ultralytics.*", "unitree_webrtc_connect.*", - "watchdog.*", "xarm.*", ] ignore_missing_imports = true