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]