Integrating Depth Camera with ArduPilot Gazebo for Obstacle Avoidance

Jazx_Jazx · January 28, 2024, 5:03pm

Hello community,

I am currently working on integrating a Realsense D435 with my drone in the ArduPilot Gazebo simulation environment. My goal is to implement obstacle avoidance during the drone’s mission. I have been using the ardupilot_gazebo package.
Maybe you could tell me how to combine all these things step by step. I will be extremely grateful
Here are the specifics of my setup:
Ubuntu 20.04 ROS kinetic
ArduPilot Gazebo version: GitHub - ArduPilot/ardupilot_gazebo: Plugins and models for vehicle simulation in Gazebo Sim with ArduPilot SITL controllers
Depth camera model: GitHub - issaiass/realsense_gazebo_plugin: || The realsense gazebo plugin for intel cameras from PAL Robotics || d435 I think i can use this : https://github.com/thien94/vision_to_mavros/blob/master/scripts/d4xx_to_mavlink.py
I could not successfully integrated the depth camera with the drone in ArduPilot Gazebo . I would appreciate any guidance or insights on how to proceed with this task.

Specifically, I am interested in:

How to configure ArduPilot to utilize the depth camera data for obstacle detection.
Are there any existing plugins or tools that facilitate this process?
Any code snippets or examples demonstrating obstacle avoidance using depth cameras with ArduPilot in Gazebo.
I have already reviewed the relevant documentation, but additional insights from the community would be immensely helpful. Thank you in advance for your assistance!

rhys · February 7, 2024, 9:17pm

Hi @Jazx_Jazx - a full answer to your question will probably take a few posts, but the first part asking to integrate a depth camera into your simulated drone should not be too difficult.

Gazebo Garden and Harmonic support depth cameras directly, so you may not need to used the one from the plugin (you may need to tune the camera intrinsics to match, which would be for a follow up).

I’ve added a branch to ardupilot_gazebo here: GitHub - srmainwaring/ardupilot_gazebo-1 at prs/pr-all-camera-sensors that adds all the available camera sensors to the gimbal mount.

The example is run using

gz sim -v4 -r gimbal.sdf

You can inspect the behaviour of each sensor by selecting the appropriate topcis in the Image Display.

Figure: depth camera

Figure: rgbd camera

Figure: thermal camera

Figure: wide angle camera

Figure: panoptic segmentation camera

The next step is to integrate the data from the camera topics with ArduPilot. For that we’ll probably want to use the tools available in ROS to carry out any image manipulation required, or at least convert the image data into a format recognised by ROS which will be more accessible to other tools. For this we’ll use the ros_gz bridge - more on that in the next post.

Jazx_Jazx · February 10, 2024, 9:33am

Thank you very much for such a detailed guide!!!

Andy_Chen · February 19, 2025, 11:50pm

Hi @rhys ,

I’ve been developing a simulation environment using your waves.sdf as the world, where I placed a BlueBoat model equipped with an OAK-D stereo camera. The boat is controlled via the ArduPilot plugin.

However, I encountered an issue: although the point cloud topic is being published in Gazebo (confirmed via gz topic -l), I cannot visualize the point cloud in either Gazebo or RViz.

Key Observations:

The point cloud topic appears in Gazebo, and I can echo it, showing data is being published.
In RViz, the PointCloud2 panel shows: “0 points from 0 messages”, despite the topic being listed and echo showing outputs.
Gazebo’s interface also doesn’t display the point cloud from the OAK-D camera.

Here’s the snippet of my SDF configuration for the OAK-D sensor:

<link name="oakd_camera_link">
      <pose>0.3 0 0.05 0 0 0</pose> 
      <inertial>
        <pose>0 0 0 0 0 0</pose>
        <mass>0.1</mass>
        <inertia>
          <ixx>0.0001</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.0001</iyy>
          <iyz>0</iyz>
          <izz>0.0001</izz>
        </inertia>
      </inertial>
      <visual name="oakd_camera_visual">
        <geometry>
          <mesh>
            <uri>package://ros_gz_blueboat_description/models/blueboat/meshes/OAK-D.dae</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>0.5 0.5 0.5 1</diffuse>
          <ambient>0.5 0.5 0.5 1</ambient>
        </material>
      </visual>
      <sensor name="oakd_camera_sensor" type="depth_camera">
        <pose relative_to='oakd_camera_link'>  0 0 0 0 0 0</pose>
        <camera>
          <horizontal_fov>1.3962634</horizontal_fov> <!-- ~80 deg FOV -->
          <image>
            <width>640</width>
            <height>480</height>
            <format>R_FLOAT32</format>
          </image>
          <clip>
            <near>0.1</near>
            <far>100</far>
          </clip>
          <depth_camera>
            <output>depths</output>
          </depth_camera>
        </camera>
        <always_on>true</always_on>
        <update_rate>10</update_rate>
        <visualize>true</visualize>
        <enable_metrics>1</enable_metrics>
      </sensor>
    </link>
    <joint name="oakd_camera_joint" type="fixed">
      <parent>base_link</parent>
      <child>oakd_camera_link</child>
    </joint>

Questions:

From your experience with this simulation setup, what could cause the point cloud not to display in Gazebo and Rvizdespite the topic publishing?

Any guidance would be highly appreciated!

Best regards,
Yan-Liang Chen

rhys · February 20, 2025, 10:03am

Hi @Andy_Chen, nice to see you’ve got the BlueBoat sim running. Most likely the issue is with the configuration of the ros_gz bridge. Reducing the resolution of the camera will help with performance.

There is a discussion in the research channel of the AP Discord server here about mapping point cloud data from Gazebo to ROS 2.

The example uses patches in these two branches:

Andy_Chen · February 20, 2025, 11:10pm

@rhys I solved the issue by setting the Fixed Frame in RViz to the sensor’s frame:

blueboat/oakd_camera_link/oakd_camera_sensor

Format follows: /model/link/sensor .

Additionally, I had to manually set up a static transform from the camera link to the sensor frame using:

ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 oakd_camera_link blueboat/oakd_camera_link/oakd_camera_sensor

The tricky part: when SDF is converted to URDF, the <sensor> tag gets dropped, and ROS doesn’t recognize the sensor frame by default. However, typing the exact frame name in RViz still works after adding the static transform. Not sure why, but it works.

rhys · February 21, 2025, 11:54am

@Andy_Chen if the TF from the model was generated correctly, that should not be necessary (except maybe a static TF from map to odom if that is not otherwise available). The iris with gimbal example in ardupilot_gz has all the necessary transforms to handle the additional elements attached to the vehicle.

I wonder if some of these are missing in your set up. Getting sdformat_urdf to work correctly on more complex models is a bit fiddly as there are some issues traversing nested models.

Muhammet_Bulut · February 22, 2025, 3:09pm

Hi friend, I know this is irrelevant but, I was wondering if you still support the “Gazebo: X-UAV Mini Talon V-Tail” project because I want to use it but I can’t find the latest files.

rhys · February 23, 2025, 10:07pm

Yes a bit off topic. The model is not released but in this PR: Gazebo: add X-UAV Mini Talon V-Tail by srmainwaring · Pull Request #98 · ArduPilot/SITL_Models · GitHub. In future please open a separate topic rather than change subject on an existing one.