#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);//相当于带时间戳的printROS_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);//设置Timeoutsp.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说明: