diff --git a/dimos/robot/unitree/g1/blueprints/perceptive/unitree_g1_shm.py b/dimos/robot/unitree/g1/blueprints/perceptive/unitree_g1_shm.py index 5d2eefad9..5ee4d4c9d 100644 --- a/dimos/robot/unitree/g1/blueprints/perceptive/unitree_g1_shm.py +++ b/dimos/robot/unitree/g1/blueprints/perceptive/unitree_g1_shm.py @@ -26,13 +26,13 @@ unitree_g1.transports( { ("color_image", Image): pSHMTransport( - "/g1/color_image", default_capacity=DEFAULT_CAPACITY_COLOR_IMAGE + "/color_image", default_capacity=DEFAULT_CAPACITY_COLOR_IMAGE ), } ), foxglove_bridge( shm_channels=[ - "/g1/color_image#sensor_msgs.Image", + "/color_image#sensor_msgs.Image", ] ), ) 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 2ba267521..36bf569f7 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 @@ -76,9 +76,8 @@ case _: _with_vis = autoconnect() -uintree_g1_primitive_no_nav = ( +_camera = ( autoconnect( - _with_vis, camera_module( transform=Transform( translation=Vector3(0.05, 0.0, 0.6), # height of camera on G1 robot @@ -93,6 +92,15 @@ camera_info=zed.CameraInfo.SingleWebcam, ), ), + ) + 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(), @@ -118,9 +126,9 @@ ("goal_pose", PoseStamped): LCMTransport("/goal_pose", PoseStamped), ("goal_reached", Bool): LCMTransport("/goal_reached", Bool), ("cancel_goal", Bool): LCMTransport("/cancel_goal", Bool), - # Camera topics (if camera module is added) - ("color_image", Image): LCMTransport("/g1/color_image", Image), - ("camera_info", CameraInfo): LCMTransport("/g1/camera_info", CameraInfo), + # Camera topics + ("color_image", Image): LCMTransport("/color_image", Image), + ("camera_info", CameraInfo): LCMTransport("/camera_info", CameraInfo), } ) ) diff --git a/dimos/robot/unitree/g1/sim.py b/dimos/robot/unitree/g1/sim.py index 4502e62a1..6888ae74a 100644 --- a/dimos/robot/unitree/g1/sim.py +++ b/dimos/robot/unitree/g1/sim.py @@ -13,6 +13,8 @@ # limitations under the License. +import threading +from threading import Thread import time from typing import TYPE_CHECKING, Any @@ -27,7 +29,7 @@ Twist, Vector3, ) -from dimos.msgs.sensor_msgs import PointCloud2 +from dimos.msgs.sensor_msgs import CameraInfo, Image, PointCloud2 from dimos.robot.unitree.type.odometry import Odometry as SimOdometry from dimos.utils.logging_config import setup_logger @@ -37,12 +39,34 @@ logger = setup_logger() +def _camera_info_static() -> CameraInfo: + """Camera intrinsics for rerun visualization (matches Go2 convention).""" + fx, fy, cx, cy = (819.553492, 820.646595, 625.284099, 336.808987) + width, height = (1280, 720) + + return CameraInfo( + frame_id="camera_optical", + height=height, + width=width, + distortion_model="plumb_bob", + D=[0.0, 0.0, 0.0, 0.0, 0.0], + K=[fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0], + R=[1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], + P=[fx, 0.0, cx, 0.0, 0.0, fy, cy, 0.0, 0.0, 0.0, 1.0, 0.0], + binning_x=0, + binning_y=0, + ) + + class G1SimConnection(Module): cmd_vel: In[Twist] lidar: Out[PointCloud2] odom: Out[PoseStamped] + color_image: Out[Image] + camera_info: Out[CameraInfo] ip: str | None _global_config: GlobalConfig + _camera_info_thread: Thread | None = None def __init__( self, @@ -54,6 +78,7 @@ def __init__( self._global_config = cfg self.ip = ip if ip is not None else self._global_config.robot_ip self.connection: MujocoConnection | None = None + self._stop_event = threading.Event() super().__init__(*args, **kwargs) @rpc @@ -69,19 +94,35 @@ def start(self) -> None: self._disposables.add(Disposable(self.cmd_vel.subscribe(self.move))) self._disposables.add(self.connection.odom_stream().subscribe(self._publish_sim_odom)) self._disposables.add(self.connection.lidar_stream().subscribe(self.lidar.publish)) + self._disposables.add(self.connection.video_stream().subscribe(self.color_image.publish)) + + 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: + info = _camera_info_static() + while not self._stop_event.is_set(): + self.camera_info.publish(info) + self._stop_event.wait(1.0) + def _publish_tf(self, msg: PoseStamped) -> None: self.odom.publish(msg) self.tf.publish(Transform.from_pose("base_link", msg)) - # Publish camera_link transform + # Publish camera_link and camera_optical transforms camera_link = Transform( translation=Vector3(0.05, 0.0, 0.6), rotation=Quaternion(0.0, 0.0, 0.0, 1.0), @@ -90,6 +131,14 @@ def _publish_tf(self, msg: PoseStamped) -> None: ts=time.time(), ) + 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=time.time(), + ) + map_to_world = Transform( translation=Vector3(0.0, 0.0, 0.0), rotation=Quaternion(0.0, 0.0, 0.0, 1.0), @@ -98,7 +147,7 @@ def _publish_tf(self, msg: PoseStamped) -> None: ts=time.time(), ) - self.tf.publish(camera_link, map_to_world) + self.tf.publish(camera_link, camera_optical, map_to_world) def _publish_sim_odom(self, msg: SimOdometry) -> None: self._publish_tf(