为Gazebo中的iris无人机添加realsense D435i相机

本机环境和实现目标

本机的系统位ubuntu20.04，已安装有ros noetic和PX4_Firmware,能够在Gazebo的环境中进行无人机的仿真。因为后续需要用实验室搭载有realsense D435i相机的无人机进行集群跟踪与避障的实验，准备为Gazebo中的iris无人机搭载上D435i相机先进行仿真验证。
注意：D435i相机是一款深度相机，由左右两个红外摄像头和一个RGB相机组成，能够给用户返回深度图像、左右眼图像和RGB图像。PX4_Firmware的gazebo模型中已有配备realsense相机和搭载有realsense相机的无人机，如下图所示。但为了在心理上觉得仿真更加可靠以及我严重的强迫症，我还是希望自己将realsense开源的D435i相机模型安装到iris无人机上，嫌麻烦的可以直接使用中的iris_realsense_camera模型，该模型同样能返回深度图像、左右眼图像和RGB图像等信息。写文档的时候对比了其中的.sdf文件，两者确实是一样的，区别只在于发布图像话题的名字不同，以及相机外观可能不同。

下载realsense_gazebo_ros功能包并进行编译

新建工作空间，在工作空间中克隆realsense_gazebo_ros功能包并进行编译，过程如下：

mkdir -p realsense_ws/src
cd realsense_ws/src
git clone https://gitee.com/nie_xun/realsense_ros_gazebo.git
cd realsense_ws
catkin_make
source devel/setup.sh

编译过后我们在该工作空间的/devel/lib目录下得到了librealsense_gazebo_plugin.so共享库，在加载D435i相机模型时会调用到该共享库。
接下来查询Gazebo插件的位置，打开终端输入：

echo $GAZEBO_PLUGIN_PATH

终端返回Gazebo插件的路径，运行Gazebo时系统会到下面的路径中寻找需要用到的.so共享库，如下图所示，我Gazebo插件的路径是/home/rui27/PX4_Firmware/build/px4_sitl_default/build_gazebo。

为了确保加载相机模型时能找到刚刚编辑的librealsense_gazebo_plugin.so共享库，我们可以把刚编译得到的librealsense_gazebo_plugin.so存放到上面的路径中，也可以将目前librealsense_gazebo_plugin.so所在的目录添加进Gazebo插件的路径中。

1、将librealsense_gazebo_plugin.so存放到GAZEBO_PLUGIN_PATH中

直接复制过去即可，若权限不够，则可以用sudo nautilus命令打开文件夹再进行复制。

2、将librealsense_gazebo_plugin.so所在的添加为GAZEBO_PLUGIN_PATH

在文件夹右上角勾选”显示隐藏文件“，在主目录找到并打开.bashrc文件，在底部添加librealsense_gazebo_plugin.so所在的路径：

export GAZEBO_PLUGIN_PATH=$GAZEBO_PLUGIN_PATH:/home/rui27/realsense_ws/devel/lib

保存.bashrc文件后打开终端，输入以下指令刷新环境变量，并再次查询GAZEBO_PLUGIN_PATH，能发现新添加的路径：

source ~/.bashrc
echo $GAZEBO_PLUGIN_PATH

将D435i模型安装在iris无人机上

首先将D435i的模型放入Gazebo的模型库中。
官方给出的添加相机的方式是基于.xacro格式的，但iris无人机是基于.sdf格式的，两者并不兼容，所以我们要将.sdf格式的D435i相机放入Gazebo的模型库中，再进行相机的安装。
打开终端输入以下指令：

echo $GAZEBO_MODEL_PATH

终端会显示Gazebo中模型的路径，如下图所示：

接下来将下图中的D435i文件夹复制到中

随后在该路径下创建iris_fusion文件夹，在文件夹中添加iris_fusion.sdf文件和model.config文件,文件内容如下：

<?xml version='1.0'?>
<sdf version='1.5'>
  <model name='iris_D435i'>

    <include>
      <uri>model://iris</uri>  //引入model中的模型iris
    </include>

    <include>
      <uri>model://D435i</uri>  //引入model中的模型D435i
      <pose>0.12 0 0.02 1.5708 0 1.5708</pose>  //设置D435i模型的位置，分别是x y z yaw pitch roll
    </include>
    <joint name="D435i_joint" type="fixed">  //创建相机关节，类型为固定关节
      <child>D435i::camera_link</child>  //关节的子关节为D435i的camera_link关节（camera_link关节在D435i文件夹的.sdf文件中定义）
      <parent>iris::base_link</parent>  //关节的父关节为iris的base_link关节
      <axis>
        <xyz>0 0 0</xyz>
        <limit>
          <upper>0</upper>
          <lower>0</lower>
        </limit>
      </axis>
    </joint>

  </model>
</sdf>

<?xml version="1.0"?>
<model>
  <name>3DR Iris with D435i camera</name>
  <version>1.0</version>
  <sdf version='1.4'>iris_fusion.sdf</sdf>

  <author>
   <name>Amy Wagoner</name>
   <email>arwagoner@gmail.com</email>
  </author>

  <description>
    This is a model of the 3DR Iris Quadrotor with an D435i camera. The original model has been created by
    Thomas Gubler and is maintained by Lorenz Meier.
  </description>
</model>

随后修改PX4_Firmware中的launch文件，打开mavros_posix_sitl.launch,添加或修改如下内容：

此时若直接修改并运行launch会报错找不到.config文件，这是因为还有必要的config文件不在D435i文件夹中，我们还需要将前面克隆的realsense_ros_gazebo文件夹整个丢进GAZEBO_MODEL_PATH中
最后运行该launch文件，Gazebo启动并出现带有D435i的iris无人机，此时可以看到相机发布的图像话题，并能用rqt显示图像

roslaunch px4 mavros_posix_sitl.launch
rostopic list
rqt_image_view