diff --git a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_basic.py b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_basic.py index 1fb591e895..8b2e2889aa 100644 --- a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_basic.py +++ b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_basic.py @@ -20,11 +20,9 @@ from dimos.robot.unitree.g1.blueprints.primitive.uintree_g1_primitive_no_nav import ( uintree_g1_primitive_no_nav, ) -from dimos.robot.unitree.g1.connection import g1_connection unitree_g1_basic = autoconnect( uintree_g1_primitive_no_nav, - g1_connection(), ros_nav(), ) diff --git a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_basic_sim.py b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_basic_sim.py index 603a9535ee..0309577767 100644 --- a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_basic_sim.py +++ b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_basic_sim.py @@ -20,11 +20,9 @@ from dimos.robot.unitree.g1.blueprints.primitive.uintree_g1_primitive_no_nav import ( uintree_g1_primitive_no_nav, ) -from dimos.robot.unitree.g1.sim import g1_sim_connection unitree_g1_basic_sim = autoconnect( uintree_g1_primitive_no_nav, - g1_sim_connection(), replanning_a_star_planner(), ) diff --git a/dimos/robot/unitree/g1/blueprints/perceptive/unitree_g1_detection.py b/dimos/robot/unitree/g1/blueprints/perceptive/unitree_g1_detection.py index 25bff97c73..2d184d6def 100644 --- a/dimos/robot/unitree/g1/blueprints/perceptive/unitree_g1_detection.py +++ b/dimos/robot/unitree/g1/blueprints/perceptive/unitree_g1_detection.py @@ -22,7 +22,6 @@ from dimos.core.blueprints import autoconnect from dimos.core.transport import LCMTransport -from dimos.hardware.sensors.camera import zed from dimos.msgs.geometry_msgs import PoseStamped from dimos.msgs.sensor_msgs import Image, PointCloud2 from dimos.msgs.vision_msgs import Detection2DArray @@ -31,6 +30,7 @@ from dimos.perception.detection.moduleDB import ObjectDBModule, detection_db_module from dimos.perception.detection.person_tracker import PersonTracker, person_tracker_module from dimos.robot.unitree.g1.blueprints.basic.unitree_g1_basic import unitree_g1_basic +from dimos.robot.unitree.g1.blueprints.primitive.uintree_g1_primitive_no_nav import G1_CAMERA_INFO def _person_only(det: Any) -> bool: @@ -42,15 +42,15 @@ def _person_only(det: Any) -> bool: unitree_g1_basic, # Person detection modules with YOLO detection3d_module( - camera_info=zed.CameraInfo.SingleWebcam, + camera_info=G1_CAMERA_INFO, detector=YoloPersonDetector, ), detection_db_module( - camera_info=zed.CameraInfo.SingleWebcam, + camera_info=G1_CAMERA_INFO, filter=_person_only, # Filter for person class only ), person_tracker_module( - cameraInfo=zed.CameraInfo.SingleWebcam, + cameraInfo=G1_CAMERA_INFO, ), ) .global_config(n_workers=8) 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 c47fdc377b..7dfb5a8715 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 @@ -15,6 +15,7 @@ """Minimal G1 stack without navigation, used as a base for larger blueprints.""" +import os from typing import Any from dimos_lcm.sensor_msgs import CameraInfo @@ -33,6 +34,8 @@ from dimos.msgs.std_msgs import Bool from dimos.navigation.frontier_exploration import wavefront_frontier_explorer from dimos.protocol.pubsub.impl.lcmpubsub import LCM +from dimos.robot.unitree.g1.connection import G1Connection, g1_connection +from dimos.robot.unitree.g1.sim import g1_sim_connection from dimos.web.websocket_vis.websocket_vis_module import websocket_vis @@ -105,6 +108,10 @@ def _g1_rerun_blueprint() -> Any: _with_vis = autoconnect() +def _camera_source() -> str: + return os.environ.get("DIMOS_G1_CAMERA_SOURCE", "unitree").strip().lower() + + def _create_webcam() -> Webcam: return Webcam( camera_index=0, @@ -114,11 +121,16 @@ def _create_webcam() -> Webcam: ) -_camera = ( - autoconnect( +G1_CAMERA_SOURCE = _camera_source() +G1_CAMERA_INFO = G1Connection.camera_info_static if G1_CAMERA_SOURCE != "webcam" else zed.CameraInfo.SingleWebcam + +if global_config.simulation: + _camera = autoconnect(g1_sim_connection()) +elif G1_CAMERA_SOURCE == "webcam": + _camera = autoconnect( camera_module( transform=Transform( - translation=Vector3(0.05, 0.0, 0.6), # height of camera on G1 robot + 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", @@ -126,9 +138,8 @@ def _create_webcam() -> Webcam: hardware=_create_webcam, ), ) - if not global_config.simulation - else autoconnect() -) +else: + _camera = autoconnect(g1_connection()) uintree_g1_primitive_no_nav = ( autoconnect( @@ -166,4 +177,4 @@ def _create_webcam() -> Webcam: ) ) -__all__ = ["uintree_g1_primitive_no_nav"] +__all__ = ["G1_CAMERA_INFO", "G1_CAMERA_SOURCE", "uintree_g1_primitive_no_nav"] diff --git a/dimos/robot/unitree/g1/connection.py b/dimos/robot/unitree/g1/connection.py index c2dbc6ab2d..8be4d13954 100644 --- a/dimos/robot/unitree/g1/connection.py +++ b/dimos/robot/unitree/g1/connection.py @@ -14,6 +14,9 @@ from abc import ABC, abstractmethod +from pathlib import Path +from threading import Event, Thread +import time from typing import TYPE_CHECKING, Any from reactivex.disposable import Disposable @@ -23,8 +26,9 @@ 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.msgs.geometry_msgs import Quaternion, Transform, Twist, Vector3 +from dimos.msgs.sensor_msgs import CameraInfo, Image from dimos.robot.unitree.connection import UnitreeWebRTCConnection from dimos.utils.logging_config import setup_logger @@ -34,6 +38,11 @@ logger = setup_logger() +def _camera_info_static() -> CameraInfo: + params_path = Path(__file__).resolve().parent.parent / "params" / "front_camera_720.yaml" + return CameraInfo.from_yaml(str(params_path)) + + class G1ConnectionBase(Module, ABC): """Abstract base for G1 connections (real hardware and simulation). @@ -63,11 +72,16 @@ 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 connection: UnitreeWebRTCConnection | None + camera_info_static: CameraInfo = _camera_info_static() + _camera_info_thread: Thread | None + _stop_event: Event def __init__( self, @@ -81,11 +95,14 @@ 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._camera_info_thread = None + self._stop_event = Event() super().__init__(*args, **kwargs) @rpc def start(self) -> None: super().start() + self._stop_event.clear() match self.connection_type: case "webrtc": @@ -103,14 +120,43 @@ 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: + self._stop_event.set() assert 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() + 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="base_link", + 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 move(self, twist: Twist, duration: float = 0.0) -> None: assert self.connection is not None