How to add a ROS camera to IRIS for gazebo simulation?

An easier method using available gazebo camera plugins would be:
You should have ROS1/2 installed for this to work.

<!--down cam link-->
        <link name="down_camera_link">
        <pose>0 0 -0.013633 1.5708 1.5708 0</pose>
        <inertial>
            <pose>0 0 0 0 0 0</pose>
            <mass>0.050075</mass>
            <inertia>
                <ixx>3.2061e-05</ixx>
                <ixy>-1.169e-08</ixy>
                <ixz>-4.2601e-10</ixz>
                <iyy>3.0815e-05</iyy>
                <iyz>5.2182e-10</iyz>
                <izz>5.1652e-06</izz>
            </inertia>
        </inertial>
        <collision name='down_camera_collision'>
            <pose>0 0 0 0 0 0</pose>
            <geometry>
                <box>
                    <size>0.023813 0.027686 0.092682</size>
                </box>
            </geometry>
            <surface>
                <friction>
                    <ode>
                        <mu>1</mu>
                        <mu2>1</mu2>
                    </ode>
                </friction>
                <contact>
                    <ode>
                        <kp>1e+8</kp>
                        <kd>1</kd>
                        <max_vel>0.01</max_vel>
                        <min_depth>0.001</min_depth>
                    </ode>
                </contact>
            </surface>
        </collision>
        <visual name='down_camera_visual'>
            <pose>0 0 0 0 0 0</pose>
            <geometry>
                <mesh>
                    <scale>1 1 1</scale>
                    <uri>model://iris/meshes/cam_link.STL</uri>
                </mesh>
            </geometry>
            <material>
                <script>
                    <name>Gazebo/DarkGrey</name>
                    <uri>file://media/materials/scripts/gazebo.material</uri>
                </script>
            </material>
        </visual>
        <gravity>1</gravity>
        <velocity_decay/>
        <self_collide>0</self_collide>
        <sensor name="down_camera_imu" type="imu">
            <always_on>1</always_on>
        </sensor>
        <sensor name="down_camera" type="camera">                 <!-- name of sensor topic as well -->
            <pose>0 0 0 -1.5708 0 0</pose>
            <camera>
                <horizontal_fov>2.0</horizontal_fov>
                <image>
                    <format>R8G8B8</format>
                    <width>640</width>
                    <height>480</height>
                </image>
                <clip>
                    <near>0.05</near>
                    <far>15000</far>
                </clip>
            </camera>
            <always_on>1</always_on>
            <update_rate>10</update_rate>
            <visualize>true</visualize>
            <plugin filename="libgazebo_ros_camera.so" name="camera_en_ros">
                <robotNamespace>iris</robotNamespace>
                <cameraName>down_camera_link</cameraName>
                <imageTopicName>down_raw_image</imageTopicName>
                <cameraInfoTopicName>down_info_image</cameraInfoTopicName>
                <FrameName>downCameraImage</FrameName>
                <Cy>0</Cy>
                <Cx>0</Cx>
                <updateRate>30.0</updateRate>
            </plugin>
            <plugin name="GstCameraPlugin" filename="libgazebo_gst_camera_plugin.so">
                <robotNamespace></robotNamespace>
                <udpPort>5600</udpPort>
            </plugin>
            <plugin name="GeotaggedImagesPlugin" filename="libgazebo_geotagged_images_plugin.so">
                <robotNamespace>iris</robotNamespace>
                <interval>1</interval>
                <width>3840</width>
                <height>2160</height>
                <maximum_zoom>8.0</maximum_zoom>
            </plugin>
        </sensor>
        </link>
        <joint name='down_camera_joint' type='fixed'>
            <child>down_camera_link</child>
            <parent>base_link</parent>
            <pose>0 0 0 0 0 0</pose>
            <axis>
                <xyz>-1 0 0</xyz>
            </axis>
        </joint>

copy the above code into your model.sdf.jinja under the imu_link. Then run make px4_sitl_rtps gazebo_model, in another tab run rqt,

make sure to tweak <pose>...</pose> under <link name="down_camera_link"> to suit your needs.
pose = position in the form [x,y,z, x0, y0, z0]

1 Like