working depth camera

reset_robot_description/description/base.urdf.xacro

@@ -1,5 +1,6 @@
 <robot name="reset_robot" xmlns:xacro="http://www.ros.org/wiki/xacro">
     <xacro:include filename="robot.xacro" />
     <xacro:include filename="imu.xacro"/>
+    <xacro:include filename="camera.xacro"/>
     <xacro:include filename="gazebo_control.xacro" />
 </robot>

reset_robot_description/description/camera.xacro

@@ -0,0 +1,58 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <joint name="camera_joint" type="fixed">
+        <parent link="chassis" />
+        <child link="camera_link" />
+        <origin xyz="0.276 0 0.18" rpy="0 0.18 0" />
+    </joint>
+
+    <link name="camera_link">
+        <visual>
+            <geometry>
+                <box size="0.010 0.03 0.03" />
+            </geometry>
+            <material name="black" />
+        </visual>
+        <visual>
+            <origin xyz="0 0 -0.05" />
+            <geometry>
+                <cylinder radius="0.002" length="0.1" />
+            </geometry>
+            <material name="black" />
+        </visual>
+    </link>
+
+
+    <joint name="camera_optical_joint" type="fixed">
+        <parent link="camera_link" />
+        <child link="camera_link_optical" />
+        <origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}" />
+    </joint>
+
+    <link name="camera_link_optical"></link>
+
+
+    <gazebo reference="camera_link">
+        <sensor name="rgbd_camera" type="rgbd_camera">
+            <camera>
+                <horizontal_fov>1.25</horizontal_fov>
+                <image>
+                    <width>320</width>
+                    <height>240</height>
+                </image>
+                <clip>
+                    <near>0.3</near>
+                    <far>15</far>
+                </clip>
+                <optical_frame_id>camera_link</optical_frame_id>
+            </camera>
+            <always_on>1</always_on>
+            <update_rate>20</update_rate>
+            <visualize>true</visualize>
+            <topic>camera</topic>
+            <gz_frame_id>camera_link</gz_frame_id>
+        </sensor>
+    </gazebo>
+
+</robot>

reset_robot_gazebo/config/gz_bridge.yaml

@@ -31,4 +31,22 @@
   gz_topic_name: "joint_states"
   ros_type_name: "sensor_msgs/msg/JointState"
   gz_type_name: "gz.msgs.Model"
-  direction: GZ_TO_ROS
+  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
+
+- ros_topic_name: "camera/depth_image"
+  gz_topic_name: "camera/depth_image"
+  ros_type_name: "sensor_msgs/msg/Image"
+  gz_type_name: "gz.msgs.Image"
+  direction: GZ_TO_ROS
+
+- ros_topic_name: "camera/points"
+  gz_topic_name: "camera/points"
+  ros_type_name: "sensor_msgs/msg/PointCloud2"
+  gz_type_name: "gz.msgs.PointCloudPacked"
+  direction: GZ_TO_ROS

reset_robot_gazebo/launch/sim.launch.py

@@ -54,13 +54,23 @@ def generate_launch_description():
         package='ros_gz_bridge',
         executable='parameter_bridge',
         arguments=[
-            '--ros-args',
+           '--ros-args',
             '-p',
             f'config_file:={bridge_params}',
         ],
         output='screen',
     )
 
+    ros_gz_image_bridge = Node(
+        package="ros_gz_image",
+        executable="image_bridge",
+        arguments=["/camera/image"],
+        parameters=[{
+            'use_sim_time': True, 
+            'camera.image.compressed.jpeg_quality': 75
+        }]
+    )
+
     # Launch them all!
     return LaunchDescription([
         world,
@@ -68,4 +78,5 @@ def generate_launch_description():
         gazebo,
         spawn_entity,
         gz_bridge,
+        ros_gz_image_bridge
     ])