PX4 + D435i：构建带深度相机的Gazebo仿真环境

1. 为什么要在Gazebo仿真中集成D435i深度相机

在无人机开发过程中，仿真环境的重要性不言而喻。Gazebo作为PX4生态中最常用的仿真工具，能够模拟真实物理环境，大幅降低开发成本和风险。而Intel RealSense D435i这款深度相机，凭借其双目视觉和IMU融合的特性，在SLAM、避障等场景中表现优异。

我刚开始接触无人机视觉开发时，最头疼的就是如何在仿真环境中测试视觉算法。真实飞行测试成本高、风险大，而普通仿真又无法模拟相机数据。直到发现D435i的Gazebo插件，这个问题才迎刃而解。通过将D435i模型绑定到Iris无人机上，我们可以获得与真实设备高度一致的RGB图像、深度图和IMU数据。

这个方案特别适合以下几类开发者：

• 需要测试视觉导航算法的无人机开发者
• 想验证SLAM建图效果的机器人工程师
• 研究多传感器融合的研究人员
• 希望降低硬件采购成本的在校学生

2. 环境准备与依赖安装

2.1 基础环境配置

在开始之前，确保你的系统已经安装以下基础组件：

• Ubuntu 18.04/20.04（推荐20.04）
• ROS Noetic或Melodic（与Ubuntu版本对应）
• PX4 v1.12或更新版本
• Gazebo 9或11

我建议使用PX4官方推荐的Ubuntu 20.04 + ROS Noetic组合，这个环境最稳定。安装PX4时，记得使用以下命令包含所有子模块：

bash复制git clone --recursive https://github.com/PX4/PX4-Autopilot.git

2.2 安装Realsense Gazebo插件

我们需要从GitHub克隆realsense_ros_gazebo仓库。这里有个小技巧：如果直接从GitHub克隆速度慢，可以使用国内镜像源：

bash复制mkdir -p ~/realsense_ws/src
cd ~/realsense_ws/src
git clone https://gitee.com/mirrors/realsense_ros_gazebo.git
cd ..
catkin_make

编译完成后，别忘记source环境变量：

bash复制source devel/setup.bash

3. 模型集成与SDF文件修改

3.1 复制必要文件到PX4目录

这一步是关键，需要将编译好的插件和模型文件放到正确位置。首先找到你的PX4安装路径（下文用${PX4_DIR}表示），然后执行：

bash复制# 复制插件
cp ~/realsense_ws/devel/lib/librealsense_gazebo_plugin.so ${PX4_DIR}/build/px4_sitl_default/build_gazebo/

# 复制模型文件
cp -r ~/realsense_ws/src/realsense_ros_gazebo/sdf/D435i ${PX4_DIR}/Tools/sitl_gazebo/models/

3.2 创建融合模型

我们需要创建一个将Iris无人机和D435i相机结合的新模型。在${PX4_DIR}/Tools/sitl_gazebo/models/目录下新建iris_D435i文件夹，包含两个关键文件：

1. model.config - 模型描述文件：

xml复制<?xml version="1.0"?>
<model>
  <name>Iris with D435i Camera</name>
  <version>1.0</version>
  <sdf version="1.6">iris_D435i.sdf</sdf>
  <description>3DR Iris quadcopter integrated with Intel RealSense D435i depth camera</description>
</model>

2. iris_D435i.sdf - 模型定义文件：

xml复制<?xml version='1.0'?>
<sdf version='1.6'>
  <model name='iris_D435i'>
    <include>
      <uri>model://iris</uri>
    </include>
    <include>
      <uri>model://D435i</uri>
      <pose>0.12 0 0.02 1.5708 0 1.5708</pose>
    </include>
    <joint name="camera_joint" type="fixed">
      <parent>iris::base_link</parent>
      <child>D435i::camera_link</child>
    </joint>
  </model>
</sdf>

这里有个细节需要注意：标签中的六个参数分别控制相机的位置(x,y,z)和姿态(roll,pitch,yaw)。我经过多次测试发现，上述参数值能让相机以最佳角度固定在无人机上。

4. 配置PX4启动文件

4.1 创建自定义启动文件

在${PX4_DIR}/launch/目录下新建mavros_posix_sitl_D435i.launch文件：

xml复制<launch>
  <!-- 基本参数配置 -->
  <arg name="x" default="0"/>
  <arg name="y" default="0"/>
  <arg name="z" default="0"/>
  <arg name="R" default="0"/>
  <arg name="P" default="0"/>
  <arg name="Y" default="0"/>
  
  <!-- 指定使用我们的融合模型 -->
  <arg name="vehicle" default="iris_D435i"/>
  
  <!-- 包含基础启动文件 -->
  <include file="$(find px4)/launch/posix_sitl.launch">
    <arg name="vehicle" value="$(arg vehicle)"/>
    <arg name="world" value="$(find mavlink_sitl_gazebo)/worlds/empty.world"/>
  </include>
  
  <!-- 启动MAVROS -->
  <include file="$(find mavros)/launch/px4.launch"/>
</launch>

4.2 测试仿真环境

现在可以启动仿真了：

bash复制roslaunch px4 mavros_posix_sitl_D435i.launch

如果一切正常，你应该能在Gazebo中看到带D435i相机的Iris无人机。通过以下命令检查相机数据：

bash复制# 查看可用的话题
rostopic list | grep camera

# 查看RGB图像
rqt_image_view /camera/color/image_raw

# 查看深度图像
rqt_image_view /camera/depth/image_raw

5. 常见问题排查

5.1 插件加载失败

如果Gazebo报错说找不到librealsense_gazebo_plugin.so，可能是环境变量问题。尝试：

bash复制export GAZEBO_PLUGIN_PATH=${GAZEBO_PLUGIN_PATH}:${PX4_DIR}/build/px4_sitl_default/build_gazebo

5.2 模型显示异常

有时D435i模型会显示为灰色盒子，这是因为Gazebo找不到模型纹理。确保D435i模型的meshes子目录中包含所有纹理文件。

5.3 相机数据延迟

在低配电脑上，可能会出现相机数据延迟。可以尝试降低图像分辨率：

bash复制rosrun dynamic_reconfigure dynparam set /camera/depth_module '{ "depth_width": 320, "depth_height": 240 }'

6. 进阶应用

6.1 使用自定义世界

你可以创建包含障碍物的自定义世界来测试避障算法。例如，在worlds目录下新建test.world：

xml复制<?xml version="1.0" ?>
<sdf version="1.6">
  <world name="test">
    <include>
      <uri>model://sun</uri>
    </include>
    <model name="wall">
      <static>true</static>
      <link name="link">
        <collision name="collision">
          <geometry>
            <box>
              <size>10 0.2 2</size>
            </box>
          </geometry>
        </collision>
        <visual name="visual">
          <geometry>
            <box>
              <size>10 0.2 2</size>
            </box>
          </geometry>
        </visual>
      </link>
    </model>
  </world>
</sdf>

然后在启动时指定这个世界：

bash复制roslaunch px4 mavros_posix_sitl_D435i.launch world:=test.world

6.2 与其他ROS节点集成

这个仿真环境可以完美与ROS生态集成。比如，你可以运行RTAB-Map进行SLAM建图：

bash复制roslaunch rtabmap_ros rtabmap.launch depth_topic:=/camera/depth/image_raw rgb_topic:=/camera/color/image_raw camera_info_topic:=/camera/color/camera_info

我在实际项目中用这套环境测试过多种视觉算法，从简单的颜色追踪到复杂的语义分割，效果都非常接近真实设备。特别是在开发初期，能节省大量调试时间。