ros中无人机基于话题＜geographic_msgs/GeoPoseStamped.h＞的指点飞行控制

提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档

前言

本篇文章是作者在学习ros时根据自己的认识所写的代码，主要是基于话题<geographic_msgs/GeoPoseStamped.h>进行gps经纬高的发布以及基于话题<sensor_msgs/NavSatFix.h>进行gps经纬高的订阅。如有错误，敬请指正。

一、话题认识

1.订阅话题sensor_msgs::NavSatFix

话题相关变量如以下链接所示 ：

http://sensor_msgs/NavSatFix.msg

我们可以 先声明一个回调函数进行经纬高的打印。

sensor_msgs::NavSatFix current_gps;//创建全局变量获取当前经纬高信息
void pos_cb(const sensor_msgs::NavSatFix::ConstPtr& msg) {
    current_gps = *msg;
    ROS_INFO("%f %f %f\n",current_gps.longitude,current_gps.latitude,current_gps.altitude);
}

 然后订阅这个话题并将回调函数写入。

ros::Subscriber gps_sub = nh.subscribe<sensor_msgs::NavSatFix>
           ("mavros/global_position/global",10,pos_cb);

2.发布话题 geographic_msgs::GeoPoseStamped

话题相关变量如以下链接所示 ：http://docs.ros.org/en/api/geographic_msgs/html/msg/GeoPoseStamped.html

发布这个话题：

ros::Subscriber gps_sub = nh.subscribe<sensor_msgs::NavSatFix>
           ("mavros/global_position/global",10,pos_cb);

 发布经纬高的例子：

 geographic_msgs::GeoPoseStamped pose;
    pose.pose.position.latitude=47.397751;
    pose.pose.position.longitude=8.545607;
    pose.pose.position.altitude=650.321901;

二、完整代码

/**
 * @file offb_node.cpp
 * @brief 指定经纬高进行指点飞行，其中高度为平均海拔高度。
 */

#include <ros/ros.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <math.h>
#include <sensor_msgs/NavSatFix.h>
#include <geographic_msgs/GeoPoseStamped.h>

sensor_msgs::NavSatFix current_gps;//创建全局变量获取当前经纬高信息
void pos_cb(const sensor_msgs::NavSatFix::ConstPtr& msg) {
    current_gps = *msg;
    ROS_INFO("%f %f %f\n",current_gps.longitude,current_gps.latitude,current_gps.altitude);
}

mavros_msgs::State current_state; // 创建全局变量
// 订阅无人机状态的回调函数将状态信息赋值给全局变量
void state_cb(const mavros_msgs::State::ConstPtr& msg) {
    current_state = *msg;
}

int main(int argc, char** argv)
{
    int Time_k=1;//sample time
    int k=0;
    //float w=0.1;
    ros::init(argc, argv, "offb_node_sin");//初始化
    ros::NodeHandle nh;//定义节点

    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
        ("mavros/state", 10, state_cb);
    ros::Subscriber gps_sub = nh.subscribe<sensor_msgs::NavSatFix>
           ("mavros/global_position/global",10,pos_cb);
    ros::Publisher local_pos_gps_pub = nh.advertise<geographic_msgs::GeoPoseStamped>
           ("mavros/setpoint_position/global", 10);
 
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
        ("mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
        ("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();
    }

    geographic_msgs::GeoPoseStamped pose;//实例化一个pose函数
    //pose.pose.position.latitude=47.397751;//初始点的经纬高
    //pose.pose.position.longitude=8.545607;
    //pose.pose.position.altitude=650.321901;
	

    // send a few setpoints before starting
      // 在切换到offboard模式之前，你必须先发送一些期望点信息到飞控中。 不然飞控会拒绝切换到offboard模式。
    for (int i = 100; ros::ok() && i > 0; --i) {
        local_pos_gps_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }
    
    //进入offboard模式
    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";

    //arm解锁
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    ros::Time last_request = ros::Time::now();//将上一次发送请求的时间改为当前
    pose.header.stamp=ros::Time::now();//
//设置的目标经纬高：47.397751-8.55-650.321901（高度有一个恒定47m的差异，暂时没搞明白原因）
    while (ros::ok()) 
    {
    	Time_k++;
	if(pose.pose.position.longitude<8.55)
	{
		pose.pose.position.latitude=47.397751;
    	pose.pose.position.longitude=pose.pose.position.longitude+0.01*k;
       	pose.pose.position.altitude=650.321901;
		k++;
	}
    	
    	else
	{
		pose.pose.position.longitude=8.55;
	}
    	    	 	
        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();
            }
        }

        local_pos_gps_pub.publish(pose);

        ros::spinOnce();
        rate.sleep();
               
    }
    return 0;
}

 以下为仿真结果展示

总结

以上就是gps指点飞行的全部内容。