ROS实现串口GPS数据的解析与通信

1. 配置串口

配置串口时，利用ROS自带的serial功能包进行串口数据的读取，具体来说就是创建一个串口对象，用成员函数read进行读取，需要注意的是其中Timeout的设置以及read在调用一次后就会清空缓存中的串口数据。

参考：
ROS之串口编程学习笔记 https://blog.csdn.net/u014695839/article/details/81209082
ROS系统的串口数据读取和解析 https://blog.csdn.net/Tansir94/article/details/81357612

2. 数据截取

从串口助手cutecom可以看到，GPS的数据为以下形式（GGA协议）：
$GNGGA,131547.30,3908.17889767,N,11715.45111804,E,1,14,1.7,18.282,M,-8.614,M, ,*54

字段0：$GNGGA，语句ID，表明该语句为Global Positioning System Fix Data（GGA）GPS定位信息
字段1：UTC 时间，hhmmss.sss，时分秒格式
字段2：纬度ddmm.mmmm，度分格式（前导位数不足则补0）
字段3：纬度N（北纬）或S（南纬）
字段4：经度dddmm.mmmm，度分格式（前导位数不足则补0）
字段5：经度E（东经）或W（西经）
字段6：GPS状态，0=未定位，1=非差分定位，2=差分定位，3=无效PPS，6=正在估算
字段7：正在使用的卫星数量（前导位数不足则补0）
字段8：HDOP水平精度因子（0.5 - 99.9）
字段9：海拔高度（-9999.9 - 99999.9）
字段10：地球椭球面相对大地水准面的高度
字段11：差分时间（从最近一次接收到差分信号开始的秒数，如果不是差分定位将为空）
字段12：差分站ID号0000 - 1023（前导位数不足则补0，如果不是差分定位将为空）
字段13：校验值
由于在串口读取过程中，有可能每次不能完全读取到完整的信息，所以根据上述GGA协议的数据格式及含义，可用以下代码截取出一段符合协议要求的数据。

C++代码片段：

//GPS起始标志
std::string gstart = "$GN";
//GPS终止标志
std::string gend = "\r\n";
int i = 0, start = -1, end = -1;
while ( i < strRece.length() )//strRece为从串口读取的待截取的字符串
{
    //找起始标志
    start = strRece.find(gstart);
    //如果没找到，丢弃这部分数据，但要留下最后2位,避免遗漏掉起始标志
    if ( start == -1)
    {
        if (strRece.length() > 2)   
        strRece = strRece.substr(strRece.length()-3);
        break;
    }
    //如果找到了起始标志，开始找终止标志
    else
    {
        //找终止标志
        end = strRece.find(gend);
        //如果没找到，把起始标志开始的数据留下，前面的数据丢弃，然后跳出循环
        if (end == -1)
        {
            strRece = strRece.substr(start);
            break;
        }
        //如果找到了终止标志，把这段有效的数据剪切给解析的函数，剩下的继续开始寻找
        else
        {
            i = end;
            //把有效的数据给解析的函数以获取经纬度
            double lat, lon;
            RecePro(strRece.substr(start,end+2-start),lat,lon);
            std::cout << std::setiosflags(std::ios::fixed)<<std::setprecision(7)<< "纬度：" << lat << " 经度："<< lon << "\n";
            //发布消息到话题
            serialPort::GPS GPS_data;
            GPS_data.lat = lat;
            GPS_data.lon = lon;
            GPS_pub.publish(GPS_data);
            //如果剩下的字符大于等于4，则继续循环寻找有效数据,如果所剩字符小于等于3则跳出循环
            if ( i+5 < strRece.length())
            strRece = strRece.substr(end+2);
            else
            {   strRece = strRece.substr(end+2);
                break;
            }
        }
    }
}

3. 数据解析

由于C++中没有字符串分割函数，用find和push_back实现字符串的分割，并用atof实现字符串的转换，得到了解析后的经纬度。

C++代码片段：

void RecePro(std::string s , double& lat , double& lon )
{
    //分割有效数据，存入vector中
    std::vector<std::string> v;
    std::string::size_type pos1, pos2;
    pos2 = s.find(",");
    pos1 = 0;
    while ( std::string::npos !=pos2 )
    {
        v.push_back( s.substr( pos1, pos2-pos1 ) );
        pos1 = pos2 + 1;
        pos2 = s.find(",",pos1);
    }
    if ( pos1 != s.length() )
        v.push_back( s.substr( pos1 ));
    //解析出经纬度
    if (v.max_size() >= 6 && (v[6] == "1" || v[6] == "2" || v[6] == "3" || v[6] == "4" || v[6] == "5" || v[6] == "6" || v[6] == "8" || v[6] == "9"))
    {
        //纬度
        if (v[2] != "") lat = std::atof(v[2].c_str()) / 100;
        int ilat = (int)floor(lat) % 100;
        lat = ilat + (lat - ilat) * 100 / 60;
        //经度
        if (v[4] != "") lon = std::atof(v[4].c_str()) / 100;
        int ilon = (int)floor(lon) % 1000;
        lon = ilon + (lon - ilon) * 100 / 60;

    }
}

4. 自定义GPS消息

自定义GPS消息类型，先暂时包含lat（纬度）和lon（经度）两个数据，其实就类似于结构体的定义，在配置CmakeList.txt时，要注意其中函数定义的先后顺序不能任意更改。
参考：
ROS之msg文件定义以及自定义发布主题消息类型 https://blog.csdn.net/myhalan/article/details/64126584
ROS 自定义消息类型、使用方法以及常见错误解决方案 https://blog.csdn.net/qq_16775293/article/details/80449203

5. 完成发布器的编写

有了以上的基础之后，即可完成消息发布器的编写，完整的C++代码（serialPort.cpp）如下：

#include <ros/ros.h> 
#include <serial/serial.h>  //ROS已经内置了的串口包 
#include <std_msgs/String.h>
#include <std_msgs/Empty.h> 
#include <string>
#include <vector>
#include <sstream>
#include <cmath>
#include <cstdlib>//string转化为double
#include <iomanip>//保留有效小数
#include "serialPort/GPS.h"
serial::Serial ser; //声明串口对象

//解析GPS
void RecePro(std::string s , double& lat , double& lon )
{
    //分割有效数据，存入vector中
    std::vector<std::string> v;
    std::string::size_type pos1, pos2;
    pos2 = s.find(",");
    pos1 = 0;
    while ( std::string::npos !=pos2 )
    {
        v.push_back( s.substr( pos1, pos2-pos1 ) );
        pos1 = pos2 + 1;
        pos2 = s.find(",",pos1);
    }
    if ( pos1 != s.length() )
        v.push_back( s.substr( pos1 ));
    //解析出经纬度
    if (v.max_size() >= 6 && (v[6] == "1" || v[6] == "2" || v[6] == "3" || v[6] == "4" || v[6] == "5" || v[6] == "6" || v[6] == "8" || v[6] == "9"))
    {
        //纬度
        if (v[2] != "") lat = std::atof(v[2].c_str()) / 100;
        int ilat = (int)floor(lat) % 100;
        lat = ilat + (lat - ilat) * 100 / 60;
        //经度
        if (v[4] != "") lon = std::atof(v[4].c_str()) / 100;
        int ilon = (int)floor(lon) % 1000;
        lon = ilon + (lon - ilon) * 100 / 60;

    }
}
int main(int argc, char** argv)
{
    //初始化节点
    ros::init(argc, argv, "serial_node");
    //声明节点句柄
    ros::NodeHandle nh;
    //注册Publisher到话题GPS
    ros::Publisher GPS_pub = nh.advertise<serialPort::GPS>("GPS",1000);
    try
    {
      //串口设置
      ser.setPort("/dev/ttyUSB0");
      ser.setBaudrate(115200);
      serial::Timeout to = serial::Timeout::simpleTimeout(1000);
      ser.setTimeout(to);
      ser.open();
    }
    catch (serial::IOException& e)
    {
        ROS_ERROR_STREAM("Unable to open Serial Port !");
        return -1;
    }
    if (ser.isOpen())
    {
        ROS_INFO_STREAM("Serial Port initialized");
    }
    else
    {
        return -1;
    }

    ros::Rate loop_rate(50);

    std::string strRece;
    while (ros::ok())
    {

        if (ser.available())
        {
            //1.读取串口信息：
            ROS_INFO_STREAM("Reading from serial port\n");
            //通过ROS串口对象读取串口信息
            //std::cout << ser.available();
            //std::cout << ser.read(ser.available());
            strRece += ser.read(ser.available());
            //std::cout <<"strRece:" << strRece << "\n" ;
            //strRece = "$GNGGA,122020.70,3908.17943107,N,11715.45190423,E,1,17,1.5,19.497,M,-8.620,M,,*54\r\n";
            //2.截取数据、解析数据：
            //GPS起始标志
            std::string gstart = "$GN";
            //GPS终止标志
            std::string gend = "\r\n";
            int i = 0, start = -1, end = -1;
            while ( i < strRece.length() )
            {
                //找起始标志

                start = strRece.find(gstart);
                //如果没找到，丢弃这部分数据，但要留下最后2位,避免遗漏掉起始标志
                if ( start == -1)
                {
                    if (strRece.length() > 2)   
                        strRece = strRece.substr(strRece.length()-3);
                        break;

                }
                //如果找到了起始标志，开始找终止标志
                else
                {
                    //找终止标志
                    end = strRece.find(gend);
                    //如果没找到，把起始标志开始的数据留下，前面的数据丢弃，然后跳出循环
                    if (end == -1)
                    {
                        if (end != 0)
                        strRece = strRece.substr(start);
                        break;
                    }
                    //如果找到了终止标志，把这段有效的数据剪切给解析的函数，剩下的继续开始寻找
                    else
                    {
                        i = end;

                        //把有效的数据给解析的函数以获取经纬度
                        double lat, lon;
                        RecePro(strRece.substr(start,end+2-start),lat,lon);
                        std::cout << std::setiosflags(std::ios::fixed)<<std::setprecision(7)<< "纬度：" << lat << " 经度："<< lon << "\n";
                        //发布消息到话题
                        serialPort::GPS GPS_data;
                        GPS_data.lat = lat;
                        GPS_data.lon = lon;
                        GPS_pub.publish(GPS_data);
                        //如果剩下的字符大于等于4，则继续循环寻找有效数据,如果所剩字符小于等于3则跳出循环
                        if ( i+5 < strRece.length())
                            strRece = strRece.substr(end+2);
                        else
                        {   strRece = strRece.substr(end+2);
                            break;
                        }
                    }
                }
            }
        }
    ros::spinOnce();
    loop_rate.sleep();
    }
}

6. 完成订阅器的编写

参考：
ROS消息发布器和订阅器的编写 https://blog.csdn.net/weixin_43795921/article/details/85055679
C++代码（listener.cpp）：

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "serialPort/GPS.h"
#include <iomanip>
void chatterCallback(const serialPort::GPSConstPtr& msg)
{
    std::cout << std::setiosflags(std::ios::fixed) << std::setprecision(7) << "纬度：" << msg->lat << " 经度：" << msg->lon << "\n";
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "listener");
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("GPS", 1000, chatterCallback);
    ros::spin();
    return 0;
}

7. 编译配置

CmakeLists.txt:

cmake_minimum_required(VERSION 2.8.3)
project(serialPort)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  serial
  std_msgs
  message_generation
)


add_message_files(
   FILES
   GPS.msg
 )
generate_messages(DEPENDENCIES
  std_msgs
)

catkin_package(

  CATKIN_DEPENDS message_runtime

)

include_directories(
# include
  ${catkin_INCLUDE_DIRS}
)


add_executable(serialPort src/serialPort.cpp)
target_link_libraries(serialPort ${catkin_LIBRARIES})
add_dependencies(serialPort ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

add_executable(listener src/listener.cpp)
target_link_libraries(listener ${catkin_LIBRARIES})
add_dependencies(listener ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

package.xml:

<?xml version="1.0"?>
<package format="2">
  <name>serialPort</name>
  <version>0.0.0</version>
  <description>The serialPort package</description>

  <!-- One maintainer tag required, multiple allowed, one person per tag -->
  <!-- Example:  -->
  <!-- <maintainer email="[email protected]">Jane Doe</maintainer> -->
  <maintainer email="[email protected]">bingo</maintainer>


  <!-- One license tag required, multiple allowed, one license per tag -->
  <!-- Commonly used license strings: -->
  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
  <license>TODO</license>


  <!-- Url tags are optional, but multiple are allowed, one per tag -->
  <!-- Optional attribute type can be: website, bugtracker, or repository -->
  <!-- Example: -->
  <!-- <url type="website">http://wiki.ros.org/serialPort</url> -->


  <!-- Author tags are optional, multiple are allowed, one per tag -->
  <!-- Authors do not have to be maintainers, but could be -->
  <!-- Example: -->
  <!-- <author email="[email protected]">Jane Doe</author> -->


  <!-- The *depend tags are used to specify dependencies -->
  <!-- Dependencies can be catkin packages or system dependencies -->
  <!-- Examples: -->
  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
  <!--   <depend>roscpp</depend> -->
  <!--   Note that this is equivalent to the following: -->
  <!--   <build_depend>roscpp</build_depend> -->
  <!--   <exec_depend>roscpp</exec_depend> -->
  <!-- Use build_depend for packages you need at compile time: -->
  <!--   <build_depend>message_generation</build_depend> -->
  <!-- Use build_export_depend for packages you need in order to build against this package: -->
  <!--   <build_export_depend>message_generation</build_export_depend> -->
  <!-- Use buildtool_depend for build tool packages: -->
  <!--   <buildtool_depend>catkin</buildtool_depend> -->
  <!-- Use exec_depend for packages you need at runtime: -->
  <!--   <exec_depend>message_runtime</exec_depend> -->
  <!-- Use test_depend for packages you need only for testing: -->
  <!--   <test_depend>gtest</test_depend> -->
  <!-- Use doc_depend for packages you need only for building documentation: -->
  <!--   <doc_depend>doxygen</doc_depend> -->
  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>serial</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>message_generation</build_depend>

  <build_export_depend>roscpp</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>serial</exec_depend>
  <exec_depend>std_msgs</exec_depend>
  <exec_depend>message_runtime</exec_depend>

  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <!-- Other tools can request additional information be placed here -->

  </export>
</package>

8. 运行测试

roscore后，分别在两个终端中运行serialPort和listener两个节点：