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

 

 

参考资料

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

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

 

安装 ros串口库

?

1	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路径下编译

?

1	catkin_make

　　

 

 

 自动生成

 

 

 

 2 创建项目工程

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

 

 

 在v1_GetGPS下创建文件.

 

2-1创建消息文件

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

 

 

 

 

 

 

 

?

1 2 3 4 5 6

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

　　

2-2 创建编辑package.xml　

 

 

 包含文件依赖

1 自定义消息

2 串口

?

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43

<?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

 

 

 

?

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59

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数据

?

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18

从串口助手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

 

?

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26

#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编译节点

 

 

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

?

1 2 3 4 5 6 7

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 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59

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下编译

?

1	catkin_make

　　

 

 

 

4运行工程

4-1 运行ros初始化

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

　　

?

1

roscore

　　

 

 

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

 

 

?

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

　　

4-3 运行节点

rosrun 项目名字 编译节点

运行发布节点

?

1	rosrun v1_GetGPS serialPort

　　

 

 

 

 

 

 运行接受节点

 

 

 

 

使用脚本一次性执行

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

创建脚本

?

1	gedit ros_run_gps.sh

　　

 

 

?

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40

#!/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 命令

　　 给与脚本执行权限

?

1	sudo chmod -R 777 ros_run_gps.sh

　　

 

 

运行脚本

 

 

 

结果

 主窗口

 

 窗口1

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

 

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

窗口2 

 

 手动输入密码

 

 

 

 

窗口3 

 

 

 

 

其他代码

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

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

?

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42

第一步：包含头文件#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

?

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207

#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; }

　　

　　

 

 

 

 

 

 

　　

 

 

分类: 3_2 ROS