ROS系统基础知识梳理(四) 串口通信

ROS系统基础知识梳理(四) 串口通信

学习ROS系统，初步接触到ROS系统外接传感器，传感器通过Uart通信向台式机发送数据，内容涉及到ROS调用串口数据、串口数据校验、以及欧拉角转换四元数。
任务系统：Ubuntu18.04LTS
ROS Melodic
传感器：WT61c（加速度传感器、6轴陀螺仪）

类声明

针对要处理的数据定义类

#ifndef WT61CUART_H
#define WT61CUART_H
#include <vector>
#include <math.h>
#include <time.h>
#include "ros/ros.h"
#include "serial/serial.h"
#include "std_msgs/String.h"
#include "std_msgs/Empty.h"
#include "sensor_msgs/Imu.h"
#include "geometry_msgs/Twist.h"
#include "tf/LinearMath/Quaternion.h"
#define PI 3.14159
namespace WTU {
	class Wt61cUart {                       //整个类基于serial封装,整理出了ROS常用接口
	private:                                //包括波特率/uart端口号/ROS系统内的话题名称等
		int baudrate_,index_;              //index_初始化置零,用作变长数组索引
		std::string com_;
		serial::Serial ser;                //serial类,实现对串口控制的核心操作
		double g_;
		std::vector<uint8_t> UartData_;
		std::string topic_pub_;
		ros::Publisher wt61c_pub_;
		ros::Publisher wt61c_turtle_;
	public:
		Wt61cUart(ros::NodeHandle&);       
		~Wt61cUart();
		int TranslateAndPub();
		int UartInit();                   //串口初始化,按照launch文件上传的参数初始化串口
		int GetAndCheck();         //校验函数
	};
}
#endif

ROS调用串口

ROS系统有专门用于调用串口进行通信的数据包：serial。
serial::Serial Class Reference：docs.ros.org/jade/api/serial/html/classserial_1_1Serial.html
利用serial命名空间下的Serial类可实现ros对于串口的调用。

//串口初始化程序
int WTU::Wt61cUart::UartInit() {
	try{
		ser.setPort(com_);           //按照launch文件设置的端口号和波特率初始化串口
		ser.setBaudrate(baudrate_);
		serial::Timeout to = serial::Timeout::simpleTimeout(1000);  //设置截止时间
		ser.setTimeout(to);
		ser.open();                         
	}
	catch(serial::IOException& e){
		ROS_ERROR_STREAM("Unable to open port. Please try again.");
		return -1;
	}
	if(ser.isOpen()) {
		//ser.flushInput();
		ROS_INFO_STREAM("The port initialize succeed.");
		ser.flushInput();      //清空输入缓存,把多余的无用数据删除
		sleep(0.1);            //延时0.1秒,确保有数据进入
		return 0;
	}
	else
		return -1;
}

数据读取与校验

ROS的串口读取用serial类中的read()函数来实现,相对于程序执行速度来讲读取速度略慢,所以按字节读取易产生bug,利用available()返回值做参数,可一次性读取串口buffer里面的全部数据,程序执行理论上会快一些.
以available()做为参数,将buffer内容全部读取到内存中来,然后按位判断包头位置,所余数据不足一包时再次读取buffer中的数据,然后进行校验.

int WTU::Wt61cUart::GetAndCheck() {
	int i,j;      //校验计数位
	int sum = 0x55;      //数据和,初始化为0x55.wt61c发送数据包包头为0x55
	while(UartData_.size()-index_<33){    //当数据校验成功并且解析发送之后,index_再次置零并且删除已处理的数据.
		while(ser.available()<33){ROS_INFO("wait");}   //保证读取大于33byte数据,即一个完整的数据包
		ser.read(UartData_,ser.available());
	}
	while(true){
		if(UartData_[index_] ==0x55 && UartData_[index_+1] ==0x51){
			//检测到index_指向的数据为0x55,并且第二个数据为0x51,此时,大概率检测到数据包头
			for (i= 1; i<10; i++)
				sum+= UartData_[index_+i];         //和校验
			if(UartData_[index_+10] == sum%0x100)
				j = 1;
			sum = 0x55;
			for (i= 12; i<21; i++)
				sum+= UartData_[index_+i];
			if(UartData_[index_+21] == sum%0x100)
				j++;
			sum = 0x55;
			for (i= 23; i<32; i++)
				sum+= UartData_[index_+i];
			if(UartData_[index_+32] == sum%0x100)
				j++;
			if (j = 3){           //校验位通过
				ROS_INFO("Yes,I got a complete package.");
				return 0;
			}
			else{               //校验位未通过,则表明index_ 指向的并非包头,index_自增,进行下一次循环检测
				sum = 0x55;
				index_++;
			}
		}
		else
			index_++;
		while(UartData_.size()-index_-32<33){   //若剩余数据不足33,即一个包的大小,则补入数据
			while(ser.available()<33){}
			ser.read(UartData_,ser.available());
		}
	}	
}

欧拉角转四元数

欧拉角转化四元数,ROS中的TF包中函数可以实现
头文件:#include "tf/LinearMath/Quaternion.h"
函数:tf::Quaternion::setRPY()
GitHub:wt61c_uart