數據預處理（15）_解析GPS數據並作爲里程計信息發佈

在機器人建圖過程中，需要激光傳感器和機器人里程信息，里程信息指的是機器人相對於某一個點的距離。一般來說，本車座標系“base_link"原點相對於odom座標系原點的距離。在導航功能包集中消息類型爲nav_msgs/Odometry。
通過rosmsg show nav_msgs/Odometry可以看到：

std_msgs/Header header
string child_frame_id
geometry_msgs/PoseWithCovariance pose
geometry_msgs/TwistWithCovariance twist

geometry_msgs/Pose 消息提供了機器人的位姿信息
geometry_msgs/Twist 消息提供了機器人的速度信息
位姿信息中包含兩個結構：一個是顯示了歐式座標系中的位置，另一個使用四元數顯示了機器人的方向。
速度信息中包含兩個結構：一個是線速度，一個是角速度
ps:一般情況下，我們經常使用的是線速度x和角速度z，使用線速度x是爲了知道機器人向前移動或向後移動，使用角速度z是爲了知道機器人向左/右旋轉。
如果你不想接入採圖車的輪速編碼器，不想去推算運動學模型，可以藉助車上搭載的差分GPS，通過其輸出的經緯高、航向信息轉換爲里程計信息的輸出。
“/nmea_sentence"爲差分GPS發佈的的topic，包含的是一系列逗號分開的字符串。
可以分爲三步走：

1. 從字符串中提取出經緯高，航向信息
2. 轉換成機器人相應的位置、航向信息。
3. 通過nav_msgs/odometry　發佈出去。

/**********************
 * @FileName: gps_odometry.cpp
 * @Author: LZY
 * @DateTime: 2019-02-22 
 * @Description: GPS to odom
 **********************/
#include<iostream>
#include<ros/ros.h>
#include<geometry_msgs/QuaternionStamped.h>
#include<nmea_msgs/Sentence.h>
#include<nav_msgs/Odometry.h>
#include<tf/transform_broadcaster.h>
#include<fstream>
#include<std_msgs/String.h>
#include<string.h>
#include<cmath>
#include "geodesy/utm.h"
#include <unistd.h>
#include <nav_msgs/Path.h> 
#include <tf/tf.h> 
#include "gps_info.h"
#include "WGS84_UTM.h"
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/PoseStamped.h> 

ros::Publisher    g_odom_pub;												
double origin_x=658357.0000,origin_y=3292682.0000; //yanjiuzongyuan
//double origin_x = 670460.0000, origin_y = 3281318.0000;// yuzui

static int GetComma(int num,const char *str)
{
    int i,j=0;
    for(i=0;i<strlen(str);i++)
    {
        if(str[i]==',')
            j++;
        if(j==num)
            return i+1;//返回當前找到的逗號位置的下一個位置
  
    }
    return 0;
}
static double get_double_number(const char *s)
{
    char buf[128];
	int i;
	double rev;
	i=GetComma(1,s);    //得到數據長度
	strncpy(buf,s,i);
	buf[i]=0;			//加字符串結束標誌
	rev=atof(buf);		//字符串轉float
	return rev;
}

/***DegToRad***/
double DegToRad(double deg)
{
  return (deg* pi / 180.0 );
}

/***RadToDeg***/
double RadToDeg(double rad)
{
  return (rad*180.0/pi);
}

void gpsCallback(const nmea_msgs::Sentence::ConstPtr& msg )
{
    static tf::TransformBroadcaster gps_tf;
    GPS_INFO rmc_info;
    GPS_INFO *prmc_info;
    prmc_info=&rmc_info;
    int i,tmp;
    float lati_cent_tmp, lati_second_tmp;
    float long_cent_tmp, long_second_tmp;
    char c,b;
    const char *buf=msg->sentence.c_str();//buf指向字符串首地址
    c=buf[5];
    b=buf[2];
    if(b=='P'&&c=='C')//解析GPRMC字段
    {
       // ROS_INFO("***************************************************************");
       // ROS_INFO("The GPRMC string is:%s",buf);
        prmc_info->D.hour=  (buf[7]-'0')*10+(buf[8]-'0');
        prmc_info->D.minute=(buf[9]-'0')*10+(buf[10]-'0');
        prmc_info->D.second=(buf[11]-'0')*10+(buf[12]-'0');
        tmp=GetComma(9,buf);//尋找第9個逗號的下一個字符串的序號
        //ROS_INFO("the number is:%d",tmp);
        prmc_info->D.day=   (buf[tmp+0]-'0')*10+(buf[tmp+1]-'0');
        prmc_info->D.month= (buf[tmp+2]-'0')*10+(buf[tmp+3]-'0');
        prmc_info->D.year=  (buf[tmp+4]-'0')*10+(buf[tmp+5]-'0')+2000;
        prmc_info->status=  buf[GetComma(2,buf)];                     //位置狀態，A=有效，V=無效
        //這裏的經緯度 (DDDmm.mm)，字符串轉換爲雙精度數，然後轉爲度deg
        prmc_info->latitude= get_double_number(&buf[GetComma(3,buf)]); //緯度,單位爲度
        prmc_info->NS=      buf[GetComma(4,buf)];
        prmc_info->longitude=get_double_number(&buf[GetComma(5,buf)]); //經度，單位爲度
        prmc_info->EW=      buf[GetComma(6,buf)];
	
	int lat_d=prmc_info->latitude/100;
	int long_d = prmc_info->longitude/100;
	double lat_fd = (prmc_info->latitude-lat_d*100)/60;
	double long_fd = (prmc_info->longitude-long_d*100)/60;
        prmc_info->latitude_d =lat_d+lat_fd;
        prmc_info->longitude_d = long_d+long_fd;
        prmc_info->direction=get_double_number(&buf[GetComma(8,buf)]);//航向角，一般轉爲弧度進行計算
	
	Local_data local_XY;
	geographic_msgs::GeoPoint gp;
	gp.latitude = prmc_info->latitude_d;
	gp.longitude =prmc_info->longitude_d;
	geodesy::UTMPoint pt(gp);
	local_XY.X = pt.easting;
	local_XY.Y  = pt.northing;
	double yaw = DegToRad(-(prmc_info->direction-90));
	//ROS_INFO("Local_X = %lf ", local_XY.X);
	//ROS_INFO("Local_Y = %lf ", local_XY.Y);
	
	tf::Quaternion qua;
	qua.setRPY(0,0,yaw);
    
	 /***publish gps_odom**/
	nav_msgs::Odometry odom;
        odom.header.stamp=ros::Time::now();
        odom.header.frame_id="map";
        odom.child_frame_id="gps";
        odom.pose.pose.position.x=local_XY.X-origin_x;
        odom.pose.pose.position.y=local_XY.Y-origin_y;
        odom.pose.pose.orientation.x=qua.x();
	    odom.pose.pose.orientation.y=qua.y();
	    odom.pose.pose.orientation.z=qua.z();
	    odom.pose.pose.orientation.w=qua.w();
        g_odom_pub.publish(odom);
	
	/***publish world to gps tf **/
	tf::StampedTransform trans;
	trans.stamp_ = odom.header.stamp;
	trans.frame_id_ =odom.header.frame_id;
	trans.child_frame_id_ = odom.child_frame_id;
	trans.setOrigin(tf::Vector3(odom.pose.pose.position.x,odom.pose.pose.position.y,odom.pose.pose.position.z));
	trans.setRotation(qua);
	gps_tf.sendTransform(trans);
    }
}

 int main(int argc,char **argv)
{
    ros::init(argc,argv,"odometry");
    ros::NodeHandle nh;
    
    ros::Subscriber sub=nh.subscribe("/nmea_sentence",1000, gpsCallback);				
    g_odom_pub=nh.advertise<nav_msgs::Odometry>("gps/odometry",1);			
    ros::spin();
    return 1;
}

解算出GPS的經緯度以後，需要發佈里程計信息，父座標系爲“map”，子座標系爲“gps”，子座標系圍繞着父座標系運動，存在一定的運動軌跡。
1）發佈里程計信息。將車輛偏航角(旋轉）由四元數表示，

tf::Quaternion qua;
qua.setRPY(0,0,yaw);
 odom.pose.pose.orientation.x=qua.x();
 odom.pose.pose.orientation.y=qua.y();
 odom.pose.pose.orientation.z=qua.z();
 odom.pose.pose.orientation.w=qua.w();
 odom.pose.pose.position.x=local_XY.X-origin_x;
 odom.pose.pose.position.y=local_XY.Y-origin_y;
 ros::Publisher    g_odom_pub;	
 g_odom_pub.publish(odom);

即可發佈里程計
2）發佈GPS與world座標系的tf關係，後續

tf::StampedTransform trans;
trans.stamp_ = odom.header.stamp;
trans.frame_id_ =odom.header.frame_id;
trans.child_frame_id_ = odom.child_frame_id;
trans.setOrigin(tf::Vector3(odom.pose.pose.position.x,odom.pose.pose.position.y,odom.pose.pose.position.z));
trans.setRotation(qua);
gps_tf.sendTransform(trans);