ROS节点解析GPS数据：GPRMC/GPFDP/HEADINGA

数据解析，肯定是要知道数据格式的：

数据格式参考：（前人已经总结的比较齐全了）

https://blog.csdn.net/u010384390/article/details/78432016?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522163031225416780274159663%2522%252C%2522scm%2522%253A%252220140713.130102334..%2522%257D&request_id=163031225416780274159663&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~sobaiduend~default-1-78432016.pc_search_result_control_group&utm_term=headinga&spm=1018.2226.3001.4187

星网宇达的XW-GI类产品，GPS解析到的协议是GPFPD：GI定位定姿消息集。其数据格式如下所示：

最高输出速率：100HZ

举例：

配置接收机的输出格式为：（使用cutecom接收，输出频率为5Hz）

[13:29:49:216] $GPFPD,1356,20.60,0.000,0.000,0.000,0.000000000,0.000000000,0.000,0.000,0.000,0.000,0.000,0,0,0*72␍␊

[13:29:49:225] $GPRMC,000006.60,V,0000.0000000,S,00000.0000000,W,0.000,0.0,010106,0.0,E,N*24␍␊

[13:29:49:230] #HEADINGA,COM1,0,0,FINESTEERING,1356,20.600,00000000,0000,0036;SOL_COMPUTED,NONE,0.000000000,0.000000000,0.000000000,0,0.000,0.000,"0000",0,0,0,0,00,00,30,17*3CB347CB␍␊

[13:29:49:416] $GPFPD,1356,20.80,0.000,0.000,0.000,0.000000000,0.000000000,0.000,0.000,0.000,0.000,0.000,0,0,0*7C␍␊

[13:29:49:425] $GPRMC,000006.80,V,0000.0000000,S,00000.0000000,W,0.000,0.0,010106,0.0,E,N*2A␍␊

[13:29:49:430] #HEADINGA,COM1,0,0,FINESTEERING,1356,20.800,00000000,0000,0036;SOL_COMPUTED,NONE,0.000000000,0.000000000,0.000000000,0,0.000,0.000,"0000",0,0,0,0,00,00,30,17*1A63E506␍

使用ROS编写节点进行解析，思路为：

1，配置串口，与接收机的波特率等设置保持一致；配置节点循环频率为5Hz，频率相同会简单很多；

2，打开串口，接收数据；

3，数据分割为三个部分，GPFPD/GPRMC/HEADINGA；

4，使用sscanf函数进行解析（也可以使用C++的分割函数，本人更熟悉C语言。。）

5，存放在结构体中，打印显示。

现实结果为：（terminal显示）

gpfpd=
$GPFPD,1356,13110.00,0.000,0.000,0.000,0.000000000,0.000000000,0.000,0.000,0.000,0.000,0.000,0,0,0*44
gprmc=
$GPRMC,033816.00,V,0000.0000000,S,00000.0000000,W,0.000,0.0,010106,0.0,E,N*2B
headingA=
#HEADINGA,COM1,0,0,FINESTEERING,1356,13110.000,00000000,0000,0036;SOL_COMPUTED,NONE,0.000000000,0.000000000,0.000000000,0,0.000,0.000,"0000",0,0,0,0,00,00,30,17*22C246A7

*************GPS_data*****************
utc_time:033816.00
status:V
latitude:S 0.000000000
longtitude:W 0.000000000
speed:0.000000
azimuth_angle:0.000000
utc_data:010106
isRTKstate:N
*************GPFDP_data*****************
GPSWeek:1356
GPSTime:13110.00
Heading:0.000000
Pitch:0.000000
Roll:0.000000
Latitude: 0.000000000
Longitude: 0.000000000
Altitude: 0.000000000
[Ve Vn Vu]=[0.000000 0.000000 0.000000]
Baseline =0.000000
Status:0
*************HEADINGA_DATA*****************
cal_state=SOL_COMPUTED
pos_type=NONE
baseline=0.000000
HeadingA=0.000000
Pitch=0.000000

（以上为显示，下面上代码）

头文件gps.h

#ifndef __GPS_H__
#define __GPS_H__#include<stdio.h>          
#include<stdlib.h>        
#include<unistd.h>       
#include<string.h>
#define GPS_LEN 512
#define BUF_SIZE 16
typedef unsigned int u32;
typedef unsigned short u16;
typedef unsigned char u8;typedef struct __gnrmc__
{u32 time;char pos_state;float latitude;float longitude;float speed;float direction;u32 date;float declination;char dd;char mode;
}GNRMC;
int gps_analyse(char *buff, GNRMC *gps_date);
int print_gps(GNRMC *gps_date);// define GPS_INFO
typedef struct gps_info
{char utc_time[BUF_SIZE];char status;double latitude_value;char latitude;double longtitude_value;char longtitude;float speed;float azimuth_angle;char utc_data[BUF_SIZE];float magnetic_declination;char magnetic_declination_dir;char isRTKstate[4];
}GPS_INFO;int gprmc_analyse(char *buff,GPS_INFO *gprmc_data);
int gprmc_print(GPS_INFO *gprmc_data);//***********define GPFDP_info**************
typedef struct gpfpd_info
{unsigned int GPSWeek;		//自1980-1-6至当前的星期数char GPSTime[BUF_SIZE];		//本周00：00：00至当前的秒数float Heading;				//偏航角float Pitch;				//俯仰角float Roll;					//滚转角double Latitude;			//纬度 degdouble Longitude;			//经度 degfloat Altitude;				//高度 mfloat Ve;					//东 速度 m/sfloat Vn;					//北 速度 m/sfloat Vu;					//天 速度 m/sfloat Baseline;				//基线长度int NSV1;		//天线1卫星数int NSV2;		//天线2卫星数char Status[4];		//系统状态
}GPFPD_INFO;int gpfdp_analyse(char *buff,GPFPD_INFO *gpfdp_data);
int gpdfp_print(GPFPD_INFO *gpfdp_data);//***********define headingA_info**************
typedef struct headingA_info
{char cal_state[32];	//SOL_COMPUTED 完全解算 INSUFFICIENT_OBS 观测量不足 COLD_START 冷启动，尚未完全解算 char pos_type[16];	//NONE 未解算/*FIXEDPOS 已设置固定坐标SINGLE 单点解定位PSRDIFF 伪距差分解定位NARROW_FLOAT 浮点解WIDE_INT 宽带固定解NARROE_INT 窄带固定解SUPER WIDE_LINE 超宽带解 */float baseline;		//基线长度float HeadingA;		//方位角float Pitch;		//俯仰角
}HEADINGA_INFO;
int headingA_analyse(char *buff,HEADINGA_INFO *headinga_data);
int headingA_print(HEADINGA_INFO *headinga_data);
#endif

gps.cpp

#include "/home/ling/catkin_ws/src/serialdemo/include/serialdemo/gps.h"
//************************get-gnrmc-data***************************
int gps_analyse (char *buff,GNRMC *gps_data)
{char *ptr = NULL;if(gps_data == NULL){return -1;}if(strlen(buff)<10){return -1;}//如果buff字符串中包含字符"$GNRMC"则将$GNRMC的地址赋值给ptrif(NULL==(ptr=strstr(buff,"$GNRMC"))){return -1;}sscanf(ptr,"$GNRMC,%d.000,%c,%f,N,%f,E,%f,%f,%d,,,%c*",&(gps_data->time),&(gps_data->pos_state),&(gps_data->latitude),&(gps_data->longitude),&(gps_data->speed),&(gps_data->direction),&(gps_data->date),&(gps_data->mode));
//sscanf函数为从字符串输入，上面这句话的意思是将ptr内存单元的值作为输入分别输入到后面的结构体成员return 0;
} int print_gps (GNRMC *gps_data)
{printf("===========================================================\n");printf("==   GPS状态位 : %c  [A:有效状态 V:无效状态]              \n",gps_data->pos_state);printf("==   GPS模式位 : %c  [A:自主定位 D:差分定位]               \n", gps_data->mode);printf("==   日期 : 20%02d-%02d-%02d                                  \n",gps_data->date%100,(gps_data->date%10000)/100,gps_data->date/10000);printf("==   时间 : %02d:%02d:%02d                                   \n",(gps_data->time/10000+8)%24,(gps_data->time%10000)/100,gps_data->time%100);printf("==   纬度 : 北纬:%d度%d分%d秒                              \n", ((int)gps_data->latitude) / 100, (int)(gps_data->latitude - ((int)gps_data->latitude / 100 * 100)), (int)(((gps_data->latitude - ((int)gps_data->latitude / 100 * 100)) - ((int)gps_data->latitude - ((int)gps_data->latitude / 100 * 100))) * 60.0));printf("==   经度 : 东经:%d度%d分%d秒                              \n", ((int)gps_data->longitude) / 100, (int)(gps_data->longitude - ((int)gps_data->longitude / 100 * 100)), (int)(((gps_data->longitude - ((int)gps_data->longitude / 100 * 100)) - ((int)gps_data->longitude - ((int)gps_data->longitude / 100 * 100))) * 60.0));printf("==   速度 : %.3f  m/s                                      \n",gps_data->speed);printf("============================================================\n");return 0;
} 
//***************************get gprmc data**************************
int gprmc_analyse(char *buff,GPS_INFO *gprmc_data)
{char *wellhandled_string;size_t i=0,readcnt=0;if(strlen(buff)<32){// length of frame is 72,if datalength is too short,return.return -1;}if ((wellhandled_string = strstr(buff,"$GPRMC")) != NULL){for (i=0; i<strlen(wellhandled_string); i++){if (wellhandled_string[i] == '\n'){wellhandled_string[i] = '\0'; //replace ‘/n’ with null}}}readcnt=sscanf(wellhandled_string,"$GPRMC,%[^,],%c,%lf,%c,%lf,%c,%f,%f,%[^,],%*f,%*c,%[^*]",gprmc_data->utc_time,\&(gprmc_data->status),&(gprmc_data->latitude_value),&(gprmc_data->latitude),\&(gprmc_data->longtitude_value),&(gprmc_data->longtitude),&(gprmc_data->speed),\&(gprmc_data->azimuth_angle),\gprmc_data->utc_data,gprmc_data->isRTKstate);//printf("readcnt=%d\n",readcnt);return 0;
}
int gprmc_print(GPS_INFO *gprmc_data)
{printf("*************GPS_data*****************\n");printf("utc_time:%s\n",gprmc_data->utc_time); printf("status:%c\n",gprmc_data->status); printf("latitude:%c ",gprmc_data->latitude); printf("%12.9lf\n",gprmc_data->latitude_value); printf("longtitude:%c ",gprmc_data->longtitude); printf("%12.9lf\n",gprmc_data->longtitude_value);  printf("speed:%f\n",gprmc_data->speed); printf("azimuth_angle:%f\n",gprmc_data->azimuth_angle); printf("utc_data:%s\n",gprmc_data->utc_data); //printf("magnetic_declination:%f %c\n",gprmc_data->magnetic_declination,gprmc_data->magnetic_declination_dir);printf("isRTKstate:%c\n",gprmc_data->isRTKstate[0]); 
}
//***************************get gpfdp data**************************int gpfdp_analyse(char *buff,GPFPD_INFO *gpfdp_data)
{char *wellhandled_temp;size_t i=0,reti=0;if(strlen(buff)<32){return -1;// length of frame is 98,if datalength is too short,return.}if ((wellhandled_temp = strstr(buff,"$GPFPD")) != NULL){for (i=0; i<strlen(wellhandled_temp); i++){if (wellhandled_temp[i] == '\n'){wellhandled_temp[i] = '\0'; }}}//printf("wellhandled_temp=%s\n\n",wellhandled_temp);reti=sscanf(wellhandled_temp,"$GPFPD,%d,%[^,],%f,%f,%f,%lf,%lf,%f,%f,%f,%f,%f,%i,%i,%[^*]",&(gpfdp_data->GPSWeek),gpfdp_data->GPSTime,&(gpfdp_data->Heading),&(gpfdp_data->Pitch),&(gpfdp_data->Roll), \&(gpfdp_data->Latitude),&(gpfdp_data->Longitude),&(gpfdp_data->Altitude),\&(gpfdp_data->Ve),&(gpfdp_data->Vn),&(gpfdp_data->Vu),\&(gpfdp_data->Baseline),&(gpfdp_data->NSV1),&(gpfdp_data->NSV2),\gpfdp_data->Status);//printf("reti=%d\n",reti);	//wellhandled_temp=$GPFPD,1356,9013.40,0.000,0.000,0.000,0.000000000,0.000000000,0.000,0.000,0.000,0.000,0.000,0,0,0*79return 0;
}
int gpdfp_print(GPFPD_INFO *gpfdp_data)
{printf("*************GPFDP_data*****************\n");printf("GPSWeek:%d\n",gpfdp_data->GPSWeek); printf("GPSTime:%s\n",gpfdp_data->GPSTime); printf("Heading:%f\n",gpfdp_data->Heading); printf("Pitch:%f\n",gpfdp_data->Pitch); printf("Roll:%f\n",gpfdp_data->Roll); printf("Latitude:%12.9lf\n",gpfdp_data->Latitude);  printf("Longitude:%12.9lf\n",gpfdp_data->Longitude); printf("Altitude:%12.9lf\n",gpfdp_data->Altitude); printf("[Ve Vn Vu]=[%f %f %f]\n",gpfdp_data->Ve,gpfdp_data->Vn,gpfdp_data->Vu); printf("Baseline =%f\n",gpfdp_data->Baseline);printf("Status:%s\n",gpfdp_data->Status);
}//***************************get headingA data**************************
int headingA_analyse(char *buff,HEADINGA_INFO *headinga_data)
{char *headingA_temp;size_t i=0,reti=0;if(strlen(buff)<100){return -1;}if ((headingA_temp = strstr(buff,";")) != NULL){for (i=0; i<strlen(headingA_temp); i++){if (headingA_temp[i] == '\n'){headingA_temp[i] = '\0'; }}}//printf("headingA_temp=%s\n\n",headingA_temp);reti=sscanf(headingA_temp,";%[^,],%[^,],%f,%f,%f",headinga_data->cal_state,headinga_data->pos_type,&(headinga_data->baseline),\&(headinga_data->HeadingA),&(headinga_data->Pitch));//printf("reti=%d\n",reti);
}
int headingA_print(HEADINGA_INFO *headinga_data)
{printf("*************HEADINGA_DATA*****************\n");printf("cal_state=%s\n",headinga_data->cal_state);printf("pos_type=%s\n",headinga_data->pos_type);printf("baseline=%f\n",headinga_data->baseline);printf("HeadingA=%f\n",headinga_data->HeadingA);printf("Pitch=%f\n",headinga_data->Pitch);
}

主节点：

serial_port.cpp

//serial_port.cpp
#include <ros/ros.h>
#include <serial/serial.h>
#include <iostream>
#include "/home/ling/catkin_ws/src/serialdemo/include/serialdemo/gps.h"GPS_INFO gps_info;
GPS_INFO *rmc_info=&gps_info;
GPFPD_INFO gpfdp_info;
GPFPD_INFO *gpfdp_pdata=&gpfdp_info;
HEADINGA_INFO headingA_info;
HEADINGA_INFO *headingA_pdata=&headingA_info;
char buff[2048];
char GPFPD_buf[1024];
char GPRMC_buf[1024];
char HEADINGA_buf[1024];
int main(int argc, char** argv)
{ros::init(argc, argv, "serial_port");//创建句柄（虽然后面没用到这个句柄，但如果不创建，运行时进程会出错）ros::NodeHandle n;//创建一个serial类serial::Serial sp;//创建timeoutserial::Timeout to = serial::Timeout::simpleTimeout(1000);//设置要打开的串口名称sp.setPort("/dev/ttyUSB0");//sp.setPort("/dev/ttyS0");//设置串口通信的波特率sp.setBaudrate(115200);//串口设置timeoutsp.setTimeout(to);try{//打开串口sp.open();}catch(serial::IOException& e){ROS_ERROR_STREAM("Unable to open port.");return -1;}//判断串口是否打开成功if(sp.isOpen()){ROS_INFO_STREAM("/dev/ttyUSB0 is opened.");}else{return -1;}ros::Rate loop_rate(5);while(ros::ok()){//获取缓冲区内的字节数size_t n = sp.available();if(n!=0){uint8_t buffer[2048];//读出数据n = sp.read(buffer, n);for(int i=0; i<n; i++){//16进制的方式打印到屏幕//std::cout << std::hex << (buffer[i] & 0xff) << " ";buff[i]=buffer[i];//printf("%c",buff[i]);if(buffer[i]=='$'&&buffer[i+1]=='G'&&buffer[i+2]=='P'&&buffer[i+3]=='F'&&buffer[i+4]=='P' && buffer[i+5]=='D'){for(int j=0;j<102 && buffer[i+j] != '\n';j++){GPFPD_buf[j]=buffer[i+j];}printf("gpfpd=\n%s\n",GPFPD_buf);}if(buffer[i]=='$'&&buffer[i+1]=='G'&&buffer[i+2]=='P'&&buffer[i+3]=='R'&&buffer[i+4]=='M' && buffer[i+5]=='C'){for(int j=0;j<100 && buffer[i+j] != '\n';j++){GPRMC_buf[j]=buffer[i+j];}printf("gprmc=\n%s\n",GPRMC_buf);} if(buffer[i]=='#'&&buffer[i+1]=='H'&&buffer[i+2]=='E'&&buffer[i+3]=='A'&&buffer[i+4]=='D' && buffer[i+5]=='I'){for(int j=0;j<300 && buffer[i+j] != '\n';j++){HEADINGA_buf[j]=buffer[i+j];}printf("headingA=\n%s\n",HEADINGA_buf);} }std::cout << std::endl;//jiexie gpsgprmc_analyse(GPRMC_buf,rmc_info);gprmc_print(rmc_info);				gpfdp_analyse(GPFPD_buf,gpfdp_pdata);gpdfp_print(gpfdp_pdata);headingA_analyse(HEADINGA_buf,headingA_pdata);headingA_print(headingA_pdata);//把数据发送回去//sp.write(buffer, n);}memset(buff,'\0',sizeof(buff));loop_rate.sleep();}//关闭串口sp.close();return 0;
}

越来越发现使用ROS可以很方便的连接起来各种传感器，彼此之间通信特别方便。以上过程中，遇到的问题有：linux中的串口配置，ros的seria功能包是使用，ros如何使用自己定义的头文件和源文件，sscanf函数的具体使用等。遇到问题欢迎留言交流～～