#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說明：
)
