ROS开发（3-1）ROS串口解析GPS数据-自定义消息

 

 

参考资料

ROS开发（2）ROS创建自定义消息-普通消息

https://www.cnblogs.com/gooutlook/p/16493595.html

 

安装 ros串口库

sudo apt-get install ros-<版本号>-serial

　　

Ubuntu20.04 noetic
Ubuntu18.04 melodic
Ubuntu16.04 kinetic
Ubuntu14.04 indigo

1创建环境工程 

1创建catkin_gps/src工程文件环境

 

 

 在catkin_gps路径下编译

catkin_make

　　

 

 

 自动生成

 

 

 

 2 创建项目工程

环境工程catkin_gps/src下创建工程v1_GetGPS

 

 

 在v1_GetGPS下创建文件.

 

2-1创建消息文件

在项目包里面创建msg文件夹

 

 

 

 

 

 

 

bool bool_msg
float64 lat
float64 lon
float64 high
string string_msg
time time_msg

　　

2-2 创建编辑package.xml　

 

 

 包含文件依赖

1 自定义消息

2 串口

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

  <!-- One maintainer tag required, multiple allowed, one person per tag -->
  <!-- Example:  -->
  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
  <maintainer email="dongdong@todo.todo">dongdong</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>

  <buildtool_depend>catkin</buildtool_depend>
  
  <build_depend>message_generation</build_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>message_runtime</exec_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>rospy</exec_depend>
  <exec_depend>serial</exec_depend>
  <exec_depend>std_msgs</exec_depend>


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

  </export>
</package>

　　

修改

1项目名字v1_GetGPS和文件夹一样

 

 

 

2-3创建编辑CMakeLists.txt

 

 

 

cmake_minimum_required(VERSION 3.0.2)
project(v1_GetGPS)

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


find_package(catkin REQUIRED COMPONENTS
  message_generation
  message_runtime
  roscpp
  rospy
  std_msgs
  serial #串口引用
)

add_message_files(
  FILES
  topic_msg.msg#消息引用-添加我们自己定义的xxx.msg文件
)

generate_messages(
  DEPENDENCIES
  std_msgs#消息引用-自带
)

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES my_topic002
#  CATKIN_DEPENDS message_generation message_runtime roscpp rospy std_msgs
#  DEPENDS system_lib
)

###########
## Build ##
###########


include_directories(
# include
  ${catkin_INCLUDE_DIRS}
)

add_executable(publisher_gps src/publisher_gps.cpp)
target_link_libraries(publisher_gps ${catkin_LIBRARIES})
add_dependencies(publisher_gps ${PROJECT_NAME}_generate_messages_cpp) 

add_executable(subscriber_gps src/subscriber_gps.cpp)
target_link_libraries(subscriber_gps ${catkin_LIBRARIES})
add_dependencies(subscriber_gps ${PROJECT_NAME}_generate_messages_cpp) 


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

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

　1修改项目名字和文件夹一样

 

 

 2 修改自定义的消息

 

 

 

 

 

3项目功能 

 

 

3-1发布器

 

gps数据

从串口助手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协议的数据格式及含义，可用以下代码截取出一段符合协议要求的数据。

　　

 

 

v1_GetGPS/src下创建serialPort.cpp

 

#include <string>
#include <vector>
#include <sstream>
#include <cmath>
#include <cstdlib>//string转化为double
#include <iomanip>//保留有效小数

#include <ros/ros.h> 
#include <serial/serial.h>  //ROS已经内置了的串口包 

#include "ros/time.h"// 时间戳


//0 导入消息 工程名字/消息名字
#include "v1_GetGPS/topic_msg.h"
#include "std_msgs/String.h"
#include <std_msgs/Empty.h> 

serial::Serial ser; //声明串口对象

//解析GPS
void RecePro(std::string s , double& lat , double& lon, double& high )
{
    //分割有效数据，存入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 ));
    //解析出经纬度
    //字段6：GPS状态，0=未定位，1=非差分定位，2=差分定位，3=无效PPS，6=正在估算
    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;
        //海拔高度
        if (v[9] != "") high = std::atof(v[9].c_str()) ;


    }
}
int main(int argc, char** argv)
{
    //初始化节点
    ros::init(argc, argv, "serial_node");
    //声明节点句柄
    ros::NodeHandle nh;
    //注册Publisher到话题GPS
    ros::Publisher GPS_pub = nh.advertise<v1_GetGPS::topic_msg>("GPS",1000);
    try
    {
      //串口设置
      ser.setPort("/dev/ttyUSB0");
      ser.setBaudrate(115200);
      serial::Timeout to = serial::Timeout::simpleTimeout(5000);
      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;

                        //ros::Time begin = ros::Time::now();
                        double secs =ros::Time::now().toSec();

                        //把有效的数据给解析的函数以获取经纬度
                        double lat, lon, high;
                        RecePro(strRece.substr(start,end+2-start),lat,lon,high);
                        //保留位数7位
                        std::cout << std::setiosflags(std::ios::fixed)<<std::setprecision(7)<< "纬度：" << lat << " 经度："<< lon <<" 高度： "<< high <<"   时间:  " << secs << "\n";
                        //发布消息到话题
                        v1_GetGPS::topic_msg GPS_data;
                        GPS_data.lat = lat;
                        GPS_data.lon = lon;
                        GPS_data.high = high;
                        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();
    }
}

 

 

 

 

修改

1包含头文件

自定义消息路径和名字 工程包名字imu_gps_publish+自定义的消息名字topic_msg

 

 

 2 所有关于自定义消息的引用名字

包含头文件

 

 

 

 

3 修改波特率和串口号

并给与GPS串口权限

https://www.cnblogs.com/gooutlook/p/16494885.html

 

3-2接收节点

v1_GetGPS/src下创建listener.cpp

 

#include <iomanip>

#include "ros/ros.h"

//自定义ros消息
#include "std_msgs/String.h"
#include "v1_GetGPS/topic_msg.h"

#include "ros/time.h"// 时间戳

void chatterCallback(const v1_GetGPS::topic_msg::ConstPtr& msg)
{

    //ros::Time begin = ros::Time::now();
    double secs =ros::Time::now().toSec();
    std::cout << "接收消息（小数10位）："<<std::setiosflags(std::ios::fixed) << std::setprecision(10) << "纬度：" << msg->lat << " 经度：" << msg->lon <<" 高度： "<< msg->high <<"  时间戳：  "<< secs <<"\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;
}

　　

 

修改

1自定义消息路径和名字 工程包名字imu_gps_publish+自定义的消息名字topic_msg

 

 

 

2 所有关于自定义消息的引用名字

 

 

 

3-3 修改CMakeLists.txt编译节点

 

 

 添加发射和接受节点的生成

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

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

　　完整的

cmake_minimum_required(VERSION 3.0.2)
project(v1_GetGPS)

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


find_package(catkin REQUIRED COMPONENTS
  message_generation
  message_runtime
  roscpp
  rospy
  std_msgs
  serial #串口引用
)

add_message_files(
  FILES
  topic_msg.msg#消息引用-添加我们自己定义的xxx.msg文件
)

generate_messages(
  DEPENDENCIES
  std_msgs#消息引用-自带
)

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES my_topic002
#  CATKIN_DEPENDS message_generation message_runtime roscpp rospy std_msgs
#  DEPENDS system_lib
)

###########
## Build ##
###########


include_directories(
# include
  ${catkin_INCLUDE_DIRS}
)

add_executable(publisher_gps src/publisher_gps.cpp)
target_link_libraries(publisher_gps ${catkin_LIBRARIES})
add_dependencies(publisher_gps ${PROJECT_NAME}_generate_messages_cpp) 

add_executable(subscriber_gps src/subscriber_gps.cpp)
target_link_libraries(subscriber_gps ${catkin_LIBRARIES})
add_dependencies(subscriber_gps ${PROJECT_NAME}_generate_messages_cpp) 


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

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

3-4 编译工程

在大工程catkin_gps下编译

catkin_make

　　

 

 

 

4运行工程

4-1 运行ros初始化

随意路径运行，必须单独运行在一个窗口

　　

roscore

　　

 

 

4-2 在命令行注册项目地址，使得命令行可以找到要执行的ros节点

 

 

source /home/dongdong/v2_Project/v4_ROS/catkin_gps/devel/setup.bash

　　

4-3 运行节点

rosrun 项目名字 编译节点

运行发布节点

rosrun v1_GetGPS serialPort

　　

 

 

 

 

 

 运行接受节点

 

 

 

 

使用脚本一次性执行

https://www.cnblogs.com/gooutlook/p/16495002.html

创建脚本

gedit ros_run_gps.sh

　　

 

 

#!/bin/sh


#延迟1秒执行
sleep 1
  
echo "ROS——GPS 测试"
#echo "GPS 测试开始，消息记录到日志里" > /home/pi/start/test_desk1.log


echo "1 开启ros节点 roscore 等待完全开启再往后执行 "
gnome-terminal -t "1_roscore" -x bash -c "\
roscore; \
exit;exec bash;"
sleep 5


echo "2 开启发送节点 serialPort "
gnome-terminal -t "2_serialPort" -x bash -c "\
echo \"dongdong\" | sudo chmod 777 /dev/ttyUSB0; \ 
source /home/dongdong/v2_Project/v4_ROS/catkin_gps/devel/setup.bash; \
rosrun v1_GetGPS serialPort; \
exit;exec bash;"
sleep 1

echo "3 开启接收节点 listener"

gnome-terminal -t "3_listener" -x bash -c "\   
source /home/dongdong/v2_Project/v4_ROS/catkin_gps/devel/setup.bash; \
rosrun v1_GetGPS listener; \
exit;exec bash;"
sleep 1


#echo "执行前确保给与脚本本身执行权限 sudo chmod -R 777 xxx.sh"
#echo "查看串口： ls /dev/ttyUSB* "
#echo "临时给与一次串口权限： sudo chmod 777 /dev/ttyUSB0 "
# 开启新的命令窗口执行
# gnome-terminal -t "窗口名字" -x bash -c "要执行的命令1；命令2；exit;exec bash;"
#Shell 脚本自动输入密码 : echo "密码" | sudo 命令

　　 给与脚本执行权限

sudo chmod -R 777 ros_run_gps.sh

　　

 

 

运行脚本

 

 

 

结果

 主窗口

 

 窗口1

逐个启动比较慢，所以要等待一会在执行后续的ROS脚本 

 

统计后1秒10次 符合GPS的数据输出

窗口2 

 

 手动输入密码

 

 

 

 

窗口3 

 

 

 

 

其他代码

1例子-追加了保存Txt的发送节点

1-1C++ 保存txt文本文件

第一步：包含头文件#include <fstream>。

#include <iostream>
#include <fstream>
using namespace std;

第二步：以覆盖的方式保存文件。

#include <iostream>
#include <fstream>
using namespace std;
int main()
{
	ofstream out("out.txt");
	if (out.is_open())
	{
		out << "This is a line.\n";
		out << "This is another line.\n";
		out.close();
	}
	system("pause");
	return 0;
}

第三步：以追加的方式保存文件 ios::app。


#include <iostream>
#include <fstream>
using namespace std;
int main()
{
	ofstream out("out.txt", ios::app);
	if (out.is_open())
	{
		out << "This is a line.\n";
		out << "This is another line.\n";
		out.close();
	}
	system("pause");
	return 0;
}

　　1-2 代码加入主工程

 

 

 

serialPort_saveTxt.cpp

#include <string>
#include <vector>
#include <sstream>
#include <cmath>
#include <cstdlib>//string转化为double
#include <iomanip>//保留有效小数

//txt保存使用
#include <fstream>
#include <iostream>

//ros依赖
#include <ros/ros.h> 
#include <serial/serial.h>  //ROS已经内置了的串口包 

#include "ros/time.h"// 时间戳

//0 导入消息 工程名字/消息名字
#include "v1_GetGPS/topic_msg.h"
#include "std_msgs/String.h"
#include <std_msgs/Empty.h> 

serial::Serial ser; //声明串口对象
using namespace std;


//解析GPS
void RecePro(std::string s , double& lat , double& lon, double& high )
{
    //分割有效数据，存入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 ));
    //解析出经纬度
    //字段6：GPS状态，0=未定位，1=非差分定位，2=差分定位，3=无效PPS，6=正在估算
    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;
        //海拔高度
        if (v[9] != "") high = std::atof(v[9].c_str()) ;


    }
}
int main(int argc, char** argv)
{
    //保存TXT使用
    string  outFileName = "out.txt";
    //ofstream outfile(outFileName, ios::app); //ios::app指追加写入
    ofstream outfile(outFileName); //覆盖方式写入
    if (!outfile.is_open())
    {
        cerr << "Error: can not find file " << outFileName << endl;
        return 0;
    }

    //初始化节点
    ros::init(argc, argv, "serial_node");
    //声明节点句柄
    ros::NodeHandle nh;
    //注册Publisher到话题GPS
    ros::Publisher GPS_pub = nh.advertise<v1_GetGPS::topic_msg>("GPS",1000);
    try
    {
      //串口设置
      ser.setPort("/dev/ttyUSB0");
      ser.setBaudrate(115200);
      serial::Timeout to = serial::Timeout::simpleTimeout(5000);
      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;

                        //ros::Time begin = ros::Time::now();
                        double secs =ros::Time::now().toSec();

                        //把有效的数据给解析的函数以获取经纬度
                        double lat, lon, high;
                        RecePro(strRece.substr(start,end+2-start),lat,lon,high);
                        //保留位数7位
                        std::cout << std::setiosflags(std::ios::fixed)<<std::setprecision(7)<< "纬度：" << lat << " 经度："<< lon <<" 高度： "<< high <<"   时间:  " << secs << "\n";
                        
                        if (lat==0 || lon==0 || high==0)
                        {

                        }
                        else{
                            std::string secs_str= std::to_string(secs) ;
                            std::string lat_str= std::to_string(lat) ;
                            std::string lon_str= std::to_string(lon) ;
                            std::string high_str= std::to_string(high) ;
                            // 
                            //#include <sstream>
                            //std::stringstream ss;
                            //ss<< std::setprecision(15) << secs;
                            //std::string secs_str=ss.str();   
                            outfile  << secs_str << " "<<lat_str <<" "<< lon_str <<" " << high_str<<" " << std::endl;//写TXT文件
                        }
                        //发布消息到话题
                        v1_GetGPS::topic_msg GPS_data;
                        GPS_data.lat = lat;
                        GPS_data.lon = lon;
                        GPS_data.high = high;
                        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();
    }

    outfile.close();// txt文件关闭
    std::cout<< "采集程序关闭" <<std::endl;
    return 1;
}