Rebase ivan g1

dimos/agents2/__init__.py

@@ -7,7 +7,7 @@
     ToolMessage,
 )
 
-from dimos.agents2.agent import Agent
+from dimos.agents2.agent import Agent, deploy
 from dimos.agents2.spec import AgentSpec
 from dimos.protocol.skill.skill import skill
 from dimos.protocol.skill.type import Output, Reducer, Stream

dimos/agents2/agent.py

@@ -28,10 +28,15 @@
     ToolMessage,
 )
 
-from dimos.agents2.spec import AgentSpec
+from dimos.agents2.spec import AgentSpec, Model, Provider
 from dimos.agents2.system_prompt import get_system_prompt
-from dimos.core import rpc
-from dimos.protocol.skill.coordinator import SkillCoordinator, SkillState, SkillStateDict
+from dimos.core import DimosCluster, rpc
+from dimos.protocol.skill.coordinator import (
+    SkillContainer,
+    SkillCoordinator,
+    SkillState,
+    SkillStateDict,
+)
 from dimos.protocol.skill.type import Output
 from dimos.utils.logging_config import setup_logger
 
@@ -284,8 +289,8 @@ def _get_state() -> str:
                 if msg.tool_calls:
                     self.execute_tool_calls(msg.tool_calls)
 
-                print(self)
-                print(self.coordinator)
+                # print(self)
+                # print(self.coordinator)
 
                 self._write_debug_history_file()
 
@@ -371,4 +376,38 @@ def stop(self) -> None:
 llm_agent = LlmAgent.blueprint
 
 
-__all__ = ["Agent", "llm_agent"]
+def deploy(
+    dimos: DimosCluster,
+    system_prompt: str = "You are a helpful assistant for controlling a Unitree Go2 robot.",
+    model: Model = Model.GPT_4O,
+    provider: Provider = Provider.OPENAI,
+    skill_containers: list[SkillContainer] | None = None,
+) -> Agent:
+    from dimos.agents2.cli.human import HumanInput
+
+    if skill_containers is None:
+        skill_containers = []
+    agent = dimos.deploy(
+        Agent,
+        system_prompt=system_prompt,
+        model=model,
+        provider=provider,
+    )
+
+    human_input = dimos.deploy(HumanInput)
+    human_input.start()
+
+    agent.register_skills(human_input)
+
+    for skill_container in skill_containers:
+        print("Registering skill container:", skill_container)
+        agent.register_skills(skill_container)
+
+    agent.run_implicit_skill("human")
+    agent.start()
+    agent.loop_thread()
+
+    return agent
+
+
+__all__ = ["Agent", "deploy", "llm_agent"]

dimos/agents2/skills/navigation.py

@@ -21,15 +21,14 @@
 from dimos.core.stream import In
 from dimos.models.qwen.video_query import BBox
 from dimos.models.vl.qwen import QwenVlModel
-from dimos.msgs.geometry_msgs import PoseStamped
+from dimos.msgs.geometry_msgs import PoseStamped, Quaternion, Vector3
 from dimos.msgs.geometry_msgs.Vector3 import make_vector3
 from dimos.msgs.sensor_msgs import Image
 from dimos.navigation.bt_navigator.navigator import NavigatorState
 from dimos.navigation.visual.query import get_object_bbox_from_image
 from dimos.protocol.skill.skill import skill
 from dimos.types.robot_location import RobotLocation
 from dimos.utils.logging_config import setup_logger
-from dimos.utils.transform_utils import euler_to_quaternion, quaternion_to_euler
 
 logger = setup_logger(__file__)
 
@@ -145,7 +144,7 @@ def set_WavefrontFrontierExplorer_is_exploration_active(self, callable: RpcCall)
         self._is_exploration_active.set_rpc(self.rpc)
 
     @skill()
-    def tag_location_in_spatial_memory(self, location_name: str) -> str:
+    def tag_location(self, location_name: str) -> str:
         """Tag this location in the spatial memory with a name.
 
         This associates the current location with the given name in the spatial memory, allowing you to navigate back to it.
@@ -159,15 +158,12 @@ def tag_location_in_spatial_memory(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."
 
-        if not self._latest_odom:
-            return "Error: No odometry data available to tag the location."
-
-        if not self._tag_location:
-            return "Error: The SpatialMemory module is not connected."
-
-        position = self._latest_odom.position
-        rotation = quaternion_to_euler(self._latest_odom.orientation)
+        position = tf.translation
+        rotation = tf.rotation.to_euler()
 
         location = RobotLocation(
             name=location_name,
@@ -179,7 +175,15 @@ def tag_location_in_spatial_memory(self, location_name: str) -> str:
             return f"Error: Failed to store '{location_name}' in the spatial memory"
 
         logger.info(f"Tagged {location}")
-        return f"The current location has been tagged as '{location_name}'."
+        return f"Tagged '{location_name}': ({position.x},{position.y})."
+
+    def _navigate_to_object(self, query: str) -> str | None:
+        position = self.detection_module.nav_vlm(query)
+        print("Object position from VLM:", position)
+        if not position:
+            return None
+        self.nav.navigate_to(position)
+        return f"Arrived to object matching '{query}' in view."
 
     @skill()
     def navigate_with_text(self, query: str) -> str:
@@ -196,7 +200,6 @@ def navigate_with_text(self, query: str) -> str:
 
         if not self._skill_started:
             raise ValueError(f"{self} has not been started.")
-
         success_msg = self._navigate_by_tagged_location(query)
         if success_msg:
             return success_msg
@@ -225,10 +228,11 @@ def _navigate_by_tagged_location(self, query: str) -> str | None:
         if not robot_location:
             return None
 
+        print("Found tagged location:", robot_location)
         goal_pose = PoseStamped(
             position=make_vector3(*robot_location.position),
-            orientation=euler_to_quaternion(make_vector3(*robot_location.rotation)),
-            frame_id="world",
+            orientation=Quaternion.from_euler(Vector3(*robot_location.rotation)),
+            frame_id="map",
         )
 
         result = self._navigate_to(goal_pose)
@@ -336,6 +340,7 @@ def _navigate_using_semantic_map(self, query: str) -> str:
 
         goal_pose = self._get_goal_pose_from_result(best_match)
 
+        print("Goal pose for semantic nav:", goal_pose)
         if not goal_pose:
             return f"Found a result for '{query}' but it didn't have a valid position."
 
@@ -423,16 +428,17 @@ def _get_goal_pose_from_result(self, result: dict[str, Any]) -> PoseStamped | No
         metadata = result.get("metadata")
         if not metadata:
             return None
-
+        print(metadata)
         first = metadata[0]
+        print(first)
         pos_x = first.get("pos_x", 0)
         pos_y = first.get("pos_y", 0)
         theta = first.get("rot_z", 0)
 
         return PoseStamped(
             position=make_vector3(pos_x, pos_y, 0),
-            orientation=euler_to_quaternion(make_vector3(0, 0, theta)),
-            frame_id="world",
+            orientation=Quaternion.from_euler(make_vector3(0, 0, theta)),
+            frame_id="map",
         )
 
 

dimos/agents2/skills/test_navigation.py

@@ -17,6 +17,7 @@
 from dimos.utils.transform_utils import euler_to_quaternion
 
 
+# @pytest.mark.skip
 def test_stop_movement(create_navigation_agent, navigation_skill_container, mocker) -> None:
     navigation_skill_container._cancel_goal = mocker.Mock()
     navigation_skill_container._stop_exploration = mocker.Mock()

dimos/conftest.py

@@ -33,6 +33,17 @@ def event_loop():
 _skip_for = ["lcm", "heavy", "ros"]
 
 
+@pytest.fixture(scope="module")
+def dimos_cluster():
+    from dimos.core import start
+
+    dimos = start(4)
+    try:
+        yield dimos
+    finally:
+        dimos.stop()
+
+
 @pytest.hookimpl()
 def pytest_sessionfinish(session):
     """Track threads that exist at session start - these are not leaks."""

dimos/core/__init__.py

@@ -2,7 +2,7 @@
 
 import multiprocessing as mp
 import signal
-from typing import Optional
+import time
 
 from dask.distributed import Client, LocalCluster
 from rich.console import Console
@@ -166,8 +166,6 @@ def close_all() -> None:
             return
         dask_client._closed = True
 
-        import time
-
         # Stop all SharedMemory transports before closing Dask
         # This prevents the "leaked shared_memory objects" warning and hangs
         try:
@@ -223,15 +221,18 @@ def close_all() -> None:
     dask_client.check_worker_memory = check_worker_memory
     dask_client.stop = lambda: dask_client.close()
     dask_client.close_all = close_all
-    return dask_client
+    return dask_client  # type: ignore[return-value]
 
 
-def start(n: int | None = None, memory_limit: str = "auto") -> Client:
+def start(n: int | None = None, memory_limit: str = "auto") -> DimosCluster:
     """Start a Dask LocalCluster with specified workers and memory limits.
 
     Args:
         n: Number of workers (defaults to CPU count)
         memory_limit: Memory limit per worker (e.g., '4GB', '2GiB', or 'auto' for Dask's default)
+
+    Returns:
+        DimosCluster: A patched Dask client with deploy(), check_worker_memory(), stop(), and close_all() methods
     """
 
     console = Console()
@@ -280,3 +281,11 @@ def signal_handler(sig, frame) -> None:
     signal.signal(signal.SIGTERM, signal_handler)
 
     return patched_client
+
+
+def wait_exit() -> None:
+    while True:
+        try:
+            time.sleep(1)
+        except KeyboardInterrupt:
+            print("exiting...")

dimos/core/stream.py

@@ -140,6 +140,11 @@ def __init__(self, *argv, **kwargs) -> None:
     def transport(self) -> Transport[T]:
         return self._transport
 
+    @transport.setter
+    def transport(self, value: Transport[T]) -> None:
+        # just for type checking
+        ...
+
     @property
     def state(self) -> State:
         return State.UNBOUND if self.owner is None else State.READY
@@ -212,6 +217,15 @@ def transport(self) -> Transport[T]:
             self._transport = self.connection.transport
         return self._transport
 
+    @transport.setter
+    def transport(self, value: Transport[T]) -> None:
+        # just for type checking
+        ...
+
+    def connect(self, value: Out[T]) -> None:
+        # just for type checking
+        ...
+
     @property
     def state(self) -> State:
         return State.UNBOUND if self.owner is None else State.READY

dimos/hardware/camera/module.py

@@ -23,12 +23,10 @@
 from reactivex.disposable import Disposable
 from reactivex.observable import Observable
 
+from dimos import spec
 from dimos.agents2 import Output, Reducer, Stream, skill
-from dimos.core import Module, Out, rpc
-from dimos.core.module import Module, ModuleConfig
-from dimos.hardware.camera.spec import (
-    CameraHardware,
-)
+from dimos.core import Module, ModuleConfig, Out, rpc
+from dimos.hardware.camera.spec import CameraHardware
 from dimos.hardware.camera.webcam import Webcam
 from dimos.msgs.geometry_msgs import Quaternion, Transform, Vector3
 from dimos.msgs.sensor_msgs import Image
@@ -49,13 +47,14 @@ class CameraModuleConfig(ModuleConfig):
     frame_id: str = "camera_link"
     transform: Transform | None = field(default_factory=default_transform)
     hardware: Callable[[], CameraHardware] | CameraHardware = Webcam
+    frequency: float = 5.0
 
 
-class CameraModule(Module):
+class CameraModule(Module, spec.Camera):
     image: Out[Image] = None
-    camera_info: Out[CameraInfo] = None
+    camera_info_stream: Out[CameraInfo] = None
 
-    hardware: CameraHardware = None
+    hardware: Callable[[], CameraHardware] | CameraHardware = None
     _module_subscription: Disposable | None = None
     _camera_info_subscription: Disposable | None = None
     _skill_stream: Observable[Image] | None = None
@@ -65,6 +64,10 @@ class CameraModule(Module):
     def __init__(self, *args, **kwargs) -> None:
         super().__init__(*args, **kwargs)
 
+    @property
+    def camera_info(self) -> CameraInfo:
+        return self.hardware.camera_info
+
     @rpc
     def start(self) -> str:
         if callable(self.config.hardware):
@@ -75,7 +78,7 @@ def start(self) -> str:
         if self._module_subscription:
             return "already started"
 
-        stream = self.hardware.image_stream().pipe(sharpness_barrier(5))
+        stream = self.hardware.image_stream().pipe(sharpness_barrier(self.config.frequency))
 
         # camera_info_stream = self.camera_info_stream(frequency=5.0)
 
@@ -108,7 +111,7 @@ def video_stream(self) -> Image:
 
         yield from iter(_queue.get, None)
 
-    def camera_info_stream(self, frequency: float = 5.0) -> Observable[CameraInfo]:
+    def camera_info_stream(self, frequency: float = 1.0) -> Observable[CameraInfo]:
         def camera_info(_) -> CameraInfo:
             self.hardware.camera_info.ts = time.time()
             return self.hardware.camera_info
@@ -122,6 +125,7 @@ def stop(self) -> None:
         if self._camera_info_subscription:
             self._camera_info_subscription.dispose()
             self._camera_info_subscription = None
+
         # Also stop the hardware if it has a stop method
         if self.hardware and hasattr(self.hardware, "stop"):
             self.hardware.stop()

dimos/models/segmentation/segment_utils.py

@@ -53,7 +53,7 @@ def sample_points_from_heatmap(heatmap, original_size: int, num_points: int=5, p
     )
 
     sampled_coords = np.array(np.unravel_index(sampled_indices, attn.shape)).T
-    medoid, sampled_coords = find_medoid_and_closest_points(sampled_coords)
+    _medoid, sampled_coords = find_medoid_and_closest_points(sampled_coords)
     pts = []
     for pt in sampled_coords.tolist():
         x, y = pt

dimos/models/vl/test_models.py

@@ -8,7 +8,6 @@
 from dimos.models.vl.moondream import MoondreamVlModel
 from dimos.models.vl.qwen import QwenVlModel
 from dimos.msgs.sensor_msgs import Image
-from dimos.perception.detection.detectors.yolo import Yolo2DDetector
 from dimos.perception.detection.type import ImageDetections2D
 from dimos.utils.data import get_data