基于gazebo和mavros实现两台无人机的位姿跟踪实验

HELLO JQ

已于 2024-08-13 15:08:56 修改

阅读量1.7k

点赞数 39

文章标签：无人机

于 2024-08-13 11:15:55 首次发布

本文链接：https://blog.csdn.net/Boorki/article/details/141158618

版权

在gazebo仿真环境中，使用mavros提供的各个话题与服务，实现1号无人机跟踪0号无人机位置与方向的功能。

文章目录

1. 实现流程
2. ROS程序
3. 演示视频
- 3.1. uav0跟踪uav1的位置
- 3.2. uav0跟踪uav1的位置和姿态
4. 问题总结

1. 实现流程

1.1. 程序框架及调用关系

程序主要涉及两部分：

multi_uav_mavros_sitl.launch： px4官方提供的多机launch文件，主要功能是启动gazebo仿真环境、启动PX4节点、启动mavros节点，其中mavros节点的实质是启动了uas节点和router节点，实现翻译与收发的功能。
用户自定义算法： 由无人机0号的uav0_node.cpp、uav0_control.cpp，无人机1号的uav1_node.cpp、follower.cpp共四个文件组成，详细代码见2. ROS程序。

1.2. 修改launch文件

修改命名空间： group内部产生的话题和参数，都在‘uavx’的命名空间内，例如‘/uav0_mavros/global_position’，因此不同无人机的同名话题不会冲突。
增加 ID： 每个无人机有唯一的ID，ID通常传递给一些特定的参数，如MAVLink目标系统ID (tgt_system)等。
修改通信端口： 包括fcu_url、mavlink_udp_port、mavlink_tcp_port 的端口号的修改，起始端口号在 ‘/PX4_Firmware/ROMFS/px4fmu_common/init.d-posix/rcS’ 文件中有设置，不能随便选取，端口号会从起始端口号开始，根据ID进行偏移，例如mavlink_udp_port为18570+ID。

1.3. 两台无人机的通信与控制逻辑

对于uav0：setpoint_position/local由键盘输入的目标值决定
对于uav1：setpoint_position/local由uav0在gazebo世界的位姿决定

2. ROS程序

核心程序共四个文件：无人机0号的uav0_node.cpp、uav0_control.cpp，无人机1号的uav1_node.cpp、uav1_follower.cpp。

uav0_node.cpp： 将0号无人机切换到offboard并解锁，订阅位置和姿态的指令。
uav0_control.cpp： 通过键盘输入目标位置和姿态，并发布。
uav1_node.cpp： 将1号无人机切换到offboard并解锁，订阅位置和姿态的指令。
uav1_follower.cpp： 订阅0号机的实时位置和姿态，发布给1号机。

2.1. uav0_node.cpp

作用：将0号无人机切换到offboard并解锁，订阅位置和姿态的指令。

#include <ros/ros.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <geometry_msgs/PoseStamped.h>

mavros_msgs::State current_state_uav0;
geometry_msgs::PoseStamped pose_uav0_global;
geometry_msgs::PoseStamped pose_uav0_local;

double map_x = 0.0;
double map_y = 0.0;
double map_z = 0.32;

void state_cb_uav0(const mavros_msgs::State::ConstPtr& msg){
    current_state_uav0 = *msg;
}

void pose_update_cb(const geometry_msgs::PoseStamped::ConstPtr& msg) {
    pose_uav0_global = *msg;
    pose_uav0_local.pose.position.x = pose_uav0_global.pose.position.x - map_x;
    pose_uav0_local.pose.position.y = pose_uav0_global.pose.position.y - map_y;
    pose_uav0_local.pose.position.z = pose_uav0_global.pose.position.z - map_z;

    pose_uav0_local.pose.orientation.x = pose_uav0_global.pose.orientation.x;
    pose_uav0_local.pose.orientation.y = pose_uav0_global.pose.orientation.y;
    pose_uav0_local.pose.orientation.z = pose_uav0_global.pose.orientation.z;
    pose_uav0_local.pose.orientation.w = pose_uav0_global.pose.orientation.w;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "uav0_node_all");
    ros::NodeHandle nh;

    ros::Subscriber state_sub_uav0 = nh.subscribe<mavros_msgs::State>
            ("uav0/mavros/state", 10, state_cb_uav0);
    ros::Publisher local_pos_pub_uav0 = nh.advertise<geometry_msgs::PoseStamped>
            ("uav0/mavros/setpoint_position/local", 10);
    ros::Subscriber pose_sub = nh.subscribe<geometry_msgs::PoseStamped>
            ("uav0/target_position", 10, pose_update_cb);
    ros::ServiceClient arming_client_uav0 = nh.serviceClient<mavros_msgs::CommandBool>
            ("uav0/mavros/cmd/arming");
    ros::ServiceClient set_mode_client_uav0 = nh.serviceClient<mavros_msgs::SetMode>
            ("uav0/mavros/set_mode");

    ros::Rate rate(20.0);

    // 等待飞控连接
    while (ros::ok() && (!current_state_uav0.connected)) {
        ros::spinOnce();
        rate.sleep();
    }

    pose_uav0_local.pose.position.x = 0;
    pose_uav0_local.pose.position.y = 0;
    pose_uav0_local.pose.position.z = 1;
    pose_uav0_local.pose.orientation.w = 0;
    pose_uav0_local.pose.orientation.x = 0; 
    pose_uav0_local.pose.orientation.y = 0;
    pose_uav0_local.pose.orientation.z = 0;

    for (int i = 100; ros::ok() && i > 0; --i) {
        local_pos_pub_uav0.publish(pose_uav0_local);
        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_uav0 = ros::Time::now();

    while (ros::ok()) {
        // 检查并设置uav0的OFFBOARD模式和解锁状态
        if (current_state_uav0.mode != "OFFBOARD" &&
            (ros::Time::now() - last_request_uav0 > ros::Duration(1.0))) {
            if (set_mode_client_uav0.call(offb_set_mode) &&
                offb_set_mode.response.mode_sent) {
                ROS_INFO("Offboard enabled for uav0");
            }
            last_request_uav0 = ros::Time::now();
        } else {
            if (!current_state_uav0.armed &&
                (ros::Time::now() - last_request_uav0 > ros::Duration(1.0))) {
                if (arming_client_uav0.call(arm_cmd) &&
                    arm_cmd.response.success) {
                    ROS_INFO("uav0 armed");
                }
                last_request_uav0 = ros::Time::now();
            }
        }
        //将订阅到的目标，发布给mavros的setpoint_position/local话题
        local_pos_pub_uav0.publish(pose_uav0_local);
        ros::spinOnce();
        rate.sleep();        
    }

    return 0;
}

2.2. uav0_control.cpp

作用：通过键盘输入目标位置和姿态，并发布。

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <iostream>
#include <thread>
#include <mutex>
#include <cmath>

std::mutex mtx;
double x = 0.0, y = 0.0, z = 0.0;
double rotation_degrees = 0.0;
bool new_input = false;

void keyboardInputThread() {
    while (ros::ok()) {
        std::cout << "Enter the target position and rotation angle: " << std::endl;
        std::cout.flush();
        // 写数据时加锁
        std::lock_guard<std::mutex> lock(mtx);
        std::cin >> x >> y >> z >> rotation_degrees;        
        new_input = true;
    }
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "uav0_control_all");
    ros::NodeHandle nh;
    ros::Publisher pose_pub = nh.advertise<geometry_msgs::PoseStamped>("uav0/target_position", 10);
    std::thread input_thread(keyboardInputThread);// 键盘输入线程
    ros::Rate rate(10.0);  // 更新频率
    geometry_msgs::PoseStamped new_pose;
    while (ros::ok()) {
        {
            std::lock_guard<std::mutex> lock(mtx);
            if (new_input) {
                new_pose.pose.position.x = x;
                new_pose.pose.position.y = y;
                new_pose.pose.position.z = z;

                // 计算四元数
                double radians = rotation_degrees * M_PI / 180.0;
                double half_angle = radians / 2.0;
                double sin_half_angle = sin(half_angle);
                double cos_half_angle = cos(half_angle);

                new_pose.pose.orientation.x = 0.0;
                new_pose.pose.orientation.y = 0.0;
                new_pose.pose.orientation.z = -sin_half_angle;
                new_pose.pose.orientation.w = cos_half_angle;

                new_input = false;
            }
        }

        // 发布新目标位置
        pose_pub.publish(new_pose);
        ros::spinOnce();
        rate.sleep();
    }

    input_thread.join();

    return 0;
}

2.3. uav1_node.cpp

将1号无人机切换到offboard并解锁，订阅位置和姿态的指令。（代码与0号无人机的大同小异）

#include <ros/ros.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <geometry_msgs/PoseStamped.h>

mavros_msgs::State current_state_uav1;
geometry_msgs::PoseStamped pose_uav1_global;
geometry_msgs::PoseStamped pose_uav1_local;

double map_x = 1.0;
double map_y = 0.0;
double map_z = 0.32;

void state_cb_uav1(const mavros_msgs::State::ConstPtr& msg){
    current_state_uav1 = *msg;
}

void pose_update_cb(const geometry_msgs::PoseStamped::ConstPtr& msg) {
    pose_uav1_global = *msg;
    pose_uav1_local.pose.position.x = pose_uav1_global.pose.position.x - map_x;
    pose_uav1_local.pose.position.y = pose_uav1_global.pose.position.y - map_y;
    pose_uav1_local.pose.position.z = pose_uav1_global.pose.position.z - map_z;

    pose_uav1_local.pose.orientation.x = pose_uav1_global.pose.orientation.x;
    pose_uav1_local.pose.orientation.y = pose_uav1_global.pose.orientation.y;
    pose_uav1_local.pose.orientation.z = pose_uav1_global.pose.orientation.z;
    pose_uav1_local.pose.orientation.w = pose_uav1_global.pose.orientation.w;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "uav1_node_all");
    ros::NodeHandle nh;

    ros::Subscriber state_sub_uav1 = nh.subscribe<mavros_msgs::State>
            ("uav1/mavros/state", 10, state_cb_uav1);
    ros::Publisher local_pos_pub_uav1 = nh.advertise<geometry_msgs::PoseStamped>
            ("uav1/mavros/setpoint_position/local", 10);
    ros::Subscriber pose_sub = nh.subscribe<geometry_msgs::PoseStamped>
            ("uav1/target_position", 10, pose_update_cb);
    ros::ServiceClient arming_client_uav1 = nh.serviceClient<mavros_msgs::CommandBool>
            ("uav1/mavros/cmd/arming");
    ros::ServiceClient set_mode_client_uav1 = nh.serviceClient<mavros_msgs::SetMode>
            ("uav1/mavros/set_mode");


    ros::Rate rate(20.0);

    // 等待飞控连接
    while (ros::ok() && (!current_state_uav1.connected)) {
        ros::spinOnce();
        rate.sleep();
    }

    pose_uav1_local.pose.position.x = 0;
    pose_uav1_local.pose.position.y = 0;
    pose_uav1_local.pose.position.z = 1;
    pose_uav1_local.pose.orientation.w = 0;
    pose_uav1_local.pose.orientation.x = 0; 
    pose_uav1_local.pose.orientation.y = 0;
    pose_uav1_local.pose.orientation.z = 0;

    for (int i = 100; ros::ok() && i > 0; --i) {
        local_pos_pub_uav1.publish(pose_uav1_local);
        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_uav1 = ros::Time::now();

    while (ros::ok()) {
        // 检查并设置uav1的OFFBOARD模式和解锁状态
        if (current_state_uav1.mode != "OFFBOARD" &&
            (ros::Time::now() - last_request_uav1 > ros::Duration(1.0))) {
            if (set_mode_client_uav1.call(offb_set_mode) &&
                offb_set_mode.response.mode_sent) {
                ROS_INFO("Offboard enabled for uav1");
            }
            last_request_uav1 = ros::Time::now();
        } else {
            if (!current_state_uav1.armed &&
                (ros::Time::now() - last_request_uav1 > ros::Duration(1.0))) {
                if (arming_client_uav1.call(arm_cmd) &&
                    arm_cmd.response.success) {
                    ROS_INFO("uav1 armed");
                }
                last_request_uav1 = ros::Time::now();
            }
        }

        local_pos_pub_uav1.publish(pose_uav1_local);
        ros::spinOnce();
        rate.sleep();        
    }

    return 0;
}

2.4. uav1_follower.cpp

订阅0号机的实时位置和姿态，发布给1号机。

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>

ros::Publisher uav1_target_pub;

void uav0PoseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
    // 设置uav1的目标坐标，使其跟随在uav0的x轴前1米的方向
    geometry_msgs::PoseStamped target_pose;
    target_pose.pose.position.x = msg->pose.position.x + 1.0;  // 跟随uav0的位置，并在x轴上前1米
    target_pose.pose.position.y = msg->pose.position.y;
    target_pose.pose.position.z = msg->pose.position.z;
    target_pose.pose.orientation = msg->pose.orientation;  // 跟随uav0的姿态
    uav1_target_pub.publish(target_pose); // 发布uav1的目标位置
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "follower_all");
    ros::NodeHandle nh;
    // 订阅uav0的groundtruth/pose话题
    ros::Subscriber uav0_pose_sub = nh.subscribe("/uav0/mavros/groundtruth/pose", 10, uav0PoseCallback);

    // 发布uav1的目标位置
    uav1_target_pub = nh.advertise<geometry_msgs::PoseStamped>("/uav1/target_position", 10);

    ros::spin();
    return 0;
}

3. 演示视频

3.1. uav0跟踪uav1的位置

将QGC的飞行日志导出到PlotJuggler软件进行分析：
在这里插入图片描述
绿色线为uav0，紫色线为uav1，三幅图代表无人机的x轴、y轴、z轴坐标。

3.2. uav0跟踪uav1的位置和姿态

将QGC的飞行日志导出到PlotJuggler软件进行分析：
在这里插入图片描述绿色线为uav0，紫色线为uav1，四幅图代表四元数的四个分量。
q.00 (q_0): 四元数的实部。
q.01 (q_1): 四元数的虚部第一个分量。
q.02 (q_2): 四元数的虚部第二个分量。
q.03 (q_3): 四元数的虚部第三个分量。

4. 问题总结

4.1. 通信端口修改出错：实际端口 = 起始端口 + 偏移量

问题：在修改fcu_url的时候，对于ID为1的飞机，在launch文件中配置"udp://:24542@localhost:34582"时通信错误。在px4启动时，px4监听的却是24541。
原因：launch文件只是mavros端的配置，而px4端的配置是实际端口 = 起始端口 + 偏移量，也就是24541(正确端口号) = 24540(rcs规定的起始端口号) + 1(偏移量，下图的px4_instance，由ID号赋值)。
解决方法：确保mavlink_udp_port、mavlink_tcp_port 和 fcu_url 中的端口号与ID正确对应。

4.2 两台无人机的坐标系不一致：使用gazebo真值

问题：在gazebo世界中设置的两台无人机起始位置分别是（0，0，0）和（1，0，0），但是打印/local_position/pose话题数据，发现两台无人机的坐标都是（0，0，0）。
原因：/mavros/local_position/pose发布的数据，是以GPS上电时刻为原点而建立的NED坐标系的位置数据和四元数数据，因此不同的无人机的坐标系不统一。
解决方法：写一个节点获取Gazebo位姿真值，并将这些信息发布到对应的ROS话题中，将两台无人机的局部位置统一在一个坐标系下。

4.3 无人机一段时间后停止转动：offboard模式需要持续发送

问题：在Offboard模式下没有持续发送set_point_position消息，无人机在一段时间后停止转动。
原因：如果PX4在500ms内没有收到新的控制指令，飞行控制器可能会认为外部控制器已经失去联系或停止发送指令，飞行控制器会触发Failsafe机制，将返回到飞机进入Offboard模式之前的最后模式。
解决方法：以20Hz的频率一直发送set_point_position消息，遇到新的目标就进行更新（没有新目标也坚持发送）。