ROS串口通信(2)以十六进制指令读取IMU数据
引言
前期准备参考博客
1、下载安装ROS的serial软件包
sudo apt-get install ros-kinetic-serial #ros为Kinect版本
进入下载的软件包的位置
roscd serial
安装成功:
opt/ros/kinetic/share/serial
这个路径也是ROS的其他库文件路径,很重要,经常会用到。相关头文件路径在opt/ros/kinetic/include里面,相关源文件在opt/ros/kinetic/share里面。比如要查看imu.msg,则源文件路径为/opt/ros/kinetic/include/sensor_msgs。
头文件或者说是消息格式路径在/opt/ros/kinetic/share/sensor_msgs/msg下面。
# This is a message to hold data from an IMU (Inertial Measurement Unit)
#
# Accelerations should be in m/s^2 (not in g's), and rotational velocity should be in rad/sec
#
# If the covariance of the measurement is known, it should be filled in (if all you know is the
# variance of each measurement, e.g. from the datasheet, just put those along the diagonal)
# A covariance matrix of all zeros will be interpreted as "covariance unknown", and to use the
# data a covariance will have to be assumed or gotten from some other source
#
# If you have no estimate for one of the data elements (e.g. your IMU doesn't produce an orientation
# estimate), please set element 0 of the associated covariance matrix to -1
# If you are interpreting this message, please check for a value of -1 in the first element of each
# covariance matrix, and disregard the associated estimate.
Header header
geometry_msgs/Quaternion orientation
float64[9] orientation_covariance # Row major about x, y, z axes
geometry_msgs/Vector3 angular_velocity
float64[9] angular_velocity_covariance # Row major about x, y, z axes
geometry_msgs/Vector3 linear_acceleration
float64[9] linear_acceleration_covariance # Row major x, y z
2、通用serial通信代码
#include <ros/ros.h>
#include <serial/serial.h> //ROS已经内置了的串口包
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>
serial::Serial ser; //声明串口对象
//回调函数
void write_callback(const std_msgs::String::ConstPtr& msg)
{
ROS_INFO_STREAM("Writing to serial port" <<msg->data);
ser.write(msg->data); //发送串口数据
}
int main (int argc, char** argv)
{
//初始化节点
ros::init(argc, argv, "serial_example_node");
//声明节点句柄
ros::NodeHandle nh;
//订阅主题,并配置回调函数
ros::Subscriber write_sub = nh.subscribe("write", 1000, write_callback);
//发布主题
ros::Publisher read_pub = nh.advertise<std_msgs::String>("read", 1000);
try
{
//设置串口属性,并打开串口
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;
}
//指定循环的频率
ros::Rate loop_rate(50);
while(ros::ok())
{
if(ser.available()){
ROS_INFO_STREAM("Reading from serial port\n");
std_msgs::String result;
result.data = ser.read(ser.available());
ROS_INFO_STREAM("Read: " << result.data);
read_pub.publish(result);
}
//处理ROS的信息,比如订阅消息,并调用回调函数
ros::spinOnce();
loop_rate.sleep();
}
}
此部分主要参考文章.该文章主要实现的功能:
3、使用ROS serial包实现IMU十六进制指令发送及数据读取编解码保存。
在一开始,ROS serial不支持直接发送十六进制指令尝试了一下更改,网上大多数资料都是直接通过串口进行数据读取,没有指令发送的环节即是很少有实现通过serial.write()函数进行指令发送,大多的应用环境只需要接收数据即可。后来参考这篇博客,此文虽然实现了十六进制的通信,但是实现过程是自己定义了一个消息类型通过topic转换还是怎么的过后就能够使用serial.write()函数进行指令发送,然后我在这上面浪费了些时间,后来查到资料说将指令转换为Unicode编码可以使用serial.write()函数进行发送,Unicode编码相当于是一个底层的通信格式,也进行了尝试,实现了Unicode的转换但是依然不能发送,原因可能是我用的C++,而资料里面是使用的python,后来也就放弃了该方法。回归到最初进行思考,发现自己真的很傻,根本不需要如此麻烦,也不需要像参考博客那样进行自定义消息类型然后进行发送,虽然我是在复现了他的机制后才醒悟的,果断改了。至于是怎样实现的直接上代码吧。
ImuCommand.h
/************************************************************************************************************
* Copyright Notice
* Copyright (c) 2018,冯****
*
* @File : ImuCommand.h
* @Brief : IMu指令
*
* @Version : V1.0
* @Date : 2018/11/23
* @History :
* Author : 冯****
* Descrip: 命令字节,由外部控制器发送至GY953,帧头0xa5,指令格式:帧头+指令+校验和
*************************************************************************************************************/
#ifndef IMUCOMMAND_H
#define IMUCOMMAND_H
#include <iostream>
#include "serial/serial.h"
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>
#include "serial_msgs/serial.h"
#define READ_BUFFERSIZE 13 //读取数据数组长度
#define cmd_num 3 //指令数组长度
#define DATA_LENGTH 3 //解码后数组长度
#define Q4DATA_LENGTH 4 //解码后四元数数组长度
#define mulitCmd_num 9 //复合指令数组长度
#define EULAR_MEASURE 100.0 //欧拉角度量,原始数据除以之
#define Q4_MEASURE 1000.0 //四元数度量,原始数据除以之
//判断本帧数据类型
#define DATA_FrameHead (unsigned char)(90) //接收数据的帧头
#define DATA_TYPE_ACC (unsigned char)(21) //该帧输出数据为加速度类型
#define DATA_TYPE_GRY (unsigned char)(37) //该帧输出数据为陀螺仪类型
#define DATA_TYPE_MAG (unsigned char)(53) //该帧输出数据为磁力计类型
#define DATA_TYPE_EULAR (unsigned char)(69) //该帧输出数据为欧拉角类型
#define DATA_TYPE_STOP (unsigned char)(85) //该帧输出数据为保留不用类型
#define DATA_TYPE_Q4 (unsigned char)(101) //该帧输出数据为四元数类型
#define DATA_TYPE_PRECISION (unsigned char)(117) //该帧输出数据为传感器精度,频率类型
#define DATA_TYPE_RANGE (unsigned char)(133) //该帧输出数据为传感器量程
//串口波特率设置指令,每次修改后需要重新上电生效,具有掉电保持特性
#define CMD_SET_BAUD115200 (unsigned char)(175) //115200(默认)
#define CMD_SET_BAUD9600 (unsigned char)(174) //9600
//传感器配置指令
#define CMD_CLOSE_JIAJI (unsigned char)(81) //关闭加计
#define CMD_CLOSE_GYR (unsigned char)(82) //关闭陀螺
#define CMD_CLOSE_MAG (unsigned char)(83) //关闭磁场
#define CMD_GYR_CALIBRATE (unsigned char)(87) //加陀校准
#define CMD_MAG_CALIBRATE (unsigned char)(88) //磁场校准
#define CMD_RES_FACTORY (unsigned char)(89) //恢复出厂设置,即清除保存的校准数据
#define CMD_OUTPUT_50HZ (unsigned char)(164) //模块输出频率50hz设置
#define CMD_OUTPUT_100HZ (unsigned char)(165) //模块输出频率100hz设置
#define CMD_OUTPUT_200HZ (unsigned char)(166) //模块输出频率200hz设置
//自动输出指令(具有开关功能,第一次发送打开,第二次发送关闭)
#define CMD_OUTPUT_ACC (unsigned char)(21) //输出加速度原始数据指令
#define CMD_OUTPUT_GYR (unsigned char)(37) //输出陀螺仪原始数据
#define CMD_OUTPUT_MAG (unsigned char)(53) //输出磁场原始数据指令
#define CMD_OUTPUT_EULAR (unsigned char)(69) //输出欧拉角指令(默认50HZ)
#define CMD_OUTPUT_CHAREULAR (unsigned char)(85) //输出欧拉角字符形式(串口助手)
#define CMD_OUTPUT_Q4 (unsigned char)(101) //输出四元数数据指令
//查询输出指令
#define CMD_READ_CALIBRATE_PRECISION (unsigned char)(117) //读取三传感器校准精度,输出频率
#define CMD_READ_RANGE (unsigned char)(133) //读取传感器量程,获取陀螺量程,加计量程,磁场量程
#define CMD_OPENCV_ALLSENSOR (unsigned char)(80) //开启所有传感器
#define CMD_PULL_OUTPUT_EULAR (unsigned char)(149) //输出欧拉角指令(默认50HZ)
#define CMD_PULL_OUTPUT_Q4 (unsigned char)(181) //输出四元数数据指令
#define CMD_PULL_OUTPUT_ACC (unsigned char)(197) //输出加速度原始数据指令
#define CMD_PULL_OUTPUT_GYR (unsigned char)(213) //输出陀螺仪原始数据
#define CMD_PULL_OUTPUT_MAG (unsigned char)(229) //输出磁场原始数据指令
namespace IPSG
{
class CImuCommand
{
serial::Serial ser;
public:
/***********************************************
* @Name :
* @Descrip : 构造函数
* @Para : None
* @Return : None
* @Notes :
***********************************************/
CImuCommand(){};
/***********************************************
* @Name :
* @Descrip : 析构函数
* @Para : None
* @Return : None
* @Notes : None
***********************************************/
~CImuCommand(){};
/***********************************************
* @Name : CImuCommand::cmdFrame
* @Descrip : 向IMU发送指令帧的封装实现
* @in : 指令
* @out : 原始数据
* @Return : bool true:success false:failed
* @Notes : None
***********************************************/
bool cmdFrame(unsigned char imucmd);
/***********************************************
* @Name : CImuCommand::muliteCmdFrame
* @Descrip : 复合指令
* @Para : [in]三个指令
* @Para : [in/out] v_ucData:原始数据帧
* @Return : bool true:success false:failed
* @Notes : None
***********************************************/
bool muliteCmdFrame(unsigned char imucmd1,unsigned char imucmd2,unsigned char imucmd3);
/***********************************************
* @Name : CImuCommand::decodeFrame
* @Descrip : 解码
* @Notes : None
***********************************************/
bool decodeFrame(unsigned char tmpBuffer[READ_BUFFERSIZE]);
bool serialInit();
bool RUN();
bool display_decodeData(float tmpBuffer[DATA_LENGTH]);
bool display_Q4decodeData(float tmpBuffer[Q4DATA_LENGTH]);
private:
// class Serial *mSerial;
public:
unsigned char cmd_buffer[cmd_num];
unsigned char mulit_cmd_buffer[mulitCmd_num];
unsigned char r_buffer[READ_BUFFERSIZE];
float EULAR[DATA_LENGTH];
float read_buffer[READ_BUFFERSIZE];
float GYR[DATA_LENGTH];
float Q4[Q4DATA_LENGTH];
float ACC[DATA_LENGTH];
};
}
#endif //IMUCOMMAND_H
大致框架就是这个,ImuCommand.cpp 就按照这个框架写吧。
就在刚刚写这个博客的时候,同学过来看了一眼:你为啥不把代码托管到 github上?好,github. 此部分主要参考资料1资料2虽然最后并不是用这个方法实现的,但也学到了东西,备忘。