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

转载 原文链接：https://blog.youkuaiyun.com/weixin_43409270/article/details/114703341
多机仿真
1、修改launch文件
在/PX4-Autopilot/launch目录下，找到multi_uav_mavros_sitl.launch文件，添加无人机和配置通信端口
可以直接复制文件中已有的配置文件，比如下面照片中是multi_uav_mavros_sitl.launch文件中已有的UAV0无人机的配置代码，我们可以直接复制，然后修改fcu_url, mavlink_udp_port和mavlink_tcp_port（在uav0的基础上加ID号即可）

比如，我们要添加第四架无人机（uav3），则udp端口为14540+3=14543：

<!-- UAV3 -->
    <group ns="uav3">
        <!-- MAVROS and vehicle configs -->
        <arg name="ID" value="3"/>
        <arg name="fcu_url" default="udp://:14543@localhost:14553"/>
        <!-- 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>

最多只能有10架无人机。
注意仿真的坐标系有点乱，gazebo中有一个坐标系，在上面配置文件中的 的坐标（x，y，z）是设置无人机在gazebo中初始生成的位置，控制代码offboard中发布的每架无人机的pose的坐标系的原点默认就是每架无人机的初始位置（x，y，z），所以多机控制时坐标要转化一下。
2、修改offboard代码
每架无人机都需要设置ros接受发送消息，通过mavros前添加前缀，例如/uav0/mavros/…具体如下：

//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");

同理下面Offboard enabled和Vehicle armed也都要分别设置：

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)

arming_client0.call(arm_cmd) &&
arming_client1.call(arm_cmd) &&
arming_client2.call(arm_cmd) &&
arming_client3.call(arm_cmd)

3、启动仿真
首先启动gazebo模型：

cd PX4-Autopilot
git submodule update --init --recursive//执行子模块更新指令
DONT_RUN=1 make px4_sitl_default gazebo
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

其中4，5行的source和export在每次开新终端launch前都需要输入

然后设置mavros的udp：

roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14550"

注意：roslaunch mavros px4.launch fcu_url:=/dev/ttyTHS2:921600
的作用在于将飞控连接上mavros

其后面的连接到底说什么呢，这其实是ros的重映射功能，用命令行中的参数替换launch文件中的参数。
fcu_url 接的是对应的串口的文件，就是比如USB在linux里是一个文件， fcu_url后面都是等于/dev/下面的一个文件。应该是你哪个口连的pixhawk，就写哪个口，这应该是指定接口用的。
最后运行offboard节点：rosrun offboard offboard_node
下面附上自己的4架无人机画圆的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;
}