fix(g1): add video stream and camera_info to G1SimConnection

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",
         ]
     ),
 )

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),
         }
     )
 )

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(