px4与gazebo的多无人机编队仿真学习

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号即可）
开启多机

2、多机测试代码

/**
 * @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;
}