在px4，gazebo环境中添加激光雷达，双目相机和下视摄像头

在搭建好px4的仿真环境后，gazebo中仅为一架裸机，不含其他传感器。本文将在该环境下把激光雷达，双目相机，下视摄像头集成到飞机上，方便后续的算法测试。

1. 修改仿真启动文件
   找到/Firmware/launch 中的mavros_posix_sitl.launch文件。在文件model调用处添加如下

`   <arg name="vehicle" default="iris"/>
    <arg name="my_model" default="iris_fusion"/>
    <arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/empty.world"/>
    <arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/$(arg my_model)/$(arg my_model).sdf"/>`

把该launch文件中调用的model改为自己的model，改完后整个文件内容如下

<?xml version="1.0"?>
<launch>
    <!-- MAVROS posix SITL environment launch script -->
    <!-- launches MAVROS, PX4 SITL, Gazebo environment, and spawns vehicle -->
    <!-- vehicle pose -->
    <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"/>
    <!-- vehicle model and world -->
    <arg name="est" default="ekf2"/>
    <arg name="vehicle" default="iris"/>
    <arg name="my_model" default="iris_fusion"/>
    <arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/empty.world"/>
    <arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/$(arg my_model)/$(arg my_model).sdf"/>

    <!-- gazebo configs -->
    <arg name="gui" default="true"/>
    <arg name="debug" default="false"/>
    <arg name="verbose" default="false"/>
    <arg name="paused" default="false"/>
    <arg name="respawn_gazebo" default="false"/>
    <!-- MAVROS configs -->
    <arg name="fcu_url" default="udp://:14540@localhost:14557"/>
    <arg name="respawn_mavros" default="false"/>
    <!-- PX4 configs -->
    <arg name="interactive" default="true"/>
    <!-- PX4 SITL and Gazebo -->
    <include file="$(find px4)/launch/posix_sitl.launch">
        <arg name="x" value="$(arg x)"/>
        <arg name="y" value="$(arg y)"/>
        <arg name="z" value="$(arg z)"/>
        <arg name="R" value="$(arg R)"/>
        <arg name="P" value="$(arg P)"/>
        <arg name="Y" value="$(arg Y)"/>
        <arg name="world" value="$(arg world)"/>
        <arg name="vehicle" value="$(arg vehicle)"/>
        <arg name="sdf" value="$(arg sdf)"/>
        <arg name="gui" value="$(arg gui)"/>
        <arg name="interactive" value="$(arg interactive)"/>
        <arg name="debug" value="$(arg debug)"/>
        <arg name="verbose" value="$(arg verbose)"/>
        <arg name="paused" value="$(arg paused)"/>
        <arg name="respawn_gazebo" value="$(arg respawn_gazebo)"/>
    </include>
    <!-- MAVROS -->
    <include file="$(find mavros)/launch/px4.launch">
        <!-- GCS link is provided by SITL -->
        <arg name="gcs_url" value=""/>
        <arg name="fcu_url" value="$(arg fcu_url)"/>
        <arg name="respawn_mavros" value="$(arg respawn_mavros)"/>
    </include>
</launch>

先进行双目相机插件的书写
在/Firmware/Tools/sitl_gazebo/models中添加文件夹kinect_self,在该文件夹下创建两个文件kinect_self.sdf model.config
更改kinect_self.sdf内容如下

<?xml version="1.0" ?>

<sdf version="1.5">
  <model name="kinect_self">
    <pose>-2 2.5 1 0 0 0</pose>
    <link name="link">
      <inertial>
        <mass>0.01</mass>
        <inertia>
          <ixx>4.16666666667e-06</ixx>
          <iyy>5.20833333333e-07</iyy>
          <izz>3.85416666667e-06</izz>
        </inertia>
      </inertial>

      <collision name="collision">
        <geometry>
          <box>
            <size>0.023000 0.076000 0.032000</size>
          </box>
        </geometry>
      </collision>
      <visual name="visual">
        <geometry>
          <box>
            <size>0.023000 0.076000 0.032000</size>
          </box>
        </geometry>
      </visual>
      <sensor type="depth" name="camera">
      <always_on>true</always_on>
      <update_rate>30.0</update_rate>
      <camera>
        <horizontal_fov>1.3962634</horizontal_fov>　　
        <image>
          <format>B8G8R8</format>
          <width>640</width>
          <height>480</height>
        </image>
        <clip>
          <near>0.4</near>
          <far>16.0</far>
        </clip>
      </camera>
      <plugin name="camera_plugin" filename="libgazebo_ros_openni_kinect.so">
        <baseline>0.2</baseline>
        <alwaysOn>true</alwaysOn>
        <!-- Keep this zero, update_rate in the parent <sensor> tag
          will control the frame rate. -->
        <updateRate>0.0</updateRate>
        <cameraName>camera_ir</cameraName>
        <imageTopicName>/camera/color/image_raw</imageTopicName>
        <cameraInfoTopicName>/camera/color/camera_info</cameraInfoTopicName>
        <depthImageTopicName>/camera/depth/image_raw</depthImageTopicName>
        <depthImageCameraInfoTopicName>/camera/depth/camera_info</depthImageCameraInfoTopicName>
        <pointCloudTopicName>/camera/depth/points</pointCloudTopicName>
        <frameName>camera_link</frameName>
        <pointCloudCutoff>0.5</pointCloudCutoff>
        <pointCloudCutoffMax>8.0</pointCloudCutoffMax>
        <distortionK1>0</distortionK1>
        <distortionK2>0</distortionK2>
        <distortionK3>0</distortionK3>
        <distortionT1>0</distortionT1>
        <distortionT2>0</distortionT2>
        <CxPrime>0</CxPrime>
        <Cx>0</Cx>
        <Cy>0</Cy>
        <focalLength>0</focalLength>
        <hackBaseline>0</hackBaseline>
      </plugin>
    </sensor>
      <gravity>0</gravity>
    </link>
  </model>
  
</sdf>

该model.config文件内容如下

<?xml version="1.0"?>

<model>
  <name>Kinect_self</name>
  <version>1.0</version>
  <sdf version="1.5">kinect_self.sdf</sdf>
  <description>
	Work in progress.

	kinetic 3D camera
  </description>

</model>

然后书写整合文件
在/Firmware/Tools/sitl_gazebo/models中添加文件夹iris_fusion,在该文件夹下创建两个文件iris_fusion.sdf model.config
修改iris_fusion.sdf内容如下

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

    <include>
      <uri>model://iris</uri>
    </include>

    <include>
      <uri>model://kinect_self</uri>
      <pose>0.1 0 0 0 0 0</pose>
    </include>
    <joint name="kinect_self_joint" type="fixed">
      <child>kinect_self::link</child>
      <parent>iris::base_link</parent>
      <axis>
        <xyz>0 0 1</xyz>
        <limit>
          <upper>0</upper>
          <lower>0</lower>
        </limit>
      </axis>
    </joint>

    <include>
      <uri>model://rplidar</uri>
      <pose>0 0 0.1 0 0 0</pose>
    </include>
    <joint name="rplidar_joint" type="revolute">
      <child>rplidar::link</child>
      <parent>iris::base_link</parent>
      <axis>
        <xyz>0 0 1</xyz>
        <limit>
          <upper>0</upper>
          <lower>0</lower>
        </limit>
      </axis>
    </joint>

    <include>
      <uri>model://fpv_cam</uri>
      <pose>0 0 0 0 1.57 0</pose>
    </include>
    <joint name="fpv_cam_joint" type="fixed">
      <child>fpv_cam::link</child>
      <parent>iris::base_link</parent>
      <axis>
        <xyz>0 0 1</xyz>
        <limit>
          <upper>0</upper>
          <lower>0</lower>
        </limit>
      </axis>
    </joint>


  </model>
</sdf>

修改model.config内容如下

<?xml version="1.0"?>
<model>
  <name>3DR Iris with FPV 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 FPV camera. The original model has been created by
    Thomas Gubler and is maintained by Lorenz Meier.
  </description>
</model>

至此就在飞机上同时加载了激光雷达，双目相机，下置摄像头
运行代码

roslaunch px4 mavros_posix_sitl.launch

就可以看到如下场景打开rqt_image_view可以看到前置双目和下置摄像头的信息，
通过rostopic_list 可以看到有/laser/scan话题的发布
至此本章结束