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