ROS获取串口信息及后续处理（以惯导IMU XW-GI5651为例）

一、问题简介

自动驾驶小车的底层的数据相当一部分是通过串口发送的，以惯导为例，惯导的定位信息大概如下所示：

$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获取串口数据信息

1. 下载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节点可以收到串口的信息，在终端上可以显示，但是串口信息在局部的回调函数上，要想真正拿到串口信息，还要进一步处理，本文列举了部分方法

1. 采用全局变量的方法

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

2. 使用类方法作为回调函数

代码来自：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;
}