#include <iostream>
#include <ros/ros.h>
#include <serial/serial.h>
#include<geometry_msgs/Twist.h>
using namespace std;

//运行打开速度控制插件：  rosrun rqt_robot_steering rqt_robot_steering
//若串口访问权限不够：    sudo chmod 777 /dev/ttyTHS1

//发送和接受数组
uint8_t  senddata[5] = {0x01, 0x02, 0x03, 0x04,0x05};
uint8_t  recdata[100] = {0};

//回调函数
void rc_cmdvel_callback(geometry_msgs::Twist vel_msg)
{
    ROS_INFO("lx:%.2f",vel_msg.linear.x);//相当于带时间戳的print
    ROS_INFO("ly:%.2f",vel_msg.linear.y);
    ROS_INFO("lz:%.2f",vel_msg.linear.z);
    ROS_INFO("ax:%.2f",vel_msg.angular.x);
    ROS_INFO("ay:%.2f",vel_msg.angular.y);
    ROS_INFO("az:%.2f",vel_msg.angular.z);
    ROS_INFO("\n");
}

int main(int argc,  char** argv) {
   
    ros::init(argc,argv,"serial_node");//初始化ROS节点
    //*********************************话题订阅配置*************************************
    ros::NodeHandle nh;
    //
	ros::Subscriber sub=nh.subscribe("/cmd_vel",5,rc_cmdvel_callback);//订阅者1       >订阅abc_test话题      缓冲区长度5   回调函数为rc_callback1
    //ros::Subscriber sub2=nh.subscribe("s2_test",5,rc_callback2); //订阅者2    订阅abc2_test话题



//*****************************************串口配置***********************************
    serial::Serial sp;//定义串口对象
    try{
            sp.setPort("/dev/ttyTHS1");//设置串口名称
            sp.setBaudrate(9600);//波特率
            sp.setBytesize(serial::bytesize_t::eightbits);//数据位--8位
            sp.setParity(serial::parity_t::parity_none);//校验位--无
            sp.setStopbits(serial::stopbits_t::stopbits_one);//停止位--1位
            sp.setFlowcontrol(serial::flowcontrol_t::flowcontrol_none);//无串口流控
            
            serial::Timeout to=serial::Timeout::simpleTimeout(1000);
            sp.setTimeout(to);//设置Timeout

            sp.open();//打开串口
    }
    catch(serial::IOException& e){
            ROS_ERROR_STREAM("Unable to open port.");
            return -1;
    }

    if(sp.isOpen())//判断串口是否打开
    {
        ROS_INFO_STREAM("Com is initialized");
    }
    else
    {
        ROS_ERROR_STREAM("Unable to open port.");
        return -1;
    }
    //******************************************************************************************

    ros::Rate loop_rate(1);
    while(ros::ok()){
        //******************************************串口相关********************************************
    	//发送数据
    	sp.write(senddata,5);//将u8类型的数组的前x个数据写入缓冲区
        //获取缓存区数据

        size_t sn=sp.available();//获取缓冲区字节数
        if(sbrk != 0)//如果缓冲区有字节
        {
            sn = sp.read(recdata, sn);//从缓冲区读n个字节到u8数组中
            for(int i = 0; i < sn; i++)
            {
            //16进制显示接受到的数据
                //cout << std::hex << (recdata[i] & 0xff) << " ";
                 ROS_INFO("%d",recdata[i] & 0xff );
            }
            
        }
        //*******************************************ROS话题相关*************************************


        ros::spinOnce();//监听消息包
		//	以1HZ的频率
        loop_rate.sleep();
    }
    sp.close();
    return 0;
}

常用API说明：