G1 - local zed via webcam module - 2d detection spatial localization

.envrc

@@ -0,0 +1 @@
+.envrc.nix

.gitignore

@@ -45,3 +45,6 @@ FastSAM-x.pt
 yolo11n.pt
 
 /thread_monitor_report.csv
+
+# symlink one of .envrc.* if you'd like to use
+.envrc

dimos/agents2/temp/webcam_agent.py

@@ -25,11 +25,15 @@
 from pathlib import Path
 
 from dotenv import load_dotenv
+from dimos.hardware.camera import zed
+from dimos.msgs.geometry_msgs import Quaternion, Transform, Vector3
 
 from dimos.agents2.cli.human import HumanInput
 
 # Add parent directories to path
 sys.path.insert(0, str(Path(__file__).parent.parent.parent.parent))
+from dimos.hardware.camera.module import CameraModule
+from dimos.hardware.camera.webcam import Webcam
 
 from threading import Thread
 
@@ -39,8 +43,9 @@
 from dimos.agents2 import Agent, Output, Reducer, Stream, skill
 from dimos.agents2.spec import Model, Provider
 from dimos.core import LCMTransport, Module, pLCMTransport, start
-from dimos.hardware.webcam import ColorCameraModule, Webcam
-from dimos.msgs.sensor_msgs import Image
+
+# from dimos.hardware.webcam import ColorCameraModule, Webcam
+from dimos.msgs.sensor_msgs import Image, CameraInfo
 from dimos.protocol.skill.test_coordinator import SkillContainerTest
 from dimos.robot.unitree_webrtc.unitree_go2 import UnitreeGo2
 from dimos.robot.unitree_webrtc.unitree_skill_container import UnitreeSkillContainer
@@ -110,7 +115,24 @@ def main():
     )
 
     testcontainer = dimos.deploy(SkillContainerTest)
-    webcam = dimos.deploy(ColorCameraModule, hardware=lambda: Webcam(camera_index=0))
+    webcam = dimos.deploy(
+        CameraModule,
+        transform=Transform(
+            translation=Vector3(0.0, 0.0, 0.0),
+            rotation=Quaternion(0.0, 0.0, 0.0, 1.0),
+            frame_id="base_link",
+            child_frame_id="camera_link",
+        ),
+        hardware=lambda: Webcam(
+            camera_index=0,
+            frequency=15,
+            stereo_slice="left",
+            camera_info=zed.CameraInfo.SingleWebcam,
+        ),
+    )
+
+    webcam.camera_info.transport = LCMTransport("/camera_info", CameraInfo)
+
     webcam.image.transport = LCMTransport("/image", Image)
 
     webcam.start()

dimos/core/module.py

@@ -108,7 +108,8 @@ def _close_rpc(self):
     @property
     def tf(self):
         if self._tf is None:
-            self._tf = self.config.tf_transport()
+            # self._tf = self.config.tf_transport()
+            self._tf = LCMTF()
         return self._tf
 
     @tf.setter

dimos/hardware/camera/module.py

@@ -0,0 +1,127 @@
+# Copyright 2025 Dimensional Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import queue
+import time
+from dataclasses import dataclass, field
+from typing import Any, Callable, Generic, Literal, Optional, Protocol, TypeVar
+
+import reactivex as rx
+from dimos_lcm.sensor_msgs import CameraInfo
+from reactivex import operators as ops
+from reactivex.disposable import Disposable
+from reactivex.observable import Observable
+
+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.hardware.camera.webcam import Webcam, WebcamConfig
+from dimos.msgs.geometry_msgs import Quaternion, Transform, Vector3
+from dimos.msgs.sensor_msgs import Image
+from dimos.msgs.sensor_msgs.Image import Image, sharpness_barrier
+
+default_transform = lambda: Transform(
+    translation=Vector3(0.0, 0.0, 0.0),
+    rotation=Quaternion(0.0, 0.0, 0.0, 1.0),
+    frame_id="base_link",
+    child_frame_id="camera_link",
+)
+
+
+@dataclass
+class CameraModuleConfig(ModuleConfig):
+    frame_id: str = "camera_link"
+    transform: Optional[Transform] = field(default_factory=default_transform)
+    hardware: Callable[[], CameraHardware] | CameraHardware = Webcam
+
+
+class CameraModule(Module):
+    image: Out[Image] = None
+    camera_info: Out[CameraInfo] = None
+
+    hardware: CameraHardware = None
+    _module_subscription: Optional[Disposable] = None
+    _camera_info_subscription: Optional[Disposable] = None
+    _skill_stream: Optional[Observable[Image]] = None
+
+    default_config = CameraModuleConfig
+
+    def __init__(self, *args, **kwargs):
+        super().__init__(*args, **kwargs)
+
+    @rpc
+    def start(self):
+        if callable(self.config.hardware):
+            self.hardware = self.config.hardware()
+        else:
+            self.hardware = self.config.hardware
+
+        if self._module_subscription:
+            return "already started"
+
+        stream = self.hardware.image_stream().pipe(sharpness_barrier(5))
+
+        # camera_info_stream = self.camera_info_stream(frequency=5.0)
+
+        def publish_info(camera_info: CameraInfo):
+            self.camera_info.publish(camera_info)
+
+            if self.config.transform is None:
+                return
+
+            camera_link = self.config.transform
+            camera_link.ts = camera_info.ts
+            camera_optical = Transform(
+                translation=Vector3(0.0, 0.0, 0.0),
+                rotation=Quaternion(-0.5, 0.5, -0.5, 0.5),
+                frame_id="camera_link",
+                child_frame_id="camera_optical",
+                ts=camera_link.ts,
+            )
+
+            self.tf.publish(camera_link, camera_optical)
+
+        self._camera_info_subscription = self.camera_info_stream().subscribe(publish_info)
+        self._module_subscription = stream.subscribe(self.image.publish)
+
+    @skill(stream=Stream.passive, output=Output.image, reducer=Reducer.latest)
+    def video_stream(self) -> Image:
+        """implicit video stream skill"""
+        _queue = queue.Queue(maxsize=1)
+        self.hardware.image_stream().subscribe(_queue.put)
+
+        for image in iter(_queue.get, None):
+            yield image
+
+    def camera_info_stream(self, frequency: float = 5.0) -> Observable[CameraInfo]:
+        def camera_info(_) -> CameraInfo:
+            self.hardware.camera_info.ts = time.time()
+            return self.hardware.camera_info
+
+        return rx.interval(1.0 / frequency).pipe(ops.map(camera_info))
+
+    def stop(self):
+        if self._module_subscription:
+            self._module_subscription.dispose()
+            self._module_subscription = 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()
+        super().stop()

dimos/hardware/camera/spec.py

@@ -0,0 +1,55 @@
+# Copyright 2025 Dimensional Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from abc import ABC, abstractmethod, abstractproperty
+from typing import Generic, Optional, Protocol, TypeVar
+
+from dimos_lcm.sensor_msgs import CameraInfo
+from reactivex.observable import Observable
+
+from dimos.msgs.sensor_msgs import Image
+from dimos.protocol.service import Configurable
+
+
+class CameraConfig(Protocol):
+    frame_id_prefix: Optional[str]
+
+
+CameraConfigT = TypeVar("CameraConfigT", bound=CameraConfig)
+
+
+class CameraHardware(ABC, Configurable[CameraConfigT], Generic[CameraConfigT]):
+    @abstractmethod
+    def image_stream(self) -> Observable[Image]:
+        pass
+
+    @abstractproperty
+    def camera_info(self) -> CameraInfo:
+        pass
+
+
+# This is an example, feel free to change spec for stereo cameras
+# e.g., separate camera_info or streams for left/right, etc.
+class StereoCameraHardware(ABC, Configurable[CameraConfigT], Generic[CameraConfigT]):
+    @abstractmethod
+    def image_stream(self) -> Observable[Image]:
+        pass
+
+    @abstractmethod
+    def depth_stream(self) -> Observable[Image]:
+        pass
+
+    @abstractproperty
+    def camera_info(self) -> CameraInfo:
+        pass

dimos/hardware/camera/test_webcam.py

@@ -0,0 +1,108 @@
+# Copyright 2025 Dimensional Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import time
+
+import pytest
+
+from dimos import core
+from dimos.hardware.camera import zed
+from dimos.hardware.camera.module import CameraModule
+from dimos.hardware.camera.webcam import Webcam
+from dimos.msgs.geometry_msgs import Quaternion, Transform, Vector3
+from dimos.msgs.sensor_msgs import CameraInfo, Image
+
+
+@pytest.mark.tool
+def test_streaming_single():
+    dimos = core.start(1)
+
+    camera = dimos.deploy(
+        CameraModule,
+        transform=Transform(
+            translation=Vector3(0.05, 0.0, 0.0),
+            rotation=Quaternion(0.0, 0.0, 0.0, 1.0),
+            frame_id="sensor",
+            child_frame_id="camera_link",
+        ),
+        hardware=lambda: Webcam(
+            stereo_slice="left",
+            camera_index=0,
+            frequency=15,
+            camera_info=zed.CameraInfo.SingleWebcam,
+        ),
+    )
+
+    camera.image.transport = core.LCMTransport("/image1", Image)
+    camera.camera_info.transport = core.LCMTransport("/image1/camera_info", CameraInfo)
+    camera.start()
+
+    try:
+        while True:
+            time.sleep(1)
+    except KeyboardInterrupt:
+        camera.stop()
+        dimos.stop()
+
+
+@pytest.mark.tool
+def test_streaming_double():
+    dimos = core.start(2)
+
+    camera1 = dimos.deploy(
+        CameraModule,
+        transform=Transform(
+            translation=Vector3(0.05, 0.0, 0.0),
+            rotation=Quaternion(0.0, 0.0, 0.0, 1.0),
+            frame_id="sensor",
+            child_frame_id="camera_link",
+        ),
+        hardware=lambda: Webcam(
+            stereo_slice="left",
+            camera_index=0,
+            frequency=15,
+            camera_info=zed.CameraInfo.SingleWebcam,
+        ),
+    )
+
+    camera2 = dimos.deploy(
+        CameraModule,
+        transform=Transform(
+            translation=Vector3(0.05, 0.0, 0.0),
+            rotation=Quaternion(0.0, 0.0, 0.0, 1.0),
+            frame_id="sensor",
+            child_frame_id="camera_link",
+        ),
+        hardware=lambda: Webcam(
+            camera_index=4,
+            frequency=15,
+            stereo_slice="left",
+            camera_info=zed.CameraInfo.SingleWebcam,
+        ),
+    )
+
+    camera1.image.transport = core.LCMTransport("/image1", Image)
+    camera1.camera_info.transport = core.LCMTransport("/image1/camera_info", CameraInfo)
+    camera1.start()
+    camera2.image.transport = core.LCMTransport("/image2", Image)
+    camera2.camera_info.transport = core.LCMTransport("/image2/camera_info", CameraInfo)
+    camera2.start()
+
+    try:
+        while True:
+            time.sleep(1)
+    except KeyboardInterrupt:
+        camera1.stop()
+        camera2.stop()
+        dimos.stop()