Adding a camera to IRIS base model

1

Recently I’ve been working on getting a basic camera added to the base IRIS model for SITL so I can stream video to QGroundControl, as well as being able to launch multiple vehicles once the model was successfully modified. I was able to find a few resources for adding a ROS camera to an IRIS model and view the stream in rviz (How to add a ROS camera to IRIS for gazebo simulation?), but this did not do what I wanted. Most of these steps are adapted from that link, so thanks to everybody who participated there. I’ve written up the steps I took to get this working in case anyone else is trying a similar thing. I am still pretty new to PX4/ROS so this may not be the best explanation or might cause other issues during development, but hopefully this gets you started.

Installed software:

  • PX4 master branch as of June 2020 (I think 1.10.x)
  • Gazebo 9
  • No ROS(1) installation
  • ROS2 Eloquent (shouldn’t be needed for this method, but I have it installed for other parts of the project)

Creating GStreamer camera macro

I created a new macro definition in Tools/sitl_gazebo/models/rotors_description/urdf/component_snippets.xacro to use the Gazebo camera plugin (libgazebo_gst_camera_plugin.so) instead of the ROS camera plugin (libgazebo_ros_camera.so). I did this by copying the entire camera_macro and naming the copy gst_camera_macro. Note that I left out a lot of information here, but if you copied directly from camera_macro it should work.

  • Line 5: Add a parameter to control the UDP port of the stream
  • Line 15: Change plugin filename to be libgazebo_gst_camera_plugin.so
  • Line 16: Add the udpPort to the be a child of the plugin tag
<!-- Macro to add a GStreamer camera. -->
  1 <xacro:macro name="gst_camera_macro"
  2   params="namespace parent_link camera_suffix frame_rate
  3   horizontal_fov image_width image_height image_format min_distance
  4   max_distance noise_mean noise_stddev enable_visual *cylinder *origin
  5   video_udp_port">
  6   <link name="${namespace}/camera_${camera_suffix}_link">
  7    ....
  8   </link>
  9   <gazebo reference="${namespace}/camera_${camera_suffix}_link">
 10     <sensor type="camera" name="${namespace}_camera_${camera_suffix}">
 11       <update_rate>${frame_rate}</update_rate>
 12       <camera name="head">
 13         ....
 14       </camera>
 15       <plugin name="${namespace}_camera_${camera_suffix}_controller" filename="libgazebo_gst_camera_plugin.so">
 16         <udpPort>${video_udp_port}</udpPort>
 17         <robotNamespace>${namespace}</robotNamespace>
 18          ....
 19       </plugin>
 20     </sensor>
 21   </gazebo>
 22 </xacro:macro>

Adding camera to model

We need to create an argument for the model to accept a UDP port for the video stream, which is important when using multiple vehicles. If multiple vehicles write to the same UDP port you can still get video but it’s very poor quality/switches frames between the feeds, so make sure you do this if that’s what’s happening. I added this near the beginning of the file where all of the different arguments are defined. I set the default to 5600 because that’s the default UDP port for video feed that QGroundControl uses.

<xacro:arg name='video_udp_port' default='5600' />

To add the camera to the model, add the following code section to Tools/sitl_gazebo/models/rotors_description/urdf/iris_base.xacro before the imu_plugin_macro. This will use the macro defined above, as well as provide the UDP port for the video stream.

    <!-- Mount a camera -->
  1 <xacro:gst_camera_macro
  2   namespace="${namespace}"
  3   parent_link="base_link"
  4   camera_suffix="iris"
  5   frame_rate="30.0"
  6   horizontal_fov="1.3962634"
  7   image_width="640"
  8   image_height="360"
  9   image_format="R8G8B8"
 10   min_distance="0.02"
 11   max_distance="300"
 12   noise_mean="0.0"
 13   noise_stddev="0.007"
 14   enable_visual="1"
 15   video_udp_port="$(arg video_udp_port)"
 16   >
 17    <box size="0.02 0.02 0.02" />
 18    <origin xyz="0 0 -0.07" rpy="0 1.57079 0"/>
 19 </xacro:gst_camera_macro>

Modify launch script

If you are planning to use multiple vehicles, you can either create your own launch script or modify existing launch scripts. I used the script found at Tools/gazebo_sitl_multiple_run.sh, and only needed to modify two lines to launch multiple IRIS models writing to different UDP ports.

Define the first UDP port which will be used. I used 5600 for this. The mavlink ports were already defined, I just put it there so it would be easier to find later.

mavlink_udp_port=14560
mavlink_tcp_port=4560
video_udp_port=5600

Find the following command in the spawn_model function and add line 3. If you used 5600 as the default port, vehicle 1 will be on 5600, vehicle 2 will be on 5601, vehicle 3 will be on 5602, etc.

  1 python3 ${src_path}/Tools/sitl_gazebo/scripts/xacro.py ${src_path}/Tools/sitl_gazebo/models/rotors_description/urdf/${MODEL}_base.xacro \
  2         rotors_description_dir:=${src_path}/Tools/sitl_gazebo/models/rotors_description mavlink_udp_port:=$(($mavlink_udp_port+$N)) \
  3         video_udp_port:=$(($video_udp_port+$N)) \
  4         mavlink_tcp_port:=$(($mavlink_tcp_port+$N))  -o /tmp/${MODEL}_${N}.urdf

You can then launch multiple vehicles following the directions for Tools/gazebo_sitl_multiple_run.sh found at https://dev.px4.io/master/en/simulation/multi_vehicle_simulation_gazebo.html.

Additional notes

Some people have mentioned you will need to make the target again whenever you make changes to any of the xacro files. This can be done with the following command.

DONT_RUN=1 make px4_sitl_default gazebo

QGroundControl will not update the video port automatically when switching between multiple vehicles, so you will need to manually change the port after switching which vehicle is being controlled.