Add a simulated RGB camera to the Gazebo sim

mote_description/config/robot.yaml

@@ -31,3 +31,14 @@ camera:
   image_size: [640, 480]
   # Default fallback intrinsics; a per-robot ~/.mote/camera_calibration.yaml overrides it.
   default_info_url: package://mote_perception/config/camera_info.default.yaml
+  # Simulated camera parameters — used only by the use_sim branch of the URDF;
+  # the real driver gets its intrinsics from default_info_url instead. The FOV
+  # matches the calibrated real lens (fx ~469.6 over 640 px -> ~68.6deg) so sim
+  # and real geometry stay consistent.
+  sim:
+    update_rate: 30.0
+    horizontal_fov: 1.20
+    width: 640
+    height: 480
+    near: 0.05
+    far: 20.0

mote_description/urdf/mote.urdf.xacro

@@ -281,6 +281,32 @@
         </lidar>
       </sensor>
     </gazebo>
+
+    <xacro:property name="camera_sim" value="${cfg['camera']['sim']}"/>
+    <gazebo reference="camera_link">
+      <sensor name="camera" type="camera">
+        <!-- gz_frame_id is the optical frame so the published image/camera_info
+             frame_id follows the ROS optical convention (z-forward). The sensor
+             body is camera_link (+X-forward), which gz uses as the view axis. -->
+        <topic>image_raw</topic>
+        <gz_frame_id>camera_optical_link</gz_frame_id>
+        <update_rate>${camera_sim['update_rate']}</update_rate>
+        <always_on>true</always_on>
+        <visualize>false</visualize>
+        <camera>
+          <horizontal_fov>${camera_sim['horizontal_fov']}</horizontal_fov>
+          <image>
+            <width>${camera_sim['width']}</width>
+            <height>${camera_sim['height']}</height>
+            <format>R8G8B8</format>
+          </image>
+          <clip>
+            <near>${camera_sim['near']}</near>
+            <far>${camera_sim['far']}</far>
+          </clip>
+        </camera>
+      </sensor>
+    </gazebo>
   </xacro:if>
 
 </robot>

mote_simulation/launch/sim_launch.py

@@ -111,6 +111,8 @@ def generate_launch_description():
         arguments=[
             "/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock",
             "/scan@sensor_msgs/msg/LaserScan[gz.msgs.LaserScan",
+            "/image_raw@sensor_msgs/msg/Image[gz.msgs.Image",
+            "/camera_info@sensor_msgs/msg/CameraInfo[gz.msgs.CameraInfo",
         ],
     )
 

mote_simulation/test/sim_smoke/verify_sim.py

@@ -17,7 +17,7 @@
 from rclpy.qos import QoSDurabilityPolicy, QoSProfile, QoSReliabilityPolicy
 from geometry_msgs.msg import TwistStamped
 from nav_msgs.msg import Odometry, OccupancyGrid
-from sensor_msgs.msg import LaserScan
+from sensor_msgs.msg import Image, LaserScan
 
 
 class Verifier(Node):
@@ -26,11 +26,17 @@ def __init__(self):
         self.set_parameters([rclpy.parameter.Parameter("use_sim_time", value=True)])
         self.odom = None
         self.scans = []
+        # Keep only running stats for images: full 640x480 RGB frames at 30 Hz
+        # would balloon memory if every message were retained.
+        self.image_count = 0
+        self.image_first_stamp = None
+        self.image_last = None
         self.map = None
         self.create_subscription(
             Odometry, "/diff_drive_controller/odom", self.on_odom, 10
         )
         self.create_subscription(LaserScan, "/scan", self.on_scan, 10)
+        self.create_subscription(Image, "/image_raw", self.on_image, 10)
         # slam_toolbox latches /map with transient-local reliable QoS
         map_qos = QoSProfile(
             depth=1,
@@ -48,6 +54,12 @@ def on_odom(self, msg):
     def on_scan(self, msg):
         self.scans.append((time.monotonic(), msg))
 
+    def on_image(self, msg):
+        if self.image_first_stamp is None:
+            self.image_first_stamp = msg.header.stamp
+        self.image_count += 1
+        self.image_last = msg
+
     def on_map(self, msg):
         self.map = msg
 
@@ -133,6 +145,28 @@ def stamp_s(msg):
     assert len(finite) > len(scan.ranges) * 0.5, "FAIL: too few finite ranges"
     assert max(finite) < 12.0 and min(finite) > 0.05, "FAIL: ranges outside lidar spec"
 
+    # camera: the sim should be publishing /image_raw at its configured rate
+    # over the same drive window. Rate is computed from header stamps (sim time).
+    assert node.image_count > 0, "FAIL: no /image_raw received"
+    img = node.image_last
+
+    def stamp_to_s(stamp):
+        return stamp.sec + stamp.nanosec / 1e9
+
+    img_span = stamp_to_s(img.header.stamp) - stamp_to_s(node.image_first_stamp)
+    img_rate = (node.image_count - 1) / img_span if img_span > 0 else 0.0
+    print(
+        f"image: {node.image_count} msgs, {img_rate:.1f} Hz, "
+        f"{img.width}x{img.height} {img.encoding}, frame_id {img.header.frame_id}"
+    )
+    assert img_rate > 5.0, f"FAIL: image rate {img_rate:.1f} Hz too low"
+    assert img.width == 640 and img.height == 480, (
+        f"FAIL: image size {img.width}x{img.height} != 640x480"
+    )
+    assert img.header.frame_id == "camera_optical_link", (
+        f"FAIL: image frame_id {img.header.frame_id} != camera_optical_link"
+    )
+
     # slam_toolbox should have built a map by now; give it a few seconds to
     # process the scans accumulated during the drive
     deadline = time.monotonic() + 20