1.串口配置

ROS串口编程学习笔记：https://blog.csdn.net/u014695839/article/details/81209082

ROS系统的串口数据读取和解析：https://blog.csdn.net/afeik/article/details/91997758

2.GPS协议分析（以GPFPD为例）

ROS实现串口解析GPS协议，并发布到话题1.串口配置2.GPS协议分析（以GPFPD为例）3.数据校验4.ROS消息收发5.运行结果

可以看出，每一帧数据的头都是由$GPFPD开始的，然后每部分都由逗号隔开，所以如果解析数据的话可以通过逗号来判断。

协议解析代码：

size_t n = sp.available();
        string str;
        string Header;
        string GPSWeek;
        string GPSTime;
        string Heading;
        string Pitch;
        string Roll;
        string Lattitude;
        string Longitude;
        string Altitude;
        string Ve;
        string Vn;
        string Vu;
        string Baseline;
        string NSV1;
        string NSV2;
        string Status;
        string Cs;
        string CrLf;
        uint8_t buffer[1024];
        int num=0;
        int flag=0;
        n = sp.read(buffer, n);
        if(n>0)
        {
            if(GPS_Data_Check(buffer,n)==0)//如果校验不通过则跳过
            {
                continue;
            }
            for(int i=0; i<n; i++)
            {
                if(buffer[i]==',')
                {
                    num++;
                }
                else 
                {
                    if(num==0)
                    {
                        Header+=buffer[i];   
                    }
                    else if(num==1)
                    {
                        GPSWeek+=buffer[i];
                    }
                    else if(num==2)
                    {
                        GPSTime+=buffer[i];
                    }
                    else if(num==3)
                    {
                        Heading+=buffer[i];
                    }
                    else if(num==4)
                    {
                        Pitch+=buffer[i];
                    }
                    else if(num==5)
                    {
                        Roll+=buffer[i];
                    }
                    else if(num==6)
                    {
                        Lattitude+=buffer[i];
                    }
                    else if(num==7)
                    {
                        Longitude+=buffer[i];
                    }
                    else if(num==8)
                    {
                        Altitude+=buffer[i];
                    }
                    else if(num==9)
                    {
                        Ve+=buffer[i];
                    }
                    else if(num==10)
                    {
                        Vn+=buffer[i];
                    }
                    else if(num==11)
                    {
                        Vu+=buffer[i];
                    }
                    else if(num==12)
                    {
                        Baseline+=buffer[i];
                    }
                    else if(num==13)
                    {
                        NSV1+=buffer[i];
                    }
                    else if(num==14)
                    {
                        NSV2+=buffer[i];
                    }
                    else if(num==15)
                    {
                        if(flag<2)
                        {
                            Status+=buffer[i];
                        }
                        else if(flag>=2&&flag<5)
                        {
                            Cs+=buffer[i];
                        }
                        else if(flag>=5)
                        {
                            CrLf+=buffer[i];
                        }
                        flag++;
                    }
                }
                str+=buffer[i];
            }

3.数据校验

ROS实现串口解析GPS协议，并发布到话题1.串口配置2.GPS协议分析（以GPFPD为例）3.数据校验4.ROS消息收发5.运行结果

其中第一种方法是针对标准语句的校验方式，第二种方法是针对二进制语句的。

第一种需要校验的数据位在$到*之间并且需要异或操作。

在校验的过程中首先要对帧头进行校验，查看是否含有$GPFPD，然后再进行校验和的计算。

校验和代码：

for(int i=0;i<length-2;i++)
    {
        if(i<head_n)
        {
            head_str+=a[i];
        }
        if(flag==3)
        {
            CS_str+=a[i];//在一帧数据中本来存在的校验位
        }
        if(a[i]=='*')
        {
            flag=3;
        }
        if(flag==2)
        {
            CS^=a[i];//异或出来的校验位
            chang++;
        }
        if(a[i]=='$')
        {
            flag=2;
        }
    }

因为异或出来的校验位是unsigned char类型的，而在一帧数据中存在的是string类型十六进制的数据。所以我们首先要将异或出来的校验位转换成十进制的ASCLL码的数值，然后将十进制的整型转换为十六进制的string型，再进行对比。

参考资料：

[C++] 十进制转十六进制

4.ROS消息收发

如果是小白的的话可以先看一下关于topic demo的视频：

ROS机器人操作系统入门-中国大学MOOC-topic_demo（上）

ROS机器人操作系统入门-中国大学MOOC-topic_demo（下）

在创建ROS包的时候要记得添加serial

看完之后修改相关文件：

gps.msg

string Header
string GPSWeek
string GPSTime
string Heading
string Pitch
string Roll
string Lattitude
string Longitude
string Altitude
string Ve
string Vn
string Vu
string Baseline
string NSV1
string NSV2
string Status
string Cs
string CrLf

CMakeLists.txt（部分需要修改的地方）

cmake_minimum_required(VERSION 2.8.3)
project(topic_demo)

## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)

## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
  message_generation 
  roscpp
  rospy
  serial
  std_msgs
)

add_message_files(FILES gps.msg)
generate_messages(DEPENDENCIES std_msgs)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)




catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES topic_demo
  CATKIN_DEPENDS roscpp rospy serial std_msgs message_runtime
#  DEPENDS system_lib
)


include_directories(
# include
  ${catkin_INCLUDE_DIRS}
)



# catkin_add_nosetests(test)
add_executable(talker src/talker.cpp)
add_dependencies(talker topic_demo_generate_messages_cpp)
target_link_libraries(talker ${catkin_LIBRARIES})

add_executable(listener src/listener.cpp)
add_dependencies(listener topic_demo_generate_messages_cpp)
target_link_libraries(listener ${catkin_LIBRARIES})

package.xml（部分需要修改的地方）

<buildtool_depend>catkin</buildtool_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>rospy</build_depend>
  <build_depend>serial</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>rospy</build_export_depend>
  <build_export_depend>serial</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>rospy</exec_depend>
  <exec_depend>serial</exec_depend>
  <exec_depend>std_msgs</exec_depend>
  <exec_depend>message_runtime</exec_depend>

发送端代码（完整）：

/***********************************************************/
/*
    Function:ROS parsing GPS agreement(GPFPD)，Post to topic.
    Author:YuTeng
    Date:2019/10/14
    Version: V0.0.1
*/
/***********************************************************/
#include "ros/ros.h"
#include "topic_demo/gps.h"
#include <serial/serial.h>
#include <iostream>
#include <string>
#include <ctime>
using namespace std; 
int GPS_Data_Check(uint8_t a[],int length);
int GPS_Data_Check(string hex_n,int dex);
int main(int argc,char ** argv)
{
    ros::init(argc,argv,"talker"); //解析参数，命名节点
    ros::NodeHandle nh; //创建句柄，实例化node
    topic_demo::gps msg; //创建gps消息
    serial::Serial sp;
    ros::Publisher pub = nh.advertise<topic_demo::gps>("gps_info",1); //创建publisherW  
    ros::Rate loop_rate(1.0); //定义循环发布的频率
    serial::Timeout to = serial::Timeout::simpleTimeout(100);
    //设置要打开的串口名称
    sp.setPort("/dev/ttyUSB0");
    //设置串口通信的波特率
    sp.setBaudrate(115200);
    //窗口设置timeout
    sp.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;
    }
    
    clock_t startTime,endTime;    
    while(ros::ok())
    {
        startTime = clock();//计时开始
        size_t n = sp.available();
        string str;
        string Header;
        string GPSWeek;
        string GPSTime;
        string Heading;
        string Pitch;
        string Roll;
        string Lattitude;
        string Longitude;
        string Altitude;
        string Ve;
        string Vn;
        string Vu;
        string Baseline;
        string NSV1;
        string NSV2;
        string Status;
        string Cs;
        string CrLf;
        uint8_t buffer[1024];
        int num=0;
        int flag=0;
        n = sp.read(buffer, n);
        if(n>0)
        {
            if(GPS_Data_Check(buffer,n)==0)//如果校验不通过则跳过
            {
                continue;
            }
            for(int i=0; i<n; i++)
            {
                if(buffer[i]==',')
                {
                    num++;
                }
                else 
                {
                    if(num==0)
                    {
                        Header+=buffer[i];   
                    }
                    else if(num==1)
                    {
                        GPSWeek+=buffer[i];
                    }
                    else if(num==2)
                    {
                        GPSTime+=buffer[i];
                    }
                    else if(num==3)
                    {
                        Heading+=buffer[i];
                    }
                    else if(num==4)
                    {
                        Pitch+=buffer[i];
                    }
                    else if(num==5)
                    {
                        Roll+=buffer[i];
                    }
                    else if(num==6)
                    {
                        Lattitude+=buffer[i];
                    }
                    else if(num==7)
                    {
                        Longitude+=buffer[i];
                    }
                    else if(num==8)
                    {
                        Altitude+=buffer[i];
                    }
                    else if(num==9)
                    {
                        Ve+=buffer[i];
                    }
                    else if(num==10)
                    {
                        Vn+=buffer[i];
                    }
                    else if(num==11)
                    {
                        Vu+=buffer[i];
                    }
                    else if(num==12)
                    {
                        Baseline+=buffer[i];
                    }
                    else if(num==13)
                    {
                        NSV1+=buffer[i];
                    }
                    else if(num==14)
                    {
                        NSV2+=buffer[i];
                    }
                    else if(num==15)
                    {
                        if(flag<2)
                        {
                            Status+=buffer[i];
                        }
                        else if(flag>=2&&flag<5)
                        {
                            Cs+=buffer[i];
                        }
                        else if(flag>=5)
                        {
                            CrLf+=buffer[i];
                        }
                        flag++;
                    }
                }
                str+=buffer[i];
            }
            msg.Header=Header;
            msg.GPSWeek=GPSWeek;
            msg.GPSTime=GPSTime;
            msg.Heading=Heading;
            msg.Pitch=Pitch;
            msg.Roll=Roll;
            msg.Lattitude=Lattitude;
            msg.Longitude=Longitude;
            msg.Altitude=Altitude;
            msg.Ve=Ve;
            msg.Vn=Vn;
            msg.Vu=Vu;
            msg.Baseline=Baseline;
            msg.NSV1=NSV1;
            msg.NSV2=NSV2;
            msg.Status=Status;
            msg.Cs=Cs;
            msg.CrLf=CrLf;
            std::cout <<"GPS:   "<<str<< std::endl;
            ROS_INFO("Header:%s",msg.Header.c_str());
            ROS_INFO("GPSWeek:%s",msg.GPSWeek.c_str());
            ROS_INFO("GPSTime:%s",msg.GPSTime.c_str());
            ROS_INFO("Heading:%s",msg.Heading.c_str());
            ROS_INFO("Pitch:%s",msg.Pitch.c_str());
            ROS_INFO("Roll:%s",msg.Roll.c_str());
            ROS_INFO("Lattitude:%s",msg.Lattitude.c_str());
            ROS_INFO("Longitude:%s",msg.Longitude.c_str());
            ROS_INFO("Altitude:%s",msg.Altitude.c_str());
            ROS_INFO("Ve:%s",msg.Ve.c_str());
            ROS_INFO("Vn:%s",msg.Vn.c_str());
            ROS_INFO("Vu:%s",msg.Vu.c_str());
            ROS_INFO("Baseline:%s",msg.Baseline.c_str());
            ROS_INFO("NSV1:%s",msg.NSV1.c_str());
            ROS_INFO("NSV2:%s",msg.NSV2.c_str());
            ROS_INFO("Status:%s",msg.Status.c_str());
            ROS_INFO("Cs:%s",msg.Cs.c_str());
            ROS_INFO("CrLf:%s",msg.CrLf.c_str());
            pub.publish(msg); //发布消息
            sp.write(buffer, n);
            endTime = clock();//计时结束
            std::cout << "The run time is: " <<(double)(endTime - startTime) / CLOCKS_PER_SEC << "s" << std::endl;
        }
        n=0;
        loop_rate.sleep(); //根据定义的发布频率，sleep
    }
    sp.close();
    return 0;
}
int GPS_Data_Check(uint8_t a[],int length)//GPS数据校验
{
    int head_n=6;
    string head_str; //帧头
    string CS_str; //数据帧校验位
    int flag=1; //数据标志位
    int CS_dex; 
    unsigned char CS=0;
    int chang=0;
    for(int i=0;i<length-2;i++)
    {
        if(i<head_n)
        {
            head_str+=a[i];
        }
        if(flag==3)
        {
            CS_str+=a[i];//在一帧数据中本来存在的校验位
        }
        if(a[i]=='*')
        {
            flag=3;
        }
        if(flag==2)
        {
            CS^=a[i];//异或出来的校验位
            chang++;
        }
        if(a[i]=='$')
        {
            flag=2;
        }
    }
    CS_dex=CS;  //十进制ASCLL码
    if((head_str=="$GPFPD")&&(GPS_Data_Check(CS_str,CS_dex)))
    {
        return 1;
    }
    else
    {
        return 0;
    }
}
int GPS_Data_Check(string hex_n,int dex)
{
    string dex_con_hex;
    string temp;
    int j=0;
    if(dex<=15)
    {
        string temp="";
        if(dex<10)
        {
            temp+=dex+'0';
        }
        else
        {
            temp+=dex-10+'A';
        }
        dex_con_hex=temp;
        for(int i=temp.length()-1;i>=0;i--)
        {
            dex_con_hex[j]=temp[i];
            j++;
        }
    }
    else
    {
        string temp="";
        do
        {
            int dex_tmp=dex%16;
            if(dex_tmp<10)
            {
                temp+=dex_tmp+'0';
            }
            else
            {
                temp+=dex_tmp-10+'A';
            }
            dex/=16;
        }while(dex>=16);
        if(dex<10)
        {
            temp+=dex+'0';
        }
        else
        {
            temp+=dex-10+'A';
        }
        dex_con_hex=temp;
        for(int i=temp.length()-1;i>=0;i--)
        {
            dex_con_hex[j]=temp[i];
            j++;
        }
    }
    int x;
    int y;
    if(dex_con_hex.compare(hex_n))//如果两个字符串相等则为0
    {
        return 0;
    }
    else
    {
        return 1;
        
    }
}

接收端代码（完整）：

#include "ros/ros.h"
#include "topic_demo/gps.h"
#include <std_msgs/Float32.h>

void gpsCallback(const topic_demo::gps::ConstPtr &msg)
{
  ROS_INFO("Header:%s",msg->Header.c_str());
  ROS_INFO("GPSWeek:%s",msg->GPSWeek.c_str());
  ROS_INFO("GPSTime:%s",msg->GPSTime.c_str());
  ROS_INFO("Heading:%s",msg->Heading.c_str());
  ROS_INFO("Pitch:%s",msg->Pitch.c_str());
  ROS_INFO("Roll:%s",msg->Roll.c_str());
  ROS_INFO("Lattitude:%s",msg->Lattitude.c_str());
  ROS_INFO("Longitude:%s",msg->Longitude.c_str());
  ROS_INFO("Altitude:%s",msg->Altitude.c_str());
  ROS_INFO("Ve:%s",msg->Ve.c_str());
  ROS_INFO("Vn:%s",msg->Vn.c_str());
  ROS_INFO("Vu:%s",msg->Vu.c_str());
  ROS_INFO("Baseline:%s",msg->Baseline.c_str());
  ROS_INFO("NSV1:%s",msg->NSV1.c_str());
  ROS_INFO("NSV2:%s",msg->NSV2.c_str());
  ROS_INFO("Status:%s",msg->Status.c_str());
  ROS_INFO("Cs:%s",msg->Cs.c_str());
  ROS_INFO("CrLf:%s",msg->CrLf.c_str());
}

int main(int argc, char **argv) 
{
  ros::init(argc, argv, "listener"); //
  ros::NodeHandle n;                 //
  ros::Subscriber sub = n.subscribe("gps_info", 1, gpsCallback); //创建subscriber
  ros::spin(); //反复调用当前可触发的回调函数，阻塞
  return 0;
}

5.运行结果

发送端：

ROS实现串口解析GPS协议，并发布到话题1.串口配置2.GPS协议分析（以GPFPD为例）3.数据校验4.ROS消息收发5.运行结果

接收端：

ROS实现串口解析GPS协议，并发布到话题1.串口配置2.GPS协议分析（以GPFPD为例）3.数据校验4.ROS消息收发5.运行结果

ROS实现串口解析GPS协议，并发布到话题1.串口配置2.GPS协议分析（以GPFPD为例）3.数据校验4.ROS消息收发5.运行结果

1.串口配置

2.GPS协议分析（以GPFPD为例）

协议解析代码：

3.数据校验

校验和代码：

4.ROS消息收发

gps.msg

CMakeLists.txt（部分需要修改的地方）

package.xml（部分需要修改的地方）

发送端代码（完整）：

接收端代码（完整）：

5.运行结果

发送端：

接收端：

继续阅读

4G模块的GPS定位差距过大

Android GPS Location Speed Unreliable

Android GPS 自动开启

android手机锁屏后GPS模块发出的NMEA数据频率降低

GPS的NMEA数据转Google Earth的KML数据工具

GPS数据类型格式 NMEA协议

Android之GPS定位类 LocationManager、LocationListener、GpsStatus.Listener、Location详解

【ROS】移动机器人导航仿真（3）——定位（amcl）和路径规划（move_base）

ROS Navigation-----amcl简介

C++实现RVIZ 2D POSE ESTIMATE 功能设置机器人初始坐标1. 首先查看设置初始坐标的话题为 /INTIALPOSE ，查看消息类型和格式从而决定怎么给它发数据2.监听RVIZ发出的数据格式3.仿照RVIZ消息格式4.编写发布节点程序

ROS2(二) - nav2_amcl节点概况

amcl ROS1.算法2.举例3.节点3.1.6Transforms

ROS Navigation之amcl源码解析（完全详解）0. 写在最前面1. amcl是干什么的2. 总体情况3. amcl_node.cpp参考

or frame [laser]: No transform to fixed frame [map]

ros安装-Ubuntu14.04

ROS Stage学习