创建多个带有摄像头的四旋翼gazebo模型

最新推荐文章于 2024-09-10 10:23:45 发布

集智飞行

最新推荐文章于 2024-09-10 10:23:45 发布

阅读量1.7k

点赞数 5

分类专栏：无人机开发 gazebo 仿真

本文链接：https://blog.csdn.net/benchuspx/article/details/116461926

版权

无人机开发同时被 3 个专栏收录

27 篇文章 7 订阅

订阅专栏

gazebo

11 篇文章 1 订阅

订阅专栏

仿真

7 篇文章 0 订阅

订阅专栏

完整的model文件和launch文件我上传在资源里：多个带有摄像头的四旋翼gazebo模型.zip

1、启动多个iris四旋翼模型

PX4源码里有iris.sdf等四旋翼模型文件，但是如果要启动多个iris模型和mavlink流，则需要用到PX4源码launch文件夹里的single_vehicle_spawn.launch或者single_vehicle_spawn_sdf.launch

去读这两个launch文件就会发现它们启动多个sdf四旋翼模型的思路不同。前者是用px4源码中的python脚本去修改jinja的mavlink端口号等，这个我没仔细研究；后者single_vehicle_spawn_sdf.launch是用xmlstarlet工具修改sdf文件里的mavlink_tcp_port号。

single_vehicle_spawn_sdf.launch如下：

<?xml version="1.0"?>
<launch>
    <!-- Posix SITL environment launch script -->
    <!-- launchs PX4 SITL 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"/>
    <!-- vehcile model and config -->
    <arg name="est" default="ekf2"/>
    <arg name="vehicle" default="plane"/>
    <arg name="ID" default="1"/>
    <env name="PX4_SIM_MODEL" value="$(arg vehicle)" />
    <env name="PX4_ESTIMATOR" value="$(arg est)" />
    <arg name="mavlink_udp_port" default="14560"/>
    <arg name="mavlink_tcp_port" default="4560"/>
    <!-- PX4 configs -->
    <arg name="interactive" default="true"/>
    <!-- generate sdf vehicle model -->
    <arg name="cmd" default="xmlstarlet ed -d '//plugin[@name=&quot;mavlink_interface&quot;]/mavlink_tcp_port' -s '//plugin[@name=&quot;mavlink_interface&quot;]' -t elem -n mavlink_tcp_port -v $(arg mavlink_tcp_port) $(find px4)/Tools/sitl_gazebo/models/$(arg vehicle)/$(arg vehicle).sdf"/>
    <param command="$(arg cmd)" name="model_description"/>
    <!-- PX4 SITL -->
    <arg unless="$(arg interactive)" name="px4_command_arg1" value=""/>
    <arg     if="$(arg interactive)" name="px4_command_arg1" value="-d"/>
    <node name="sitl_$(arg ID)" pkg="px4" type="px4" output="screen" args="$(find px4)/build/px4_sitl_default/etc -s etc/init.d-posix/rcS -i $(arg ID) -w sitl_$(arg vehicle)_$(arg ID) $(arg px4_command_arg1)">
    </node>
    <!-- spawn vehicle -->
    <node name="$(arg vehicle)_$(arg ID)_spawn" output="screen" pkg="gazebo_ros" type="spawn_model" args="-sdf -param model_description -model $(arg vehicle)_$(arg ID) -x $(arg x) -y $(arg y) -z $(arg z) -R $(arg R) -P $(arg P) -Y $(arg Y)"/>
</launch>

因为sdf模型文件也是xml格式，所以可以用xmlstarlet来修改。首先需要安装xmlstarlet：

sudo apt install xmlstarlet

xmlstarlet的用法可以参考help：

xmlstarlet -h
xmlstarlet ed -h

用的最多的是xmlstarlet ed，就是编辑功能，它可以删除、修改、添加xml文件里的元素（element）、属性（attribute）的名字和值。

xmlstarlet的使用说明可以参考官网http://xmlstar.sourceforge.net/docs.php以及里面的pdf、html格式的教程和示例。

其中需要了解以下xml中的xpath路径写法，可以参考https://www.w3school.com.cn/xpath/xpath_nodes.asp

比如以上'//plugin[@name="mavlink_interface"]' 就是一个xpath，表示xml文件中所有name属性是“mavlink_interface"的plugin元素。这里它是先把mavlink_interface的plugin里的mavlink_tcp_port删掉，再加一个改了端口值的mavlink_tcp_port。这样其实也没必要，可以用xmlstarlet -u功能直接修改mavlink_tcp_port的值。不过xmlstarlet -u好像是比较新的xmlstarlet版本才支持。

2、多个摄像头模型

当然px4源码里面也给了depth_camera和fpv_camera，我用的是depth_camera，但是它的sdf文件没有加命名空间，导致打开多个摄像头模型时camera的话题都是一样的，会冲突。因此我加了一个robotNamespace，再结合前面的xmlstarlet修改元素值的功能，就能给不同的摄像头模型设置不同的话题前缀，而不需要重复编辑多个摄像头模型。最后把摄像头模型和iris四旋翼模型放在一起的sdf模型如下（注意不能用<include>添加两个模型，因为xmlstarlet是直接编辑这个.sdf文件）

<sdf version='1.6'>
  <model name='iris_usb_camera_down'>

<!--the camera is changed from depth camera model in PX4-->
  <model name="usb_cam_uavros">
    <pose>0.08 0 -0.01 0 1.5708 0</pose>
    <link name="link">
      <inertial>
        <pose>0.01 0.025 0.025 0 0 0</pose>
        <mass>0.01</mass>
        <inertia>
          <ixx>4.15e-6</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>2.407e-6</iyy>
          <iyz>0</iyz>
          <izz>2.407e-6</izz>
        </inertia>
      </inertial>
      <visual name="visual">
        <pose>0 0 0 0 1.57 0</pose>
        <geometry>
          <mesh>
            <uri>model://iris_usb_camera_down/meshes/hokuyo.dae</uri>
          </mesh>
        </geometry>
      </visual>
        <collision name='collision'>
          <geometry>
            <box>
              <size>0.06 0.06 0.06</size>
            </box>
          </geometry>
          <max_contacts>10</max_contacts>
          <surface>
            <contact>
              <ode/>
            </contact>
            <bounce/>
            <friction>
              <torsional>
                <ode/>
              </torsional>
              <ode/>
            </friction>
          </surface>
        </collision>

      <sensor name="depth_camera" type="depth">
        <update_rate>20</update_rate>
        <camera>
          <horizontal_fov>1.047198</horizontal_fov>
          <image>
            <format>R8G8B8</format>
            <width>1280</width>
            <height>720</height>
          </image>
          <clip>
            <near>0.05</near>
            <far>15</far>
          </clip>
        </camera>
        <plugin filename="libgazebo_ros_openni_kinect.so" name="camera_controller">
          <baseline>0.2</baseline>
          <robotNamespace></robotNamespace> <!--add by spx, default is the robot model name-->
          <cameraName>usb_cam</cameraName> <!--camera-->
          <alwaysOn>true</alwaysOn>
          <updateRate>20</updateRate>
          <pointCloudCutoff>0.05</pointCloudCutoff>
          <imageTopicName>image_raw</imageTopicName> <!--rgb/image_raw-->
          <cameraInfoTopicName>camera_info</cameraInfoTopicName> <!--rgb/camera_info-->
          <depthImageTopicName>depth/image_raw</depthImageTopicName>
          <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
          <pointCloudTopicName>depth/points</pointCloudTopicName>
          <frameName>depth_camera_link</frameName>
          <distortion_k1>0.0</distortion_k1>
          <distortion_k2>0.0</distortion_k2>
          <distortion_k3>0.0</distortion_k3>
          <distortion_t1>0.0</distortion_t1>
          <distortion_t2>0.0</distortion_t2>
          <CxPrime>0</CxPrime>
          <Cx>0</Cx>
          <Cy>0</Cy>
          <focalLength>0</focalLength>
          <hackBaseline>0</hackBaseline>
        </plugin>
      </sensor>
     </link>
  </model>

    <joint name="iris_camera_joint" type="fixed">
      <child>usb_cam_uavros::link</child>
      <parent>base_link</parent>
      <axis>
        <xyz>0 0 1</xyz>
        <limit>
          <upper>0</upper>
          <lower>0</lower>
        </limit>
      </axis>
    </joint>

<!--the rest is iris model-->
    <link name='base_link'>
      <pose>0 0 0 0 0 0</pose>
      <inertial>
        <pose>0 0 0 0 0 0</pose>
        <mass>1.5</mass>
        <inertia>
       …………………省略…………………

  </model>
</sdf>

完整的model文件和launch文件我上传在资源里：多个带有摄像头的四旋翼gazebo模型.zip

注意下载后还要根据实际调整ros package以及model的位置和路径。替换world和其他不需要的节点。

3、启动多个带有摄像头的四旋翼gazebo模型

可以看我上面上传的资源里的两个launch文件，比较于px4原本的multi_uav_mavros_sitl_sdf.launch和single_vehicle_spawn_sdf.launch，我修改后可以任意指定模型sdf的位置，而且添加了摄像头的camNamespace参数，可以给摄像头图像话题加上前缀了。下面是我改的其中一个launch文件single_iris_usbcam_down.launch：

<launch>
    <!-- Posix SITL environment launch script -->
    <!-- launchs PX4 SITL 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"/>
    <!-- vehcile model and config -->
    <arg name="est" default="ekf2"/>
    <arg name="vehicle" default="iris"/>
    <arg name="sdf" default="$(find uavros_gazebo)/models/iris_usb_camera_down/iris_usb_camera_down.sdf"/> <!--spx add-->
    <arg name="ID" default="1"/>
    <env name="PX4_SIM_MODEL" value="$(arg vehicle)" />
    <env name="PX4_ESTIMATOR" value="$(arg est)" />
    <arg name="mavlink_udp_port" default="14560"/> <!-- no use here-->
    <arg name="mavlink_tcp_port" default="4560"/>
    <arg name="camNamespace" default=""/> <!--by spx-->
    <!-- PX4 configs -->
    <arg name="interactive" default="true"/>
    <!-- generate sdf vehicle model -->
    <arg name="cmd" default="xmlstarlet ed -d '//plugin[@name=&quot;mavlink_interface&quot;]/mavlink_tcp_port' -s '//plugin[@name=&quot;mavlink_interface&quot;]' -t elem -n mavlink_tcp_port -v $(arg mavlink_tcp_port) -u '//plugin[@name=&quot;camera_controller&quot;]/robotNamespace' -v '$(arg camNamespace)' $(arg sdf)"/><!--camera added by spx-->
    <param command="$(arg cmd)" name="model_description"/>
    <!-- PX4 SITL -->
    <arg unless="$(arg interactive)" name="px4_command_arg1" value=""/>
    <arg     if="$(arg interactive)" name="px4_command_arg1" value="-d"/>
    <node name="sitl_$(arg ID)" pkg="px4" type="px4" output="screen" args="$(find px4)/build/px4_sitl_default/etc -s etc/init.d-posix/rcS -i $(arg ID) -w sitl_$(arg vehicle)_$(arg ID) $(arg px4_command_arg1)">
    </node>
    <!-- spawn vehicle -->
    <node name="$(arg vehicle)_$(arg ID)_spawn" output="screen" pkg="gazebo_ros" type="spawn_model" args="-sdf -param model_description -model $(arg vehicle)_$(arg ID) -x $(arg x) -y $(arg y) -z $(arg z) -R $(arg R) -P $(arg P) -Y $(arg Y)"/>
</launch>