ROS无人机初始化GPS定位漂移误差，确保无人机稳定飞行

引言： 由于GPS在室外漂移的误差比较大，在长时间静止后启动，程序发布的位置可能已经和预期的位置相差较大，导致无法完成任务，尤其是气压计的数据不准，可能会导致无人机不能起飞或者一飞冲天。本文主要是在进行程序控制的时候，首先拿到最新的漂移误差，在此基础上进行程序控制，可以保证程序的稳定运行。

步骤一：创建订阅者订阅无人机的里程计信息

//创建一个Subscriber订阅者，订阅名为/mavros/local_position/odom的topic，注册回调函数local_pos_cb
ros::Subscriber local_pos_sub = nh.subscribe<nav_msgs::Odometry>("/mavros/local_position/odom", 10, local_pos_cb);

步骤二：创建相关变量，获取无人机的实时位置信息

这里定义了较多的变量，最重要的是位置和标志为，其余的可以自行设置

//定义变量，用于接收无人机的里程计信息
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)
{
    local_pos = *msg;
    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);
}

注：启动这个控制节点后，或订阅最新的初始位置，并且通过标志为控制只订阅一次，这个值就是最新的漂移值

步骤四：发布目标点，一定要把获取到的初始漂移值加上，如下：

//发布期望位置信息
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);

这里ALTITUDE是我宏定义的0.5，表明无人机飞0.5米高度即可。室外使用的话，最好高度设置大于1米

步骤五：整体代码如下：

//包含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>

#define ALTITUDE  0.5

//定义变量，用于接收无人机的状态信息
mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& 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 state_cb(const mavros_msgs::State::ConstPtr& msg)
{
    current_state = *msg;
}

//回调函数接收无人机的里程计信息
void local_pos_cb(const nav_msgs::Odometry::ConstPtr& msg)
{
    local_pos = *msg;
    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)
{
	//防止中文乱码
	setlocale(LC_ALL, "");

    //ROS节点初始化，节点名为offboard_single_position
    ros::init(argc, argv, "offboard_single_position");

    //创建节点句柄
    ros::NodeHandle nh;

    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 10);

    //创建一个Subscriber订阅者，订阅名为/mavros/state的topic，注册回调函数state_cb
    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("mavros/state", 10, state_cb);
   
    //创建一个Subscriber订阅者，订阅名为/mavros/local_position/odom的topic，注册回调函数local_pos_cb
    ros::Subscriber local_pos_sub = nh.subscribe<nav_msgs::Odometry>("/mavros/local_position/odom", 10, local_pos_cb);

    //创建一个服务客户端，连接名为/mavros/cmd/arming的服务，用于请求无人机解锁
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");

    //创建一个服务客户端，连接名为/mavros/set_mode的服务，用于请求无人机进入offboard模式
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");

    //设置话题发布频率，需要大于2Hz，飞控连接有500ms的心跳包
    ros::Rate rate(20.0);

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

    //设置无人机的期望位置
    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;

    //send a few setpoints before starting
    for(int i = 100; ros::ok() && i > 0; --i)
    {
        local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }

    //定义客户端变量，设置为offboard模式
    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();
 
    while(ros::ok())
    {
    	//请求进入OFFBOARD模式
		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();
	   	}
		else 
		{
			//请求解锁
			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();
			}
		}
	    if(fabs(local_pos.pose.pose.position.z- init_position_z_take_off -ALTITUDE)<0.2)
		{	
			if(ros::Time::now() - last_request > ros::Duration(3.0))
			{
				break;
			}
		}
		//发布期望位置信息
		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())
    {
        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();
    }
    return 0;
}