feat(sim): add a temporary camera to the simulated robot

README.md

@@ -174,6 +174,12 @@ its previews.
 `sim.launch.py` is `world.launch.py` (Gazebo with the world) and `sim_robot.launch.py` (the simulated robot with the
 same localization, SLAM and navigation as the real one) together, with RViz and joystick teleop; tools such as the
 GUI start them one by one.
+
+**Camera.** The simulated robot has a camera on the front of the body, a temporary one until the real robot gets
+one: `/camera/image_raw` (also compressed through `image_transport`) and `/camera/camera_info`, 640×480 at 30 Hz,
+frame `camera_rgb_frame`. RViz shows it in the *Camera* display (disabled by default). `camera:=false` starts the
+model without the camera.
+
 In a simulation shared by several robots each one is started with `sim_robot.launch.py robot_ns:=<name>`: the name
 is given to the model, its Gazebo topics and its controller manager. The topics of the controller manager move under
 `/<name>` too (`robot_description`, `joint_states`, `diff_drive_controller/cmd_vel`, `diff_drive_controller/odom`), so

src/platform/kpi_rover_bringup/config/gz_bridge.yaml

@@ -15,3 +15,9 @@
   ros_type_name: "sensor_msgs/msg/Imu"
   gz_type_name: "gz.msgs.IMU"
   direction: GZ_TO_ROS
+
+- ros_topic_name: "/camera/camera_info"
+  gz_topic_name: "camera/camera_info"
+  ros_type_name: "sensor_msgs/msg/CameraInfo"
+  gz_type_name: "gz.msgs.CameraInfo"
+  direction: GZ_TO_ROS

src/platform/kpi_rover_bringup/launch/sim_robot.launch.py

@@ -18,6 +18,7 @@ def generate_launch_description():
     z_pose = LaunchConfiguration('z_pose')
     yaw_pose = LaunchConfiguration('yaw_pose')
     robot_ns = LaunchConfiguration('robot_ns')
+    camera = LaunchConfiguration('camera')
 
     declared_arguments = [
         DeclareLaunchArgument(
@@ -41,6 +42,9 @@ def generate_launch_description():
         DeclareLaunchArgument(
             'load_controllers', default_value='true',
             description='Load the controllers; false when something else loads them'),
+        DeclareLaunchArgument(
+            'camera', default_value='true',
+            description='Add the camera to the model: /camera/image_raw, /camera/camera_info'),
     ]
 
     # robot_state_publisher with the sim_mode URDF (gz_ros2_control plugin)
@@ -52,6 +56,7 @@ def generate_launch_description():
             'sim_mode': 'true',
             'wheel_radius': wheel_radius,
             'robot_ns': robot_ns,
+            'camera': camera,
         }.items(),
     )
 
@@ -81,6 +86,19 @@ def generate_launch_description():
         output='screen',
     )
 
+    # The image goes through ros_gz_image, not gz_bridge.yaml: it adds the image_transport topics (compressed).
+    # The Gazebo topic is <robot_ns>/camera/image_raw, the ROS topics /camera/image_raw[/<transport>]:
+    # image_transport names its topics from the topic before the remapping, so each one is remapped
+    camera_bridge = Node(
+        package='ros_gz_image',
+        executable='image_bridge',
+        arguments=[[robot_ns, '/camera/image_raw']],
+        remappings=[([robot_ns, '/camera/image_raw' + t], '/camera/image_raw' + t)
+                    for t in ('', '/compressed', '/compressedDepth', '/theora', '/zstd')],
+        output='screen',
+        condition=IfCondition(camera),
+    )
+
     joint_state_broadcaster = Node(
         package='controller_manager',
         executable='spawner',
@@ -148,6 +166,7 @@ def generate_launch_description():
         description,
         spawn_robot,
         gz_bridge,
+        camera_bridge,
         spawn_controllers,
         twist_stamper,
         localization,

src/platform/kpi_rover_bringup/rviz/view_robot.rviz

@@ -109,6 +109,20 @@ Visualization Manager:
       Use Fixed Frame: true
       Use rainbow: true
       Value: true
+    - Class: rviz_default_plugins/Image
+      Enabled: false
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Camera
+      Normalize Range: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /camera/image_raw
+      Value: false
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48

src/platform/kpi_rover_description/launch/description.launch.py

@@ -17,6 +17,7 @@ def generate_launch_description():
     baud_rate = LaunchConfiguration('baud_rate')
     wheel_radius = LaunchConfiguration('wheel_radius')
     robot_ns = LaunchConfiguration('robot_ns')
+    camera = LaunchConfiguration('camera')
 
     pkg_share = FindPackageShare('kpi_rover_description')
 
@@ -30,7 +31,8 @@ def generate_launch_description():
         ' serial_device:=', serial_device,
         ' baud_rate:=', baud_rate,
         ' wheel_radius:=', wheel_radius,
-        ' robot_ns:=', robot_ns
+        ' robot_ns:=', robot_ns,
+        ' camera:=', camera
     ])
 
     # Launch!
@@ -59,6 +61,10 @@ def generate_launch_description():
             'robot_ns',
             default_value='',
             description='Name of the robot in a simulation shared by several robots (Gazebo topics, controller manager)'),
+        DeclareLaunchArgument(
+            'camera',
+            default_value='false',
+            description='Add the camera to the model'),
 
         Node(
             package='robot_state_publisher',

src/platform/kpi_rover_description/urdf/camera.xacro

@@ -8,7 +8,8 @@
   <joint name="camera_joint" type="fixed">
       <parent link="base_link"/>
       <child link="camera_link"/>
-      <origin xyz="${base_x_size/2} 0 ${base_z_size}" rpy="0 0 0"/>
+      <!-- A temporary mount on the front of the body, until the real camera is chosen -->
+      <origin xyz="0.11 0 0.09" rpy="0 0 0"/>
   </joint>
 
   <link name="camera_link">
@@ -35,16 +36,16 @@
           <always_on>true</always_on>
           <visualize>true</visualize>
           <update_rate>30</update_rate>
-          <topic>camera/image_raw</topic>
+          <topic>$(arg robot_ns)/camera/image_raw</topic>
           <gz_frame_id>camera_rgb_frame</gz_frame_id>
           <camera name="intel_realsense_r200">
-            <camera_info_topic>camera/camera_info</camera_info_topic>
+            <camera_info_topic>$(arg robot_ns)/camera/camera_info</camera_info_topic>
             <horizontal_fov>1.02974</horizontal_fov>
             <!-- Hide world visuals meant only for the Gazebo GUI (bit 1, see kpi_rover_worlds/common/python/sdf.py) -->
             <visibility_mask>4294967293</visibility_mask>
             <image>
-              <width>1920</width>
-              <height>1080</height>
+              <width>640</width>
+              <height>480</height>
               <format>R8G8B8</format>
             </image>
             <clip>

src/platform/kpi_rover_description/urdf/robot.urdf.xacro

@@ -8,11 +8,14 @@
     <xacro:arg name="wheel_radius" default="0.034"/>
     <!-- Name of the robot in a simulation shared by several robots (Gazebo topics, controller manager namespace) -->
     <xacro:arg name="robot_ns" default=""/>
+    <xacro:arg name="camera" default="false"/>
 
     <xacro:include filename="materials.xacro" />
     <xacro:include filename="inertia.xacro" />
     <xacro:include filename="robot_core.xacro" />
-    <!-- <xacro:include filename="camera.xacro" /> -->
+    <xacro:if value="$(arg camera)">
+        <xacro:include filename="camera.xacro" />
+    </xacro:if>
     <xacro:include filename="ros2_control.xacro" />
     <xacro:include filename="imu.xacro" />
     <xacro:include filename="lidar.xacro" />