devbox
菜鸟追梦旅行
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

PX4 + Gazebo：多机仿真测试，Offboard模式控制_offboard可以控制多个无人机吗

作者：菜鸟追梦旅行 | 2024-06-08 05:39:58

赞

踩

offboard可以控制多个无人机吗

参考文章：

CSDN：px4与gazebo的多无人机编队仿真，offboard模式

PX4官网：Multiple Vehicles with ROS and Gazebo

预备知识：

默认单机模式已经可以起飞，可以参考其他文章，在此不赘述

实现效果：

多机飞行

具体操作：

1. 修改lauch文件

打开自己的PX4文件目录launch文件夹里的：“multi_uav_mavros_sitl.launch”文件，这里需要对里边的内容修改。

这里列出来我的四架无人机设置的设置：

  1. <?xml version="1.0"?>
  2. <launch>
  3.     <!-- MAVROS posix SITL environment launch script -->
  4.     <!-- launches Gazebo environment and 2x: MAVROS, PX4 SITL, and spawns vehicle -->
  5.     <!-- vehicle model and world -->
  6.     <arg name="est" default="ekf2"/>
  7.     <arg name="vehicle" default="iris"/>
  8.     <arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/empty.world"/>
  9.     <!-- gazebo configs -->
  10.     <arg name="gui" default="true"/>
  11.     <arg name="debug" default="false"/>
  12.     <arg name="verbose" default="false"/>
  13.     <arg name="paused" default="false"/>
  14.     <!-- Gazebo sim -->
  15.     <include file="$(find gazebo_ros)/launch/empty_world.launch">
  16.         <arg name="gui" value="$(arg gui)"/>
  17.         <arg name="world_name" value="$(arg world)"/>
  18.         <arg name="debug" value="$(arg debug)"/>
  19.         <arg name="verbose" value="$(arg verbose)"/>
  20.         <arg name="paused" value="$(arg paused)"/>
  21.     </include>
  22.     <!-- UAV0 -->
  23.     <group ns="uav0">
  24.         <!-- MAVROS and vehicle configs -->
  25.         <arg name="ID" value="0"/>
  26.         <arg name="fcu_url" default="udp://:14540@localhost:14580"/>
  27.         <!-- PX4 SITL and vehicle spawn -->
  28.         <include file="$(find px4)/launch/single_vehicle_spawn.launch">
  29.             <arg name="x" value="-3"/>
  30.             <arg name="y" value="0"/>
  31.             <arg name="z" value="0"/>
  32.             <arg name="R" value="0"/>
  33.             <arg name="P" value="0"/>
  34.             <arg name="Y" value="0"/>
  35.             <arg name="vehicle" value="$(arg vehicle)"/>
  36.             <arg name="mavlink_udp_port" value="14560"/>
  37.             <arg name="mavlink_tcp_port" value="4560"/>
  38.             <arg name="ID" value="$(arg ID)"/>
  39.             <arg name="gst_udp_port" value="$(eval 5600 + arg('ID'))"/>
  40.             <arg name="video_uri" value="$(eval 5600 + arg('ID'))"/>
  41.             <arg name="mavlink_cam_udp_port" value="$(eval 14530 + arg('ID'))"/>
  42.         </include>
  43.         <!-- MAVROS -->
  44.         <include file="$(find mavros)/launch/px4.launch">
  45.             <arg name="fcu_url" value="$(arg fcu_url)"/>
  46.             <arg name="gcs_url" value=""/>
  47.             <arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
  48.             <arg name="tgt_component" value="1"/>
  49.         </include>
  50.     </group>
  51.     <!-- UAV1 -->
  52.     <group ns="uav1">
  53.         <!-- MAVROS and vehicle configs -->
  54.         <arg name="ID" value="1"/>
  55.         <arg name="fcu_url" default="udp://:14541@localhost:14581"/>
  56.         <!-- PX4 SITL and vehicle spawn -->
  57.         <include file="$(find px4)/launch/single_vehicle_spawn.launch">
  58.             <arg name="x" value="-1"/>
  59.             <arg name="y" value="0"/>
  60.             <arg name="z" value="0"/>
  61.             <arg name="R" value="0"/>
  62.             <arg name="P" value="0"/>
  63.             <arg name="Y" value="0"/>
  64.             <arg name="vehicle" value="$(arg vehicle)"/>
  65.             <arg name="mavlink_udp_port" value="14561"/>
  66.             <arg name="mavlink_tcp_port" value="4561"/>
  67.             <arg name="ID" value="$(arg ID)"/>
  68.             <arg name="gst_udp_port" value="$(eval 5600 + arg('ID'))"/>
  69.             <arg name="video_uri" value="$(eval 5600 + arg('ID'))"/>
  70.             <arg name="mavlink_cam_udp_port" value="$(eval 14530 + arg('ID'))"/>
  71.         </include>
  72.         <!-- MAVROS -->
  73.         <include file="$(find mavros)/launch/px4.launch">
  74.             <arg name="fcu_url" value="$(arg fcu_url)"/>
  75.             <arg name="gcs_url" value=""/>
  76.             <arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
  77.             <arg name="tgt_component" value="1"/>
  78.         </include>
  79.     </group>
  80.     <!-- UAV2 -->
  81.     <group ns="uav2">
  82.         <!-- MAVROS and vehicle configs -->
  83.         <arg name="ID" value="2"/>
  84.         <arg name="fcu_url" default="udp://:14542@localhost:14582"/>
  85.         <!-- PX4 SITL and vehicle spawn -->
  86.         <include file="$(find px4)/launch/single_vehicle_spawn.launch">
  87.             <arg name="x" value="1"/>
  88.             <arg name="y" value="0"/>
  89.             <arg name="z" value="0"/>
  90.             <arg name="R" value="0"/>
  91.             <arg name="P" value="0"/>
  92.             <arg name="Y" value="0"/>
  93.             <arg name="vehicle" value="$(arg vehicle)"/>
  94.             <arg name="mavlink_udp_port" value="14562"/>
  95.             <arg name="mavlink_tcp_port" value="4562"/>
  96.             <arg name="ID" value="$(arg ID)"/>
  97.             <arg name="gst_udp_port" value="$(eval 5600 + arg('ID'))"/>
  98.             <arg name="video_uri" value="$(eval 5600 + arg('ID'))"/>
  99.             <arg name="mavlink_cam_udp_port" value="$(eval 14530 + arg('ID'))"/>
  100.         </include>
  101.         <!-- MAVROS -->
  102.         <include file="$(find mavros)/launch/px4.launch">
  103.             <arg name="fcu_url" value="$(arg fcu_url)"/>
  104.             <arg name="gcs_url" value=""/>
  105.             <arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
  106.             <arg name="tgt_component" value="1"/>
  107.         </include>
  108.     </group>
  109.     <group ns="uav3">
  110.         <!-- MAVROS and vehicle configs -->
  111.         <arg name="ID" value="3"/>
  112.         <arg name="fcu_url" default="udp://:14542@localhost:14583"/>
  113.         <!-- PX4 SITL and vehicle spawn -->
  114.         <include file="$(find px4)/launch/single_vehicle_spawn.launch">
  115.             <arg name="x" value="3"/>
  116.             <arg name="y" value="0"/>
  117.             <arg name="z" value="0"/>
  118.             <arg name="R" value="0"/>
  119.             <arg name="P" value="0"/>
  120.             <arg name="Y" value="0"/>
  121.             <arg name="vehicle" value="$(arg vehicle)"/>
  122.             <arg name="mavlink_udp_port" value="14563"/>
  123.             <arg name="mavlink_tcp_port" value="4563"/>
  124.             <arg name="ID" value="$(arg ID)"/>
  125.             <arg name="gst_udp_port" value="$(eval 5600 + arg('ID'))"/>
  126.             <arg name="video_uri" value="$(eval 5600 + arg('ID'))"/>
  127.             <arg name="mavlink_cam_udp_port" value="$(eval 14530 + arg('ID'))"/>
  128.         </include>
  129.         <!-- MAVROS -->
  130.         <include file="$(find mavros)/launch/px4.launch">
  131.             <arg name="fcu_url" value="$(arg fcu_url)"/>
  132.             <arg name="gcs_url" value=""/>
  133.             <arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
  134.             <arg name="tgt_component" value="1"/>
  135.         </include>
  136.     </group>
  137. </launch>
  138. <!-- to add more UAVs (up to 10):
  139. Increase the id
  140. Change the name space
  141. Set the FCU to default="udp://:14540+id@localhost:14550+id"
  142. Set the malink_udp_port to 14560+id) -->

此文件重点关注Group里的内容，把一个group里的参数解释如下

  1. <!-- UAV0 -->
  2.     <group ns="uav0">
  3.         <!-- MAVROS and vehicle configs -->
  4.         <arg name="ID" value="0"/>  //这个是无人机的id，可以根据自己增加的数量修改
  5.         <arg name="fcu_url" default="udp://:14540@localhost:14580"/> //这个是和mavros连接的时候的地址 需要根据自己的id修改，比如uav1的话就是："udp://:14541@localhost:14581"
  6.         <!-- PX4 SITL and vehicle spawn -->
  7.         <include file="$(find px4)/launch/single_vehicle_spawn.launch"> //这里是调用spawn.launch 我不太懂这句话的意思，希望有大佬指点一下。
  8.             <arg name="x" value="-3"/>  //下边 6个变量 代表无人机起始位置的位置和姿态
  9.             <arg name="y" value="0"/>
  10.             <arg name="z" value="0"/>
  11.             <arg name="R" value="0"/>
  12.             <arg name="P" value="0"/>
  13.             <arg name="Y" value="0"/>
  14.             <arg name="vehicle" value="$(arg vehicle)"/>
  15.             <arg name="mavlink_udp_port" value="14560"/> //这里是udp端口，需要修改最后一位
  16.             <arg name="mavlink_tcp_port" value="4560"/>  //这里是tcp端口，需要修改最后一位
  17.             <arg name="ID" value="$(arg ID)"/> //获取id，后边的都不用改了，自动加id了
  18.             <arg name="gst_udp_port" value="$(eval 5600 + arg('ID'))"/>
  19.             <arg name="video_uri" value="$(eval 5600 + arg('ID'))"/>
  20.             <arg name="mavlink_cam_udp_port" value="$(eval 14530 + arg('ID'))"/>
  21.         </include>
  22.         <!-- MAVROS --> //下边的内容是启动mavros 和自己运行 roslaunch mavros px4.launch fcu_url:="xxxx"类似，不赘述
  23.         <include file="$(find mavros)/launch/px4.launch">
  24.             <arg name="fcu_url" value="$(arg fcu_url)"/>
  25.             <arg name="gcs_url" value=""/>
  26.             <arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
  27.             <arg name="tgt_component" value="1"/>
  28.         </include>
  29.     </group>

根据上边提示修改好自己的launch文件，官方提示最多仿真10架。

2.运行仿真

  1. 进入自己的代码目录，进行编译（如果自己单机能跑，这一步可以省略）

    1. cd Firmware_clone
    2. git submodule update --init --recursive
    3. DONT_RUN=1 make px4_sitl_default gazebo

  2. Source 环境（这一步需要大家自己看一下文件目录里的位置，每个版本可能存在一定的差异，更换到自己路径即可）

    1. source Tools/setup_gazebo.bash $(pwd) $(pwd)/build/px4_sitl_default
    2. export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd):$(pwd)/Tools/sitl_gazebo

  3. 运行启动文件:

    roslaunch px4 multi_uav_mavros_sitl.launch

 没有什么问题的话应该是可以看到Gazebo里出现了四架飞机，然后初始位置和我们设置的相同。

此时打开终端测试一下mavros连接情况：

rostopic list

注意到这里的的话题会增加一个飞机编号：

3.offboard控制

单机的代码里的话题名称没有前边的“/uav1”，所以修改的话也很简单，给每个无人机单独编写一下话题名称，然后在发布位置或者解锁的时候调用各自的话题。这里直接参考上述CSDN中的文章给的代码，实现四架无人机的画圆飞行。

这里和官方给的单机的offboard代码类似，增加了多机的控制和发布位置的设置。

能把官方的读懂这个代码看起应该问题不大。

  1. /**
  2.  * @file offb_node.cpp
  3.  * @brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro Flight
  4.  * Stack and tested in Gazebo SITL
  5.  */
  6.  
  7. #include <ros/ros.h>
  8. #include <geometry_msgs/PoseStamped.h>
  9. #include <mavros_msgs/CommandBool.h>
  10. #include <mavros_msgs/SetMode.h>
  11. #include <mavros_msgs/State.h>
  12.  
  13. mavros_msgs::State current_state;
  14. void state_cb(const mavros_msgs::State::ConstPtr& msg){
  15.     current_state = *msg;
  16. }
  17.  
  18. int main(int argc, char **argv)
  19. {
  20.     ros::init(argc, argv, "offb_node");
  21.  
  22.     //uav0
  23.     ros::NodeHandle nh0;
  24.     ros::Subscriber state_sub0 = nh0.subscribe<mavros_msgs::State>
  25.             ("uav0/mavros/state", 10, state_cb);
  26.     ros::Publisher local_pos_pub0 = nh0.advertise<geometry_msgs::PoseStamped>
  27.             ("/uav0/mavros/setpoint_position/local", 10);
  28.     ros::ServiceClient arming_client0 = nh0.serviceClient<mavros_msgs::CommandBool>
  29.             ("/uav0/mavros/cmd/arming");
  30.     ros::ServiceClient set_mode_client0 = nh0.serviceClient<mavros_msgs::SetMode>
  31.             ("/uav0/mavros/set_mode");
  32.  
  33.     //uav1
  34.     ros::NodeHandle nh1;
  35.     ros::Subscriber state_sub1 = nh1.subscribe<mavros_msgs::State>
  36.             ("uav1/mavros/state", 10, state_cb);
  37.     ros::Publisher local_pos_pub1 = nh1.advertise<geometry_msgs::PoseStamped>
  38.             ("/uav1/mavros/setpoint_position/local", 10);
  39.     ros::ServiceClient arming_client1 = nh1.serviceClient<mavros_msgs::CommandBool>
  40.             ("/uav1/mavros/cmd/arming");
  41.     ros::ServiceClient set_mode_client1 = nh1.serviceClient<mavros_msgs::SetMode>
  42.             ("/uav1/mavros/set_mode");
  43.  
  44.     //uav2
  45.     ros::NodeHandle nh2;
  46.     ros::Subscriber state_sub2 = nh2.subscribe<mavros_msgs::State>
  47.             ("uav2/mavros/state", 10, state_cb);
  48.     ros::Publisher local_pos_pub2 = nh2.advertise<geometry_msgs::PoseStamped>
  49.             ("/uav2/mavros/setpoint_position/local", 10);
  50.     ros::ServiceClient arming_client2 = nh2.serviceClient<mavros_msgs::CommandBool>
  51.             ("/uav2/mavros/cmd/arming");
  52.     ros::ServiceClient set_mode_client2 = nh2.serviceClient<mavros_msgs::SetMode>
  53.             ("/uav2/mavros/set_mode");
  54.  
  55.     //uav3
  56.     ros::NodeHandle nh3;
  57.     ros::Subscriber state_sub3 = nh3.subscribe<mavros_msgs::State>
  58.             ("uav3/mavros/state", 10, state_cb);
  59.     ros::Publisher local_pos_pub3 = nh3.advertise<geometry_msgs::PoseStamped>
  60.             ("/uav3/mavros/setpoint_position/local", 10);
  61.     ros::ServiceClient arming_client3 = nh3.serviceClient<mavros_msgs::CommandBool>
  62.             ("/uav3/mavros/cmd/arming");
  63.     ros::ServiceClient set_mode_client3 = nh3.serviceClient<mavros_msgs::SetMode>
  64.             ("/uav3/mavros/set_mode");
  65.  
  66.     //the setpoint publishing rate MUST be faster than 2Hz
  67.     ros::Rate rate(20.0);
  68.  
  69.     // wait for FCU connection
  70.     while(ros::ok() && !current_state.connected){
  71.         ros::spinOnce();
  72.         rate.sleep();
  73.     }
  74.     // x_num, y_num  refer to the desired uav input position
  75.     //all the uavs have the same height : z
  76.  
  77.     float x0 = 0.0, y0 = 0.0;
  78.     float x1 = 0.0, y1 = 0.0;
  79.     float x2 = 0.0, y2 = 0.0;
  80.     float x3 = 0.0, y3 = 0.0;
  81.     float z = 2.0;
  82.     float w = 0.0;
  83.     const float pi = 3.1415926;
  84.     geometry_msgs::PoseStamped pose0;
  85.     geometry_msgs::PoseStamped pose1;
  86.     geometry_msgs::PoseStamped pose2;
  87.     geometry_msgs::PoseStamped pose3;
  88.     pose0.pose.position.x = x0;
  89.     pose0.pose.position.y = y0;
  90.     pose0.pose.position.z = z;
  91.     pose1.pose.position.x = x1;
  92.     pose1.pose.position.y = y1;
  93.     pose1.pose.position.z = z;
  94.     pose2.pose.position.x = x2;
  95.     pose2.pose.position.y = y2;
  96.     pose2.pose.position.z = z;
  97.     pose3.pose.position.x = x3;
  98.     pose3.pose.position.y = y3;
  99.     pose3.pose.position.z = z;
  100.  
  101.     //uavs have the initial position offset because of the .launch config file
  102.     float x0_offset = -3;
  103.     float x1_offset = -1;
  104.     float x2_offset = 1;
  105.     float x3_offset = 3;
  106.  
  107.     //define uavs' coordinates in the formation coordinate system
  108.     float x0_f = -0.5, y0_f = 0.5;
  109.     float x1_f = 0.5, y1_f = 0.5;
  110.     float x2_f = -0.5, y2_f = -0.5;
  111.     float x3_f = 0.5, y3_f = -0.5;
  112.     float xx, yy;
  113.  
  114.     //send a few setpoints before starting
  115.     for(int i = 100; ros::ok() && i > 0; --i){
  116.         local_pos_pub0.publish(pose0);
  117.         local_pos_pub1.publish(pose1);
  118.         local_pos_pub2.publish(pose2);
  119.         local_pos_pub3.publish(pose3);
  120.         ros::spinOnce();
  121.         rate.sleep();
  122.     }
  123.  
  124.     mavros_msgs::SetMode offb_set_mode;
  125.     offb_set_mode.request.custom_mode = "OFFBOARD";
  126.  
  127.     mavros_msgs::CommandBool arm_cmd;
  128.     arm_cmd.request.value = true;
  129.  
  130.     ros::Time last_request = ros::Time::now();
  131.  
  132.     while(ros::ok()){
  133.         if( current_state.mode != "OFFBOARD" &&
  134.             (ros::Time::now() - last_request > ros::Duration(5.0))){
  135.                 
  136.             if( set_mode_client0.call(offb_set_mode) &&
  137.                 set_mode_client1.call(offb_set_mode) &&
  138.                 set_mode_client2.call(offb_set_mode) &&
  139.                 set_mode_client3.call(offb_set_mode) &&
  140.                 offb_set_mode.response.mode_sent){
  141.                 ROS_INFO("Offboard enabled : 4");
  142.             }
  143.             last_request = ros::Time::now();
  144.         } else {
  145.             if( !current_state.armed &&
  146.                 (ros::Time::now() - last_request > ros::Duration(5.0))){
  147.                 if( arming_client0.call(arm_cmd) &&
  148.                     arming_client1.call(arm_cmd) &&
  149.                     arming_client2.call(arm_cmd) &&
  150.                     arming_client3.call(arm_cmd) &&
  151.                     arm_cmd.response.success){
  152.                     ROS_INFO("Vehicle armed : 4");
  153.                     
  154.                 }
  155.                 last_request = ros::Time::now();
  156.             }
  157.         }
  158.  
  159.         local_pos_pub0.publish(pose0);
  160.         local_pos_pub1.publish(pose1);
  161.         local_pos_pub2.publish(pose2);
  162.         local_pos_pub3.publish(pose3);
  163.  
  164.         w = w + 2*pi/(30/(1/20.0));
  165.         if(w > 2*pi){
  166.             w = w - 2*pi;
  167.         }
  168.         xx = 4.75*cos(w);
  169.         yy = 4.75*sin(w);
  170.         
  171.         x0 = x0_f*cos(w) - y0_f*sin(w) + xx - x0_offset;
  172.         y0 = y0_f*cos(w) + x0_f*sin(w) + yy;
  173.         pose0.pose.position.x = x0;
  174.         pose0.pose.position.y = y0;
  175.  
  176.         x1 = x1_f*cos(w) - y1_f*sin(w) + xx - x1_offset;
  177.         y1 = y1_f*cos(w) + x1_f*sin(w) + yy;
  178.         pose1.pose.position.x = x1;
  179.         pose1.pose.position.y = y1;
  180.  
  181.         x2 = x2_f*cos(w) - y2_f*sin(w) + xx - x2_offset;
  182.         y2 = y2_f*cos(w) + x2_f*sin(w) + yy;
  183.         pose2.pose.position.x = x2;
  184.         pose2.pose.position.y = y2;
  185.  
  186.         x3 = x3_f*cos(w) - y3_f*sin(w) + xx - x3_offset;
  187.         y3 = y3_f*cos(w) + x3_f*sin(w) + yy;
  188.         pose3.pose.position.x = x3;
  189.         pose3.pose.position.y = y3;
  190.  
  191.         ros::spinOnce();
  192.         rate.sleep();
  193.     }
  194.  
  195.     return 0;
  196. }
  197.

然后运行即可。

常见问题：

1.rostopic list 的时候无人机不全：

检查第一步的launch文件，查看参数是否修改完成，可以参考我贴出来我的内容。

2.运行代码之后，飞机一直卡在 ：“Offboard enabled : 4"  没有进入arm状态

这种问题在单机飞行的时候也遇到了，猜测是因为仿真没有给出遥控器信号，然后PX4禁止解锁。

解决方法参考这篇文章：PX4 & gazebo仿真 offboard模式无法起飞解决思路 2023.5.19更新

我的解决方法如下：

1.打开QGC，此时一般正常链接四架飞机。

2.点击左上角图标，进入设置

3. 编辑COM_RCL_EXCEPT参数，设置为“Offboard”

此时问题解决

3.部分无人机不飞

实际上边的控制代码是存在缺陷的，因为在切换off board和 arm的时候只检查了uav0的状态。如果uav0正常起飞了，后续就不会在进入设置offboard 和 arm了，容易导致部分无人机在地上不动。

两个解决方法：

（1）修改源码：增加其他飞机状态的回调函数，在 if 判断判断语句的时候增加条件，确定所有无人机都被Off board或者 arm之后才会跳过这个过程。如果能读懂代码应该知道我在说什么。

（2）QGC切换状态：再QGC中，如果找到不飞的那架无人机，点击takeoff，滑动下方滑块

飞机起飞之后手动切换到off board模式（这是实际情况中常用的方法，使用遥控器切换）

结束语：

至此多机仿真结束，下一步计划实现多机的实物试飞 

声明：本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：【wpsshop博客】
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号