[Ros] Read serial port data

Sometimes, some devices send data through the serial port, and want to read the serial port data in the ros, and record the operation:

1. Customize gnrmc.msg

First you need to define it yourself gnrmc.msg

int64 stamp
string header
float64 utc
string pos_status
float64 lat
string latdir
float64 lon
string londir
float64 speed
float64 track_ture
string date 
float64 mag_var
float64 vardir
string mode_ind

2. Code

This part of the code is ok to read other serial data afterwards in theory, just change it slightly.

#include <ros/ros.h>
#include <ros/timer.h>
#include <stdio.h>
#include <math.h>
#include <serial/serial.h>  //ROS已经内置了的串口包
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>
//#include <serial_port/data.h>
#include <common/gnrmc.h>

#include <fstream>
#include <string>
#include <iostream>
#include <vector>
#include <sstream>

#include <inttypes.h>
#include <Eigen/Eigen>

#include <iostream>
#include <fstream>

using namespace std;
using namespace ros;
using namespace Eigen;

serial::Serial ser; //声明串口对象
//serial_port::data Serial_Data;

common::gnrmc GnrmcMsg;

string buffer="";

//数据类型转换模板函数
template <class Type>
Type stringToNum(const string str)
{
    
    
    istringstream iss(str);
    Type num;
    iss >> num;
    return num;
}

int main (int argc, char** argv)
{
    
    

    ros::init(argc, argv, "GNRMCInfo");

    ros::NodeHandle nh;

    ros::Publisher GnrmcInfo_pub = nh.advertise<common::gnrmc>("GNRMCInfo", 1000);


    try
    {
    
    
    //设置串口属性，并打开串口
        // ser.setPort("/dev/ttyS0");
        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 port ");
        return -1;
    }

    //检测串口是否已经打开，并给出提示信息
    if(ser.isOpen())
    {
    
    
        ROS_INFO_STREAM("Serial Port initialized");                
    }
    else
    {
    
    
        return -1;
    }


    //指定循环的频率50Hz
    ros::Rate loop_rate(50);
    while(ros::ok())
    {
    
    

        if(ser.available())
        {
    
    
            int another_msg_flag=0;

            string line;
            string Msg_temp;

           
            Msg_temp = ser.read(ser.available());
            buffer += Msg_temp;

            if(buffer.length()>1000)
            {
    
    
                buffer="";
                cout<<"so much!"<<endl;
                continue;
            }

            // if(buffer.length()<130)
            // {
    
    
            //     //continue;
            // }
            
            int mid=0;
            string s="\r\n";
            while((mid=buffer.find(s))!=-1)
            {
    
    
                //cout<<"process!"<<endl;


                line=buffer.substr(0,mid);
                buffer.erase(0,mid+2); //将已读取的数据删去

                string temp_s;
                int temp_position;

                temp_position = line.find(",");
                // vector<string> arr;  
                // int position = 0;

                temp_s = line.substr(0,temp_position);

                if (temp_s == "$GNRMC")
                {
    
    
                    temp_position = line.find(",");
                    temp_s = line.substr(0,temp_position);
                    line.erase(0,temp_position+1);
                    GnrmcMsg.header = temp_s;
                    vector<string> temp_array;

                    do
                    {
    
    
                        temp_position = line.find(",");
                        temp_s = line.substr(0,temp_position);
                        line.erase(0,temp_position+1); //将已读取的数据删去
                        temp_array.push_back(temp_s);   //将字符串压入容器中
                    }while(temp_position != -1);

                    GnrmcMsg.utc = stringToNum<double>(temp_array[0]);  
                    GnrmcMsg.pos_status = temp_array[1];
                    GnrmcMsg.lat = stringToNum<double>(temp_array[2]); 
                    GnrmcMsg.latdir = temp_array[3];
                    GnrmcMsg.lon = stringToNum<double>(temp_array[4]); 
                    GnrmcMsg.londir = temp_array[5];
                    GnrmcMsg.speed = stringToNum<double>(temp_array[6]); 
                    GnrmcMsg.track_ture = stringToNum<double>(temp_array[7]);
                    GnrmcMsg.date = temp_array[8];
                    GnrmcMsg.mag_var = stringToNum<double>(temp_array[9]); 
                    GnrmcMsg.vardir = stringToNum<double>(temp_array[10]); 
                    GnrmcMsg.mode_ind = temp_array[11];

                    GnrmcMsg.stamp = int64_t((ros::Time::now().toSec())*1000.0);
                    GnrmcInfo_pub.publish(GnrmcMsg);

                } 

            }

        }

        //处理ROS的信息，比如订阅消息,并调用回调函数
        ros::spinOnce();
        loop_rate.sleep();

    }
}

3. Results

4. Precautions

Serial port should be given permission

chmod 777 /dev/ttyS*