PX4 GAZEBO无人机添加相机并进行图像识别

PX4 GAZEBO无人机添加摄像头并进行图像识别

在之前完成了ROS的安装和PX4的安装，并可以通过roslaunch启动软件仿真。接下来为无人及添加相机，并将图像用python函数读取，用于后续操作

1. 添加相机，参考：ROS PX4添加相机

2. 读取摄像头图像

rqt_image_view

3. 高级用法，自己添加model，参考：添加向下的相机
   但是并没有成功，博主提供了github的链接，但不知道从何处下手。
4. 添加激光雷达、前摄像头，向下摄像头，成功了。参考：添加激光雷达和摄像头

总结操作方法：

1. 修改mavros_posix_sitl.launch文件

添加内容

<arg name="my_model" default="iris_fusion"/>

修改内容，注意需要修改udp的端口，否则无法获取到传感器数据

# <arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/$(arg my_model)/$(arg my_model).sdf"/>
<arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/$(arg vehicle)/$(arg vehicle).sdf"/>
# <arg name="fcu_url" default="udp://:14540@localhost:14557"/>
<arg name="fcu_url" default="udp://:24540@localhost:14557"/>

2. models文件夹下增加iris_fusion

文件夹：PX4_Firmware/Tools/sitl_gazebo/models/

1. 添加文件夹iris_fusion
2. 添加文件iris_fusion.sdf

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

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

  <!-- Built-in depth camera -->
        <include>
      <uri>model://fpv_cam_self</uri>
      <pose>0.1 0 0 0 0.57 0</pose>
    </include>
    <joint name="fpv_cam_self_joint" type="fixed">
      <child>fpv_cam_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://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>

3. 添加文件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>

3. models文件夹下增加fpv_cam_self

1. 添加文件夹fpv_cam_self
2. 添加文件fpv_cam_self.sdf

<?xml version="1.0" ?>

<sdf version="1.5">
  <model name="fpv_cam_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/image_raw</imageTopicName>
        <cameraInfoTopicName>/camera/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>180</focalLength>
        <hackBaseline>0</hackBaseline>
      </plugin>
    </sensor>

      <gravity>0</gravity>
    </link>
  </model>

</sdf>

3. 添加文件model.config

<?xml version="1.0"?>

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

	kinetic 3D camera
  </description>

</model>

4. 测试

# 运行
roslaunch px4 mavros_posix_sitl.launch
# 查看图像
rqt_image_view