GPS开发（2）GPS ROS串口采集-pubx数据解析

标签：std ROS ros add 串口 msg include pubx GPS

 

 

pubx协议数据解析

 

 

 

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) 



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


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

　　

package.xml 

 

 

<?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="[email protected]">Jane Doe</maintainer> -->
  <maintainer email="[email protected]">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>

　　

msg/topic_msg.msg

 

 

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

　　

src/serialPort_saveTxt_v2.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
// 0-PUBX 1-00 2-timesharp 3-lat 4-N 5-lon 6-E 7-high 8-G3 9-Hacc 10-Vacc 
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=DeadReckonimgslolution，2=标准独立2D，3=标准独立3D，4=差分2D 5-差分3D 6-联合GPS和DeadReckonimgslolution
    if (v.max_size() >= 11 && (v[8] == "G3"))
    {
        //纬度
        if (v[3] != "") lat = std::atof(v[3].c_str()) / 100;
        int ilat = (int)floor(lat) % 100;
        lat = ilat + (lat - ilat) * 100 / 60;
        //经度
        if (v[5] != "") lon = std::atof(v[5].c_str()) / 100;
        int ilon = (int)floor(lon) % 1000;
        lon = ilon + (lon - ilon) * 100 / 60;
        //海拔高度
        if (v[7] != "") high = std::atof(v[7].c_str()) ;


    }
}


//1 txt文件名名字 时间戳
//2 ROS发送 开关参数化
//3 加入hacc 和  vacc

int main(int argc, char** argv)
{   

    string save_name=argv[1];
    cout<< save_name <<endl;
    //保存TXT使用
    string  outFileName = save_name+".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/ttyACM0");
      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<< "1============"<<strRece << std::endl;
            //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 = "$PU";
            //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;
                        std::cout<< "2==========="<<strRece.substr(start,end+2-start) << std::endl;
                        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";
                        
                        outfile  << secs << " "<<lat <<" "<< lon <<" " << high<<" " << 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;
}

　　subscriber_gps.cpp

 

 

 

 

 

#include <cstdlib>
#include <iostream>
#include <string>
#include <cstring>

//0 导入ros依赖
#include <ros/ros.h>

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


// 接收到订阅的消息后，会进入消息回调函数
void test_topic_cb(const v1_GetGPS::topic_msg::ConstPtr & msg)
{
    // 将接收到的消息打印出来
    ROS_INFO("------------------ msg ---------------------");
    ROS_INFO("bool_msg:    [%d]", msg->bool_msg);
    ROS_INFO("string_msg:  [%s]", msg->string_msg.c_str());
    ROS_INFO("float32_msg: [%f]", msg->lat);
    ROS_INFO("float64_msg: [%f]", msg->lon);
    ROS_INFO("time_msg:    [%d.%d]", msg->time_msg.sec, msg->time_msg.nsec);
}


int main(int argc, char **argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "subscriber_gps");

    // 创建节点句柄
    ros::NodeHandle n;

    // 创建一个Subscriber，订阅名为my_topic_msg的消息，注册回调函数test_topic_cb
    ros::Subscriber sub_topic = n.subscribe<v1_GetGPS::topic_msg>("my_topic_msg", 100, test_topic_cb);

    // 循环等待回调函数
    ros::spin();

    return 0;
}

　　

 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

# /dev/ttyACM0
# /dev/ttyUSB0
echo "2 开启发送节点 serialPort "
gnome-terminal -t "2_serialPort" -x bash -c "\
echo \"dongdong\" | sudo chmod 777 /dev/ttyACM0; \ 
source /home/dongdong/v2_Project/v4_ROS/catkin_gps/devel/setup.bash; \
rosrun v1_GetGPS serialPort_saveTxt savatxtname; \
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 命令

　　

 

 

 

标签：std,ROS,ros,add,串口,msg,include,pubx,GPS
From： https://www.cnblogs.com/gooutlook/p/16986578.html