菜鸟追梦旅行

这个屌丝很懒，什么也没留下！

热门标签

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”文件，这里需要对里边的内容修改。

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


<?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="vehicle" default="iris"/>
    <arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/empty.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>
    <!-- UAV0 -->
    <group ns="uav0">
        <!-- MAVROS and vehicle configs -->
        <arg name="ID" value="0"/>
        <arg name="fcu_url" default="udp://:14540@localhost:14580"/>
        <!-- PX4 SITL and vehicle spawn -->
        <include file="$(find px4)/launch/single_vehicle_spawn.launch">
            <arg name="x" value="-3"/>
            <arg name="y" value="0"/>
            <arg name="z" value="0"/>
            <arg name="R" value="0"/>
            <arg name="P" value="0"/>
            <arg name="Y" value="0"/>
            <arg name="vehicle" value="$(arg vehicle)"/>
            <arg name="mavlink_udp_port" value="14560"/>
            <arg name="mavlink_tcp_port" value="4560"/>
            <arg name="ID" value="$(arg ID)"/>
            <arg name="gst_udp_port" value="$(eval 5600 + arg('ID'))"/>
            <arg name="video_uri" value="$(eval 5600 + arg('ID'))"/>
            <arg name="mavlink_cam_udp_port" value="$(eval 14530 + arg('ID'))"/>
        </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>
    <!-- UAV1 -->
    <group ns="uav1">
        <!-- MAVROS and vehicle configs -->
        <arg name="ID" value="1"/>
        <arg name="fcu_url" default="udp://:14541@localhost:14581"/>
        <!-- PX4 SITL and vehicle spawn -->
        <include file="$(find px4)/launch/single_vehicle_spawn.launch">
            <arg name="x" value="-1"/>
            <arg name="y" value="0"/>
            <arg name="z" value="0"/>
            <arg name="R" value="0"/>
            <arg name="P" value="0"/>
            <arg name="Y" value="0"/>
            <arg name="vehicle" value="$(arg vehicle)"/>
            <arg name="mavlink_udp_port" value="14561"/>
            <arg name="mavlink_tcp_port" value="4561"/>
            <arg name="ID" value="$(arg ID)"/>
            <arg name="gst_udp_port" value="$(eval 5600 + arg('ID'))"/>
            <arg name="video_uri" value="$(eval 5600 + arg('ID'))"/>
            <arg name="mavlink_cam_udp_port" value="$(eval 14530 + arg('ID'))"/>
        </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>
    <!-- UAV2 -->
    <group ns="uav2">
        <!-- MAVROS and vehicle configs -->
        <arg name="ID" value="2"/>
        <arg name="fcu_url" default="udp://:14542@localhost:14582"/>
        <!-- PX4 SITL and vehicle spawn -->
        <include file="$(find px4)/launch/single_vehicle_spawn.launch">
            <arg name="x" value="1"/>
            <arg name="y" value="0"/>
            <arg name="z" value="0"/>
            <arg name="R" value="0"/>
            <arg name="P" value="0"/>
            <arg name="Y" value="0"/>
            <arg name="vehicle" value="$(arg vehicle)"/>
            <arg name="mavlink_udp_port" value="14562"/>
            <arg name="mavlink_tcp_port" value="4562"/>
            <arg name="ID" value="$(arg ID)"/>
            <arg name="gst_udp_port" value="$(eval 5600 + arg('ID'))"/>
            <arg name="video_uri" value="$(eval 5600 + arg('ID'))"/>
            <arg name="mavlink_cam_udp_port" value="$(eval 14530 + arg('ID'))"/>
        </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>
    <group ns="uav3">
        <!-- MAVROS and vehicle configs -->
        <arg name="ID" value="3"/>
        <arg name="fcu_url" default="udp://:14542@localhost:14583"/>
        <!-- PX4 SITL and vehicle spawn -->
        <include file="$(find px4)/launch/single_vehicle_spawn.launch">
            <arg name="x" value="3"/>
            <arg name="y" value="0"/>
            <arg name="z" value="0"/>
            <arg name="R" value="0"/>
            <arg name="P" value="0"/>
            <arg name="Y" value="0"/>
            <arg name="vehicle" value="$(arg vehicle)"/>
            <arg name="mavlink_udp_port" value="14563"/>
            <arg name="mavlink_tcp_port" value="4563"/>
            <arg name="ID" value="$(arg ID)"/>
            <arg name="gst_udp_port" value="$(eval 5600 + arg('ID'))"/>
            <arg name="video_uri" value="$(eval 5600 + arg('ID'))"/>
            <arg name="mavlink_cam_udp_port" value="$(eval 14530 + arg('ID'))"/>
        </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>
<!-- to add more UAVs (up to 10):
Increase the id
Change the name space
Set the FCU to default="udp://:14540+id@localhost:14550+id"
Set the malink_udp_port to 14560+id) -->

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


<!-- UAV0 -->
    <group ns="uav0">
        <!-- MAVROS and vehicle configs -->
        <arg name="ID" value="0"/>  //这个是无人机的id，可以根据自己增加的数量修改
        <arg name="fcu_url" default="udp://:14540@localhost:14580"/> //这个是和mavros连接的时候的地址 需要根据自己的id修改，比如uav1的话就是："udp://:14541@localhost:14581"
        <!-- PX4 SITL and vehicle spawn -->
        <include file="$(find px4)/launch/single_vehicle_spawn.launch"> //这里是调用spawn.launch 我不太懂这句话的意思，希望有大佬指点一下。
            <arg name="x" value="-3"/>  //下边 6个变量 代表无人机起始位置的位置和姿态
            <arg name="y" value="0"/>
            <arg name="z" value="0"/>
            <arg name="R" value="0"/>
            <arg name="P" value="0"/>
            <arg name="Y" value="0"/>
            <arg name="vehicle" value="$(arg vehicle)"/>
            <arg name="mavlink_udp_port" value="14560"/> //这里是udp端口，需要修改最后一位
            <arg name="mavlink_tcp_port" value="4560"/>  //这里是tcp端口，需要修改最后一位
            <arg name="ID" value="$(arg ID)"/> //获取id，后边的都不用改了，自动加id了
            <arg name="gst_udp_port" value="$(eval 5600 + arg('ID'))"/>
            <arg name="video_uri" value="$(eval 5600 + arg('ID'))"/>
            <arg name="mavlink_cam_udp_port" value="$(eval 14530 + arg('ID'))"/>
        </include>
        <!-- MAVROS --> //下边的内容是启动mavros 和自己运行 roslaunch mavros px4.launch fcu_url:="xxxx"类似，不赘述
        <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文件，官方提示最多仿真10架。

2.运行仿真

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


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

Source 环境（这一步需要大家自己看一下文件目录里的位置，每个版本可能存在一定的差异，更换到自己路径即可）
```
source Tools/setup_gazebo.bash $(pwd) $(pwd)/build/px4_sitl_default
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd):$(pwd)/Tools/sitl_gazebo
```

运行启动文件:

roslaunch px4 multi_uav_mavros_sitl.launch

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

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

rostopic list

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

3.offboard控制

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

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

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


/**
 * @file offb_node.cpp
 * @brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro Flight
 * Stack and tested in Gazebo SITL
 */
 
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
 
mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}
 
int main(int argc, char **argv)
{
    ros::init(argc, argv, "offb_node");
 
    //uav0
    ros::NodeHandle nh0;
    ros::Subscriber state_sub0 = nh0.subscribe<mavros_msgs::State>
            ("uav0/mavros/state", 10, state_cb);
    ros::Publisher local_pos_pub0 = nh0.advertise<geometry_msgs::PoseStamped>
            ("/uav0/mavros/setpoint_position/local", 10);
    ros::ServiceClient arming_client0 = nh0.serviceClient<mavros_msgs::CommandBool>
            ("/uav0/mavros/cmd/arming");
    ros::ServiceClient set_mode_client0 = nh0.serviceClient<mavros_msgs::SetMode>
            ("/uav0/mavros/set_mode");
 
    //uav1
    ros::NodeHandle nh1;
    ros::Subscriber state_sub1 = nh1.subscribe<mavros_msgs::State>
            ("uav1/mavros/state", 10, state_cb);
    ros::Publisher local_pos_pub1 = nh1.advertise<geometry_msgs::PoseStamped>
            ("/uav1/mavros/setpoint_position/local", 10);
    ros::ServiceClient arming_client1 = nh1.serviceClient<mavros_msgs::CommandBool>
            ("/uav1/mavros/cmd/arming");
    ros::ServiceClient set_mode_client1 = nh1.serviceClient<mavros_msgs::SetMode>
            ("/uav1/mavros/set_mode");
 
    //uav2
    ros::NodeHandle nh2;
    ros::Subscriber state_sub2 = nh2.subscribe<mavros_msgs::State>
            ("uav2/mavros/state", 10, state_cb);
    ros::Publisher local_pos_pub2 = nh2.advertise<geometry_msgs::PoseStamped>
            ("/uav2/mavros/setpoint_position/local", 10);
    ros::ServiceClient arming_client2 = nh2.serviceClient<mavros_msgs::CommandBool>
            ("/uav2/mavros/cmd/arming");
    ros::ServiceClient set_mode_client2 = nh2.serviceClient<mavros_msgs::SetMode>
            ("/uav2/mavros/set_mode");
 
    //uav3
    ros::NodeHandle nh3;
    ros::Subscriber state_sub3 = nh3.subscribe<mavros_msgs::State>
            ("uav3/mavros/state", 10, state_cb);
    ros::Publisher local_pos_pub3 = nh3.advertise<geometry_msgs::PoseStamped>
            ("/uav3/mavros/setpoint_position/local", 10);
    ros::ServiceClient arming_client3 = nh3.serviceClient<mavros_msgs::CommandBool>
            ("/uav3/mavros/cmd/arming");
    ros::ServiceClient set_mode_client3 = nh3.serviceClient<mavros_msgs::SetMode>
            ("/uav3/mavros/set_mode");
 
    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(20.0);
 
    // wait for FCU connection
    while(ros::ok() && !current_state.connected){
        ros::spinOnce();
        rate.sleep();
    }
    // x_num, y_num  refer to the desired uav input position
    //all the uavs have the same height : z
 
    float x0 = 0.0, y0 = 0.0;
    float x1 = 0.0, y1 = 0.0;
    float x2 = 0.0, y2 = 0.0;
    float x3 = 0.0, y3 = 0.0;
    float z = 2.0;
    float w = 0.0;
    const float pi = 3.1415926;
    geometry_msgs::PoseStamped pose0;
    geometry_msgs::PoseStamped pose1;
    geometry_msgs::PoseStamped pose2;
    geometry_msgs::PoseStamped pose3;
    pose0.pose.position.x = x0;
    pose0.pose.position.y = y0;
    pose0.pose.position.z = z;
    pose1.pose.position.x = x1;
    pose1.pose.position.y = y1;
    pose1.pose.position.z = z;
    pose2.pose.position.x = x2;
    pose2.pose.position.y = y2;
    pose2.pose.position.z = z;
    pose3.pose.position.x = x3;
    pose3.pose.position.y = y3;
    pose3.pose.position.z = z;
 
    //uavs have the initial position offset because of the .launch config file
    float x0_offset = -3;
    float x1_offset = -1;
    float x2_offset = 1;
    float x3_offset = 3;
 
    //define uavs' coordinates in the formation coordinate system
    float x0_f = -0.5, y0_f = 0.5;
    float x1_f = 0.5, y1_f = 0.5;
    float x2_f = -0.5, y2_f = -0.5;
    float x3_f = 0.5, y3_f = -0.5;
    float xx, yy;
 
    //send a few setpoints before starting
    for(int i = 100; ros::ok() && i > 0; --i){
        local_pos_pub0.publish(pose0);
        local_pos_pub1.publish(pose1);
        local_pos_pub2.publish(pose2);
        local_pos_pub3.publish(pose3);
        ros::spinOnce();
        rate.sleep();
    }
 
    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";
 
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;
 
    ros::Time last_request = ros::Time::now();
 
    while(ros::ok()){
        if( current_state.mode != "OFFBOARD" &&
            (ros::Time::now() - last_request > ros::Duration(5.0))){
                
            if( set_mode_client0.call(offb_set_mode) &&
                set_mode_client1.call(offb_set_mode) &&
                set_mode_client2.call(offb_set_mode) &&
                set_mode_client3.call(offb_set_mode) &&
                offb_set_mode.response.mode_sent){
                ROS_INFO("Offboard enabled : 4");
            }
            last_request = ros::Time::now();
        } else {
            if( !current_state.armed &&
                (ros::Time::now() - last_request > ros::Duration(5.0))){
                if( arming_client0.call(arm_cmd) &&
                    arming_client1.call(arm_cmd) &&
                    arming_client2.call(arm_cmd) &&
                    arming_client3.call(arm_cmd) &&
                    arm_cmd.response.success){
                    ROS_INFO("Vehicle armed : 4");
                    
                }
                last_request = ros::Time::now();
            }
        }
 
        local_pos_pub0.publish(pose0);
        local_pos_pub1.publish(pose1);
        local_pos_pub2.publish(pose2);
        local_pos_pub3.publish(pose3);
 
        w = w + 2*pi/(30/(1/20.0));
        if(w > 2*pi){
            w = w - 2*pi;
        }
        xx = 4.75*cos(w);
        yy = 4.75*sin(w);
        
        x0 = x0_f*cos(w) - y0_f*sin(w) + xx - x0_offset;
        y0 = y0_f*cos(w) + x0_f*sin(w) + yy;
        pose0.pose.position.x = x0;
        pose0.pose.position.y = y0;
 
        x1 = x1_f*cos(w) - y1_f*sin(w) + xx - x1_offset;
        y1 = y1_f*cos(w) + x1_f*sin(w) + yy;
        pose1.pose.position.x = x1;
        pose1.pose.position.y = y1;
 
        x2 = x2_f*cos(w) - y2_f*sin(w) + xx - x2_offset;
        y2 = y2_f*cos(w) + x2_f*sin(w) + yy;
        pose2.pose.position.x = x2;
        pose2.pose.position.y = y2;
 
        x3 = x3_f*cos(w) - y3_f*sin(w) + xx - x3_offset;
        y3 = y3_f*cos(w) + x3_f*sin(w) + yy;
        pose3.pose.position.x = x3;
        pose3.pose.position.y = y3;
 
        ros::spinOnce();
        rate.sleep();
    }
 
    return 0;
}

然后运行即可。

常见问题：

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博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/菜鸟追梦旅行/article/detail/688612