一、问题简介
自动驾驶小车的底层的数据相当一部分是通过串口发送的,以惯导为例,惯导的定位信息大概如下所示:
$GPFPD,0,1666.330,0.000,0.015,0.129,0.00000000,0.00000000,0.000,0.000,0.000,0.000,0.000,0,0,00*4E
报文中包含着丰富的车辆状态信息,通过ROS节点,拿到这些数据,并进行处理在自动驾驶中是非常重要的,本文主要介绍了利用ROS获取串口信息的方式。
点击跳至原贴。
二、ROS获取串口数据信息
- 下载serial包
sudo apt-get install ros-kinetic-serial #ros为Kinect版本
2.编写ros节点cpp文件
#include <ros/ros.h>
#include <serial/serial.h> //ROS已经内置了的串口包
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>
serial::Serial ser; //声明串口对象
//回调函数
void write_callback(const std_msgs::String::ConstPtr& msg)
{
ROS_INFO_STREAM("Writing to serial port" <<msg->data);
ser.write(msg->data); //发送串口数据
}
int main (int argc, char** argv)
{
//初始化节点
ros::init(argc, argv, "serial_example_node");
//声明节点句柄
ros::NodeHandle nh;
//订阅主题,并配置回调函数 ,此处未使用,后续通过串口发送数据可以进行配置
ros::Subscriber write_sub = nh.subscribe("write", 1000, write_callback);
//发布主题 ,主题为“read”
ros::Publisher read_pub = nh.advertise<std_msgs::String>("read", 1000);
try
{
//设置串口属性,并打开串口
ser.setPort("/dev/ttyS0"); //此处需要根据惯导接的串口来进行修改。
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 port ");
return -1;
}
//检测串口是否已经打开,并给出提示信息
if(ser.isOpen())
{
ROS_INFO_STREAM("Serial Port initialized");
}
else
{
return -1;
}
//指定循环的频率
ros::Rate loop_rate(10);
while(ros::ok())
{
if(ser.available()){
ROS_INFO_STREAM("Reading from serial port\n");
std_msgs::String result;
result.data = ser.read(ser.available());
ROS_INFO_STREAM("Read: " << result.data); //将接收到的信息打印在终端上
read_pub.publish(result);//发送话题数据
}
//处理ROS的信息,比如订阅消息,并调用回调函数
ros::spinOnce();
loop_rate.sleep();
}
} //别忘了添加serial依赖,否则编译失败
串口信息的处理
在通过上面的ROS节点可以收到串口的信息,在终端上可以显示,但是串口信息在局部的回调函数上,要想真正拿到串口信息,还要进一步处理,本文列举了部分方法
- 采用全局变量的方法
#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sstream>//包含流的操作的C++头文件
std::string myStr="init";//定义全局变量并进行初始化
void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
std::stringstream ss;
ss << msg->data.c_str();
ss >> ::myStr;//通过流操作进行传参。此处不能用=,报错显示=没有被重装载。该全局变量就可以在main()中使用了。
std::cout <<"copy_data is: " << myStr <<"\n";
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "listener");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("read", 1000, chatterCallback);
ros::spin();
return 0;
}
- 使用类方法作为回调函数
代码来自:https://blog.csdn.net/xinwenfei/article/details/89384578
http://wiki.ros.org/roscpp_tutorials/Tutorials/UsingClassMethodsAsCallbacks
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <sstream>
/**
* This tutorial demonstrates subscribing to a topic using a class method as the callback.
*/
// %Tag(CLASS_WITH_DECLARATION)%
class Listener
{
public:
std::string copy_data = "init init init";
int count = 0;
public:
void callback(const std_msgs::String::ConstPtr& msg);
void print_data2(){std::cout << "Copy data is :" << copy_data << "\n";}
};
// %EndTag(CLASS_WITH_DECLARATION)%
void Listener::callback(const std_msgs::String::ConstPtr& msg)
{
ROS_INFO("I heard: [%s]", msg->data.c_str());
std::stringstream ss;
ss << msg->data.c_str();
ss >> copy_data;
std::cout <<"copy_data is: " << copy_data <<"\n";
print_data2();
++count;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "listener_class");
ros::NodeHandle n;
// %Tag(SUBSCRIBER)%
Listener listener;
ros::Subscriber sub = n.subscribe("chatter", 1000, &Listener::callback, &listener);
ros::Rate loop_rate(10);
// %EndTag(SUBSCRIBER)%
while(ros::ok() and listener.count <=3){
ros::spinOnce();
loop_rate.sleep();
}
std::cout << "After spin: \n";
listener.print_data2();
return 0;
}