PX4无人机仿真—多无人机模型launch文件分析

Redunc

已于 2023-11-06 11:56:38 修改

阅读量1.2k

点赞数 2

文章标签：算法无人机人工智能

于 2023-11-05 20:40:53 首次发布

本文链接：https://blog.csdn.net/weixin_41097249/article/details/134234454

版权

1. 单无人机仿真launch文件分析

首先还是分析最初官方的 mavros_posix_sitl.launch这个文件，下面是这个文件的文件框架导图

大致分为以下几个步骤：

1. 声明后续需要的变量，并进行赋值
2. 引入posix_sitl文件，进行PX4软件在环仿真和运行Gazebo，并将前面定义好的变量值对该launch内成员变量进行赋值，在这个launch文件中，主要是开启了两个节点，一个是负责启动PX4固件的软件在环仿真，一个是加载无人机对应的Gazebo模型，并且又引入了一个新的launch文件，这个文件主要的功能就是加载一个世界模型，并启动Gazebo。
3. 引入px4文件，加载mavros相关的参数，

所以总的来说，想要启动一个可以通讯的px4模型，需要运行px4软件在环仿真，启动Gazebo，配置MAVROS这三大步骤。

2. XTdrone单个无人机例程launch文件分析

以indoor01.launch这个文件为例，下面是文件源码

<?xml version="1.0"?>
<launch>
    <!-- MAVROS posix SITL environment launch script -->
    <!-- launches Gazebo environment and 2x: MAVROS, PX4 SITL, and spawns vehicle -->
    <!-- vehicle model and world -->
    <arg name="est" default="ekf2"/>
    <arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/indoor1.world"/>
    <!-- gazebo configs -->
    <arg name="gui" default="true"/>
    <arg name="debug" default="false"/>
    <arg name="verbose" default="false"/>
    <arg name="paused" default="false"/>
    <!-- Gazebo sim -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="gui" value="$(arg gui)"/>
        <arg name="world_name" value="$(arg world)"/>
        <arg name="debug" value="$(arg debug)"/>
        <arg name="verbose" value="$(arg verbose)"/>
        <arg name="paused" value="$(arg paused)"/>
    </include>
     <!-- iris_0 -->
     <group ns="iris_0">
        <!-- MAVROS and vehicle configs -->
            <arg name="ID" value="0"/>
            <arg name="ID_in_group" value="0"/>
            <arg name="fcu_url" default="udp://:24540@localhost:34580"/>
        <!-- PX4 SITL and vehicle spawn -->
        <include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
            <arg name="x" value="0"/>
            <arg name="y" value="7.5"/>
            <arg name="z" value="0.5"/>
            <arg name="R" value="0"/>
            <arg name="P" value="0"/>
            <arg name="Y" value="0"/>
            <arg name="vehicle" value="iris"/>
            <arg name="sdf" value="iris_stereo_camera"/>
            <arg name="mavlink_udp_port" value="18570"/>
            <arg name="mavlink_tcp_port" value="4560"/>
            <arg name="ID" value="$(arg ID)"/>
            <arg name="ID_in_group" value="$(arg ID_in_group)"/>
        </include>
        <!-- MAVROS -->
        <include file="$(find mavros)/launch/px4.launch">
            <arg name="fcu_url" value="$(arg fcu_url)"/>
            <arg name="gcs_url" value=""/>
            <arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
            <arg name="tgt_component" value="1"/>
        </include>
    </group>

</launch>
<!--the launch file is generated by XTDrone multi-vehicle generator.py  -->

这个代码框架就比官方mavros_posix_sitl这个启动框架清楚的多，首先在20行之前，是引用了一个自定义的世界模型，并且加载到启动Gazebo的launch文件中，执行到20行，会启动一个引用前面自定义世界模型的Gazebo环境。之后就是分组对无人机进行配置，每个无人机是一组，这样做的好处就是删减新的无人机时，比较方便，且ros相关话题也根据不同的分组进行了区分，在这里进行PX4软件在环仿真和启动mavros通讯节点，在启动软件在环仿真和加载无人机模型时这里用到了另一个launch文件 single_vehicle_spawn_xtd.launch，这个文件是XTdone参考官方single_vehicle_spawn_sdf.launch 自己写的，下面我把这两者的源码附到下面：

这个是官方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>

这个是XTdrone的single_vehicle_spawn_xtd.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="sdf" default="plane"/>
    <arg name="ID" default="0"/>
    <arg name="ID_in_group" default="0"/>
    <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"/>
    <arg name="udp_gimbal_port" default="13030"/>
    <param name="robotNamespace" value="$(arg vehicle)_$(arg ID)"/>
    <!-- PX4 configs -->
    <arg name="interactive" default="true"/>
    <!-- generate sdf vehicle model -->
    <arg name="cmd" default="xmlstarlet ed -d '//plugin[@name=&quot;gimbal_controller&quot;]/udp_gimbal_port_remote' -s '//plugin[@name=&quot;gimbal_controller&quot;]' -t elem -n udp_gimbal_port_remote -v $(arg udp_gimbal_port) -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 sdf)/$(arg sdf).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_in_group) -x $(arg x) -y $(arg y) -z $(arg z) -R $(arg R) -P $(arg P) -Y $(arg Y)"/>
</launch>

对比二者发现其实区别不大，XTdrone比官方多定义了一个 udp_gimbal_port 端口，以及一个模型名字，在这个文件里主要是做了三件事情：

1. 修改模型的sdf文件内容
2. 启动PX4软件在环仿真
3. 启动模型对应的Gazebo节点

这里后两项是大差不差的，关键在于第一项，修改sdf文件，这里就比较陌生了，为什么前面单个无人机没有修改呢，答案是不需要，这些官方已经配置好了，但是在这里修改，是为了后续添加多架无人机，每个无人机都有自己mavlink协议对应的端口号，所以要修改模型内部的配置文件，具体这个xmlstarlet命令什么意思，引用路径用的什么语法，可以参考这篇博客：

创建多个带有摄像头的四旋翼gazebo模型_iris.sdf_集智飞行的博客-CSDN博客

我们把这个命令拿出来单独分析一下：

官方的：

"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"

XTdrone的：

xmlstarlet ed -d '//plugin[@name=&quot;gimbal_controller&quot;]/udp_gimbal_port_remote' 
							-s '//plugin[@name=&quot;gimbal_controller&quot;]' 
							-t elem -n udp_gimbal_port_remote 
							-v $(arg udp_gimbal_port) 
							-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 sdf)/$(arg sdf).sdf"

这里我截取了部分官方的使用说明

xml ed --help
XMLStarlet Toolkit: Edit XML document(s)
Usage: xml ed <global-options> {<action>} [ <xml-file-or-uri> ... ]
where
 <global-options> - global options for editing
 <xml-file-or-uri> - input XML document file name/uri (stdin is used if missing)
<global-options> are:
 -P (or --pf) - preserve original formatting
 -S (or --ps) - preserve non-significant spaces
 -O (or --omit-decl) - omit XML declaration (<?xml ...?>)
 -N <name>=<value> - predefine namespaces (name without ’xmlns:’)
 ex: xsql=urn:oracle-xsql
 Multiple -N options are allowed.
 -N options must be last global options.
 --help or -h - display help
where <action>
 -d or --delete <xpath>
 -i or --insert <xpath> -t (--type) elem|text|attr -n <name> -v (--value) <value>
 -a or --append <xpath> -t (--type) elem|text|attr -n <name> -v (--value) <value>
 -s or --subnode <xpath> -t (--type) elem|text|attr -n <name> -v (--value) <value>
 -m or --move <xpath1> <xpath2>
 -r or --rename <xpath1> -v <new-name>
 -u or --update <xpath> -v (--value) <value>
-x (--expr) <xpath> (-x is not implemented yet)

xmlstarlet ed -d 根据官方例程很好理解，就是删除这个路径下的udp_gimbal_port_remote节点，“-s”这个命令我在官方的文档也没找到相关的例子，在上面博客提到，好像这个用法被“-u”这个命令取代了，官方的例程也是都是介绍这个命令的，我GPT了一下，大致意思就是把上面删除的udp_gimbal_port_remote节点重新创建，并将其的值设置为之前定义的$(arg udp_gimbal_port)变量的值。这里我们看到其实XTdrone的是要比官方的多操作一个udp_gimbal_port_remote，这个是单独针对一个台风480无人机配置的，普通无人机上面是没有这个端口的，我猜可能是台风480下面的摄像头数据，由于我们只是做集群仿真，其实是用不上这个的，可以去掉。

分析完single_vehicle_spawn_xtd.launch这个文件接下来看indoor01.launch里面的后续MAVROS配置部分，这部分有一个坑，px4.launch这个文件在PX4_Firmware工作空间下有一个，但按照文件路径其实我们最后定位到的文件路径是/opt/ros/noetic/share/mavros/launch，在看源码时千万别搞混。这部分在上一篇文章里讲过了，就是启动一个MAVROS通讯节点。

至此，indoor这个文件分析完了，可以看到用这个文件框架去做多无人机扩展是很方便的，下面是我参照XTdrone的集群launch文件写的一个四架无人机仿真的launch文件:

<?xml version="1.0"?>
<launch>
    <!-- MAVROS posix SITL environment launch script -->
    <!-- launches Gazebo environment and 2x: MAVROS, PX4 SITL, and spawns vehicle -->
    <!-- vehicle model and world -->
    <arg name="est" default="ekf2"/>
    <arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/outdoor2.world"/>
    <!-- gazebo configs -->
    <arg name="gui" default="true"/>
    <arg name="debug" default="false"/>
    <arg name="verbose" default="false"/>
    <arg name="paused" default="false"/>
    <!-- Gazebo sim -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="gui" value="$(arg gui)"/>
        <arg name="world_name" value="$(arg world)"/>
        <arg name="debug" value="$(arg debug)"/>
        <arg name="verbose" value="$(arg verbose)"/>
        <arg name="paused" value="$(arg paused)"/>
    </include>
    <!-- iris_0 -->
     <group ns="iris_0">
        <!-- MAVROS and vehicle configs -->
            <arg name="ID" value="0"/>
            <arg name="ID_in_group" value="0"/>
            <arg name="fcu_url" default="udp://:24540@localhost:34580"/>
        <!-- PX4 SITL and vehicle spawn -->
        <include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
            <arg name="x" value="0"/>
            <arg name="y" value="3"/>
            <arg name="z" value="0"/>
            <arg name="R" value="0"/>
            <arg name="P" value="0"/>
            <arg name="Y" value="0"/>
            <arg name="vehicle" value="iris"/>
            <arg name="sdf" value="iris"/>
            <arg name="mavlink_udp_port" value="18570"/>
            <arg name="mavlink_tcp_port" value="4560"/>
            <arg name="udp_gimbal_port" value="13030"/>
            <arg name="ID" value="$(arg ID)"/>
            <arg name="ID_in_group" value="$(arg ID_in_group)"/>
        </include>
        <!-- MAVROS -->
        <include file="$(find mavros)/launch/px4.launch">
            <arg name="fcu_url" value="$(arg fcu_url)"/>
            <arg name="gcs_url" value=""/>
            <arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
            <arg name="tgt_component" value="1"/>
        </include>
    </group>

    <!-- iris_1 -->
     <group ns="iris_1">
        <!-- MAVROS and vehicle configs -->
            <arg name="ID" value="1"/>
            <arg name="ID_in_group" value="1"/>
            <arg name="fcu_url" default="udp://:24541@localhost:34581"/>
        <!-- PX4 SITL and vehicle spawn -->
        <include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
            <arg name="x" value="0"/>
            <arg name="y" value="6"/>
            <arg name="z" value="0"/>
            <arg name="R" value="0"/>
            <arg name="P" value="0"/>
            <arg name="Y" value="0"/>
            <arg name="vehicle" value="iris"/>
            <arg name="sdf" value="iris"/>
            <arg name="mavlink_udp_port" value="18571"/>
            <arg name="mavlink_tcp_port" value="4561"/>
            <arg name="udp_gimbal_port" value="13031"/>
            <arg name="ID" value="$(arg ID)"/>
            <arg name="ID_in_group" value="$(arg ID_in_group)"/>
        </include>
        <!-- MAVROS -->
        <include file="$(find mavros)/launch/px4.launch">
            <arg name="fcu_url" value="$(arg fcu_url)"/>
            <arg name="gcs_url" value=""/>
            <arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
            <arg name="tgt_component" value="1"/>
        </include>
    </group>
    
    <!-- iris_2 -->
     <group ns="iris_2">
    <!-- MAVROS and vehicle configs -->
            <arg name="ID" value="2"/>
            <arg name="ID_in_group" value="2"/>
            <arg name="fcu_url" default="udp://:24542@localhost:34582"/>
        <!-- PX4 SITL and vehicle spawn -->
        <include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
            <arg name="x" value="0"/>
            <arg name="y" value="9"/>
            <arg name="z" value="0"/>
            <arg name="R" value="0"/>
            <arg name="P" value="0"/>
            <arg name="Y" value="0"/>
            <arg name="vehicle" value="iris"/>
            <arg name="sdf" value="iris"/>
            <arg name="mavlink_udp_port" value="18572"/>
            <arg name="mavlink_tcp_port" value="4562"/>
            <arg name="udp_gimbal_port" value="13032"/>
            <arg name="ID" value="$(arg ID)"/>
            <arg name="ID_in_group" value="$(arg ID_in_group)"/>
        </include>
        <!-- MAVROS -->
        <include file="$(find mavros)/launch/px4.launch">
            <arg name="fcu_url" value="$(arg fcu_url)"/>
            <arg name="gcs_url" value=""/>
            <arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
            <arg name="tgt_component" value="1"/>
        </include>
    </group>

    <!-- iris_3 -->
     <group ns="iris_3">
    <!-- MAVROS and vehicle configs -->
            <arg name="ID" value="3"/>
            <arg name="ID_in_group" value="3"/>
            <arg name="fcu_url" default="udp://:24543@localhost:34583"/>
        <!-- PX4 SITL and vehicle spawn -->
        <include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
            <arg name="x" value="0"/>
            <arg name="y" value="12"/>
            <arg name="z" value="0"/>
            <arg name="R" value="0"/>
            <arg name="P" value="0"/>
            <arg name="Y" value="0"/>
            <arg name="vehicle" value="iris"/>
            <arg name="sdf" value="iris"/>
            <arg name="mavlink_udp_port" value="18573"/>
            <arg name="mavlink_tcp_port" value="4563"/>
            <arg name="udp_gimbal_port" value="13033"/>
            <arg name="ID" value="$(arg ID)"/>
            <arg name="ID_in_group" value="$(arg ID_in_group)"/>
        </include>
        <!-- MAVROS -->
        <include file="$(find mavros)/launch/px4.launch">
            <arg name="fcu_url" value="$(arg fcu_url)"/>
            <arg name="gcs_url" value=""/>
            <arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
            <arg name="tgt_component" value="1"/>
        </include>
    </group>
</launch>

这个文件相对于indoor.launch，其实就是多添加了三组无人机，并修改了相应的udp端口号。下面是仿真出来的效果，四架无人机都可以通过QGC地面站去操控。