超维小课堂 | 7、ROS使用offboard模式控制无人机定点悬停源码分析

引言：ROS使用offboard模式控制无人机进入定点悬停是学习ROS无人机控制的最经典的基本功能之一。基于此，本篇主要对此处的控制流程著一个简要的代码分析。（室内外通用代码）

顾名思义：offboard模式下的定点悬停是指通过机载电脑ROS发布指定的目标点，无人机按照程序发布的目标位置进行PID控制，到达后目标点后保持悬停不动。源码分析可以查看代码注释即可。

此处为视频详解：

源码视频详解：

特别注意：由于气压计以及GPS等定位存在漂移，会导致无人机在飞行过程中偏离程序设置的目标点。基于此，我们的代码已经对初始化漂移做了抵消，相对稳定，此处也是本此代码讲解的着重点。请确保使用我们的程序进行测试，否则可能造成无人机的失控

//包含ROS和MAVROS相关头文件 
#include <string> 
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <std_msgs/Bool.h>
#include <geometry_msgs/TwistStamped.h>
#include <mavros_msgs/PositionTarget.h>
#include <cmath>
#include <tf/transform_listener.h>
#include <nav_msgs/Odometry.h>
#include <mavros_msgs/CommandLong.h>   
#include <string>
#include <geometry_msgs/Twist.h>

//室外宏定义飞行高度2米，室内可适当降低高度
#define ALTITUDE  2

mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg);
void state_cb(const mavros_msgs::State::ConstPtr& msg)
{
    current_state = *msg;
}

//定义变量，用于接收无人机的里程计和姿态信息
tf::Quaternion quat; 
double roll, pitch, yaw;
float init_position_x_take_off =0;
float init_position_y_take_off =0;
float init_position_z_take_off =0;
bool  flag_init_position = false;
nav_msgs::Odometry local_pos;
void local_pos_cb(const nav_msgs::Odometry::ConstPtr& msg);
//回调函数接收无人机的里程计和姿态信息
void local_pos_cb(const nav_msgs::Odometry::ConstPtr& msg)
{
    local_pos = *msg;
    //程序启动后获取一次最新的GPS和气压计漂移数据作为初始值
    if (flag_init_position==false && (local_pos.pose.pose.position.z!=0))
    {
        init_position_x_take_off = local_pos.pose.pose.position.x;
        init_position_y_take_off = local_pos.pose.pose.position.y;
        init_position_z_take_off = local_pos.pose.pose.position.z;
        flag_init_position = true;          
    }
    //四元数转为欧拉角，后续程序使用
    tf::quaternionMsgToTF(local_pos.pose.pose.orientation, quat);   
    tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
}

int main(int argc, char **argv)
{
    //初始化节点，ROSAPI接口
    ros::init(argc, argv, "offboard_single_position");
    
    //创建nh句柄，ROSAPI接口
    ros::NodeHandle nh;
  
    //创建订阅者，用于订阅无人机的当前飞行状态等信息
    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("mavros/state", 10, state_cb);

    //创建发布者，发布无人机的期望位置
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 10);
    
    //创建订阅者，订阅无人机的实时位置信息
    ros::Subscriber local_pos_sub = nh.subscribe<nav_msgs::Odometry>("/mavros/local_position/odom", 10, local_pos_cb);

    //创建无人机解锁客户端，用于想飞控请求解锁命令
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
 
    //创建模式切换客户端，用于ROS程序想底层飞控请求进入offbaord或者其他模式
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");

    //offboard模式下，需要保持2Hz以上频率的心跳包，此处设置为20Hz，可适当调整
    ros::Rate rate(20.0);

    //等待连接飞控，连接后再执行后续部分
    while(ros::ok() && !current_state.connected)
    {
        ros::spinOnce();
        rate.sleep();
    }

    //设置预发布位置，有了预发布位置，才能切入到offboard模式
    geometry_msgs::PoseStamped pose;
    pose.pose.position.x =init_position_x_take_off + 0;
    pose.pose.position.y =init_position_y_take_off + 0;
    pose.pose.position.z =init_position_z_take_off + ALTITUDE;
     
    //发布者发布期望位置
    for(int i = 100; ros::ok() && i > 0; --i)
    {
        local_pos_pub.publish(pose);
        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;

    //获取系统当前时间给变量last_request
    ros::Time last_request = ros::Time::now();
 
    //此处满足一次请求进入offboard模式即可，官方例成循环切入offboard会导致无人机无法使用遥控器控制
    while(ros::ok())
    {
        //请求进入OFFBOARD模式，每隔5秒请求一次
        if( current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0)))
        {
            if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent)
            {
                ROS_INFO("Offboard enabled");
            }
            last_request = ros::Time::now();
            flag_init_position = false;         
        }
        else 
        {
            //请求解锁，每隔5秒请求一次
            if( !current_state.armed && (ros::Time::now() - last_request > ros::Duration(5.0)))
            {
                if( arming_client.call(arm_cmd) && arm_cmd.response.success)
                {
                    ROS_INFO("Vehicle armed");
                }
                last_request = ros::Time::now();
                flag_init_position = false;         
            }
        }
        
        //1、添加高度判断，使得无人机跳出模式切换循环，误差小于0.5米即认为已经达到
        if(fabs(local_pos.pose.pose.position.z- init_position_z_take_off -ALTITUDE)<0.5)
        {   
            //延时三秒后推出循环
            if(ros::Time::now() - last_request > ros::Duration(3.0))
            {
                break;
            }
        }
        //2、添加时间判断，使得无人机跳出模式切换循环
        if(ros::Time::now() - last_request > ros::Duration(8.0))
        {
            break;
        }
        //此处添加是为增加无人机的安全性能，在实际测试过程中，采用某款国产的GPS和飞控，气压计和GPS定位误差极大，
        //导致了无人机起飞后直接飘走，高度和位置都不正常，无法跳出模式循环，导致遥控且无法接管
        //因此增加了时间判断，确保无人机在切入offboard模式和解锁后，确保任何情况下，8秒后遥控器都能切入其他模式接管无人机  
        //注意：一定要确定GPS和飞控传感器都是正常的
        //注意：一定要确定GPS和飞控传感器都是正常的
        //注意：一定要确定GPS和飞控传感器都是正常的
        //注意：一定要确定GPS和飞控传感器都是正常的
        //注意：一定要确定GPS和飞控传感器都是正常的
            
        //发布期望位置信息
        pose.pose.position.x =init_position_x_take_off + 0;
        pose.pose.position.y =init_position_y_take_off + 0;
        pose.pose.position.z =init_position_z_take_off + ALTITUDE;
        local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }       
    
    while(ros::ok())
    {
        //发布期望位置
        local_pos_pub.publish(pose);
        ros::spinOnce();

        //配合rate(20.0)使用，使得while按照20Hz的频率执行;
        rate.sleep();
    }
    return 0;
}