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++代码片段:
std::string gstart = "$GN";
std::string gend = "\r\n";
int i = 0, start = -1, end = -1;
while ( i < strRece.length() )
{
start = strRece.find(gstart);
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);
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 )
{
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>
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>
#include <string>
#include <vector>
#include <sstream>
#include <cmath>
#include <cstdlib>
#include <iomanip>
#include "serialPort/GPS.h"
serial::Serial ser;
void RecePro(std::string s , double& lat , double& lon )
{
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;
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())
{
ROS_INFO_STREAM("Reading from serial port\n");
strRece += ser.read(ser.available());
std::string gstart = "$GN";
std::string gend = "\r\n";
int i = 0, start = -1, end = -1;
while ( i < strRece.length() )
{
start = strRece.find(gstart);
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);
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>
<maintainer email="bingo@todo.todo">bingo</maintainer>
<license>TODO</license>
<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>
<export>
</export>
</package>
8. 运行测试
roscore后,分别在两个终端中运行serialPort和listener两个节点:
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)