From 00408fecbeb47fdb1f2754c11271301640e9ec76 Mon Sep 17 00:00:00 2001 From: Michael Johnson Date: Fri, 26 Jun 2026 22:46:39 +0100 Subject: [PATCH] Add a simulated RGB camera to the Gazebo sim Add a gz camera sensor (use_sim branch only) that publishes /image_raw and /camera_info in frame camera_optical_link, mirroring the simulated lidar, so camera perception (L1) can be developed and tested headless. Sim params live in robot.yaml's camera.sim block (FOV matched to the calibrated real lens); sim_launch.py bridges the two topics; the sim smoke test now asserts /image_raw. Non-sim URDF output is unchanged. Co-Authored-By: Claude Opus 4.8 --- mote_description/config/robot.yaml | 11 ++++++ mote_description/urdf/mote.urdf.xacro | 26 ++++++++++++++ mote_simulation/launch/sim_launch.py | 2 ++ mote_simulation/test/sim_smoke/verify_sim.py | 36 +++++++++++++++++++- 4 files changed, 74 insertions(+), 1 deletion(-) diff --git a/mote_description/config/robot.yaml b/mote_description/config/robot.yaml index 55a0ceb..152450d 100644 --- a/mote_description/config/robot.yaml +++ b/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 diff --git a/mote_description/urdf/mote.urdf.xacro b/mote_description/urdf/mote.urdf.xacro index a86d4fc..d63d283 100644 --- a/mote_description/urdf/mote.urdf.xacro +++ b/mote_description/urdf/mote.urdf.xacro @@ -281,6 +281,32 @@ + + + + + + image_raw + camera_optical_link + ${camera_sim['update_rate']} + true + false + + ${camera_sim['horizontal_fov']} + + ${camera_sim['width']} + ${camera_sim['height']} + R8G8B8 + + + ${camera_sim['near']} + ${camera_sim['far']} + + + + diff --git a/mote_simulation/launch/sim_launch.py b/mote_simulation/launch/sim_launch.py index 23fe7e9..09e46f9 100644 --- a/mote_simulation/launch/sim_launch.py +++ b/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", ], ) diff --git a/mote_simulation/test/sim_smoke/verify_sim.py b/mote_simulation/test/sim_smoke/verify_sim.py index 1758c5f..404357b 100755 --- a/mote_simulation/test/sim_smoke/verify_sim.py +++ b/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