From d620ace2895e67f683b2e9439db8b0fbe64ccc6c Mon Sep 17 00:00:00 2001 From: kaiknower Date: Sat, 14 Mar 2026 15:30:07 +0800 Subject: [PATCH] Use G1 WebRTC camera in headless setup --- .../primitive/uintree_g1_primitive_no_nav.py | 36 +----------- dimos/robot/unitree/g1/connection.py | 56 +++++++++++++++++-- 2 files changed, 53 insertions(+), 39 deletions(-) diff --git a/dimos/robot/unitree/g1/blueprints/primitive/uintree_g1_primitive_no_nav.py b/dimos/robot/unitree/g1/blueprints/primitive/uintree_g1_primitive_no_nav.py index c47fdc377..de11efe10 100644 --- a/dimos/robot/unitree/g1/blueprints/primitive/uintree_g1_primitive_no_nav.py +++ b/dimos/robot/unitree/g1/blueprints/primitive/uintree_g1_primitive_no_nav.py @@ -17,19 +17,14 @@ from typing import Any -from dimos_lcm.sensor_msgs import CameraInfo - from dimos.core.blueprints import autoconnect from dimos.core.global_config import global_config from dimos.core.transport import LCMTransport -from dimos.hardware.sensors.camera import zed -from dimos.hardware.sensors.camera.module import camera_module # type: ignore[attr-defined] -from dimos.hardware.sensors.camera.webcam import Webcam from dimos.mapping.costmapper import cost_mapper from dimos.mapping.voxels import voxel_mapper -from dimos.msgs.geometry_msgs import PoseStamped, Quaternion, Transform, Twist, Vector3 +from dimos.msgs.geometry_msgs import PoseStamped, Twist from dimos.msgs.nav_msgs import Odometry, Path -from dimos.msgs.sensor_msgs import Image, PointCloud2 +from dimos.msgs.sensor_msgs import CameraInfo, Image, PointCloud2 from dimos.msgs.std_msgs import Bool from dimos.navigation.frontier_exploration import wavefront_frontier_explorer from dimos.protocol.pubsub.impl.lcmpubsub import LCM @@ -104,36 +99,9 @@ def _g1_rerun_blueprint() -> Any: else: _with_vis = autoconnect() - -def _create_webcam() -> Webcam: - return Webcam( - camera_index=0, - fps=15, - stereo_slice="left", - camera_info=zed.CameraInfo.SingleWebcam, - ) - - -_camera = ( - autoconnect( - camera_module( - transform=Transform( - translation=Vector3(0.05, 0.0, 0.6), # height of camera on G1 robot - rotation=Quaternion.from_euler(Vector3(0.0, 0.2, 0.0)), - frame_id="sensor", - child_frame_id="camera_link", - ), - hardware=_create_webcam, - ), - ) - if not global_config.simulation - else autoconnect() -) - uintree_g1_primitive_no_nav = ( autoconnect( _with_vis, - _camera, voxel_mapper(voxel_size=0.1), cost_mapper(), wavefront_frontier_explorer(), diff --git a/dimos/robot/unitree/g1/connection.py b/dimos/robot/unitree/g1/connection.py index c2dbc6ab2..b8c35fdb5 100644 --- a/dimos/robot/unitree/g1/connection.py +++ b/dimos/robot/unitree/g1/connection.py @@ -12,8 +12,10 @@ # See the License for the specific language governing permissions and # limitations under the License. - from abc import ABC, abstractmethod +import threading +from threading import Thread +import time from typing import TYPE_CHECKING, Any from reactivex.disposable import Disposable @@ -23,8 +25,10 @@ from dimos.core.global_config import GlobalConfig, global_config from dimos.core.module import Module from dimos.core.module_coordinator import ModuleCoordinator -from dimos.core.stream import In -from dimos.msgs.geometry_msgs import Twist +from dimos.core.stream import In, Out +from dimos.hardware.sensors.camera import zed +from dimos.msgs.sensor_msgs import CameraInfo, Image +from dimos.msgs.geometry_msgs import Quaternion, Transform, Twist, Vector3 from dimos.robot.unitree.connection import UnitreeWebRTCConnection from dimos.utils.logging_config import setup_logger @@ -34,6 +38,10 @@ logger = setup_logger() +def _camera_info_static() -> CameraInfo: + return zed.CameraInfo.SingleWebcam + + class G1ConnectionBase(Module, ABC): """Abstract base for G1 connections (real hardware and simulation). @@ -63,9 +71,13 @@ def publish_request(self, topic: str, data: dict[str, Any]) -> dict[Any, Any]: . class G1Connection(G1ConnectionBase): cmd_vel: In[Twist] + color_image: Out[Image] + camera_info: Out[CameraInfo] ip: str | None connection_type: str | None = None _global_config: GlobalConfig + camera_info_static: CameraInfo = _camera_info_static() + _camera_info_thread: Thread | None = None connection: UnitreeWebRTCConnection | None @@ -81,11 +93,13 @@ def __init__( self.ip = ip if ip is not None else self._global_config.robot_ip self.connection_type = connection_type or self._global_config.unitree_connection_type self.connection = None + self._stop_event = threading.Event() super().__init__(*args, **kwargs) @rpc def start(self) -> None: super().start() + self._stop_event.clear() match self.connection_type: case "webrtc": @@ -103,12 +117,22 @@ def start(self) -> None: assert self.connection is not None self.connection.start() + self._disposables.add(self.connection.video_stream().subscribe(self.color_image.publish)) self._disposables.add(Disposable(self.cmd_vel.subscribe(self.move))) + self._camera_info_thread = Thread( + target=self._publish_camera_info_loop, + daemon=True, + ) + self._camera_info_thread.start() + @rpc def stop(self) -> None: - assert self.connection is not None - self.connection.stop() + self._stop_event.set() + if self.connection is not None: + self.connection.stop() + if self._camera_info_thread and self._camera_info_thread.is_alive(): + self._camera_info_thread.join(timeout=1.0) super().stop() @rpc @@ -116,6 +140,28 @@ def move(self, twist: Twist, duration: float = 0.0) -> None: assert self.connection is not None self.connection.move(twist, duration) + def _publish_camera_info_loop(self) -> None: + while not self._stop_event.is_set(): + ts = time.time() + self.camera_info.publish(self.camera_info_static.with_ts(ts)) + self.tf.publish( + Transform( + translation=Vector3(0.05, 0.0, 0.6), + rotation=Quaternion.from_euler(Vector3(0.0, 0.2, 0.0)), + frame_id="sensor", + child_frame_id="camera_link", + ts=ts, + ), + 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=ts, + ), + ) + self._stop_event.wait(1.0) + @rpc def publish_request(self, topic: str, data: dict[str, Any]) -> dict[Any, Any]: logger.info(f"Publishing request to topic: {topic} with data: {data}")