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="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>
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>//保留有效小数
#include <termio.h>
#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
//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>
#include <ctime>
serial::Serial ser; //声明串口对象
using namespace std;
#define TTY_PATH "/dev/tty"
#define STTY_US "stty raw -echo -F "
#define STTY_DEF "stty -raw echo -F "
int get_char();
int get_char()
{
fd_set rfds;
struct timeval tv;
int ch = 0;
FD_ZERO(&rfds);
FD_SET(0, &rfds);
tv.tv_sec = 0;
tv.tv_usec = 10; //设置等待超时时间
//检测键盘是否有输入
if (select(1, &rfds, NULL, NULL, &tv) > 0){
ch = getchar();
}
return ch;
}
//解析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,double& hacc ,double& vacc ,double& state )
{
//分割有效数据，存入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 )
{
//纬度
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()) ;
//GPS状态 G3有数据精度还可以
//if (v[8] == "G3" || (v[8] == "D3") {state=1;}
//else{state=0;}
state=1; //解析正常
//水平误差 m
if (v[9] != "") hacc=std::atof(v[9].c_str()) ;
// 垂直误差 m
if (v[10] != "") vacc=std::atof(v[10].c_str()) ;
}
else{
state=0;
}
}
//1 txt文件名名字 时间戳
//2 ROS发送 开关参数化
//3 加入hacc 和 vacc
int main(int argc, char** argv)
{
bool sendRos=1;
double hacc_range=5; //水平误差阈值
double vacc_range=5; //垂直误差阈值
string save_name=argv[1];
cout<< save_name <<endl;
//获取当前时间保持到字符串中
time_t t = time(NULL);
struct tm local_tm;
struct tm *tm = localtime_r(&t, &local_tm);
char cNowTime[64];
sprintf(cNowTime, "%02d-%02d-%02d-%02d-%02d-%02d",
tm->tm_year + 1900,tm->tm_mon + 1,tm->tm_mday,
tm->tm_hour, tm->tm_min, tm->tm_sec);
//保存TXT使用
string outFileName = save_name + cNowTime +".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(20);
std::string strRece;
//捕获按键
int ch = 0;
system(STTY_US TTY_PATH);
while (ros::ok())
{
//捕获按键
ch = get_char();
if(ch != 0){
printf("%d\n\r",ch);
if(ch == 27){
outfile.close();// txt文件关闭
std::cout<< "采集程序关闭" <<std::endl;
break;
}
}
if(ch == 3){
system(STTY_DEF TTY_PATH);
return 0;
}
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,hacc,vacc,state;
//std::cout<< "2==========="<<strRece.substr(start,end+2-start) << std::endl;
RecePro(strRece.substr(start,end+2-start),lat,lon,high,hacc,vacc,state);
//保留位数7位
std::cout << std::setiosflags(std::ios::fixed) <<std::setprecision(7)<< " 时间: " << secs << " 更新状态： "<< state <<" 纬度：" << lat << " 经度："<< lon <<" 高度： "<< high << " 垂直误差: "<< hacc << " 水平误差: " <<vacc << "\n";
//状态1 且垂直和水平误差小于x米 保存
if(state==1 && hacc<hacc_range && vacc<vacc_range){
outfile << std::setiosflags(std::ios::fixed) << std::setprecision(8) << secs << " "<<lat <<" "<< lon <<" " << high<<" "<< hacc << " "<< vacc<< " "<< std::endl;//写TXT文件
if(sendRos){
//发布消息到话题
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;
}
}
}
}
}
if(sendRos){
ros::spinOnce();
loop_rate.sleep();
}
}//while
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 命令
浙公网安备 33010602011771号