ROS串口通信（2）以十六进制指令读取IMU数据
-
-
引言
-
1、下载安装ROS的serial软件包
-
2、通用serial通信代码
-
3、使用ROS serial包实现IMU十六进制指令发送及数据读取编解码保存。
引言
20200527更新：最近发现阅读量还蛮高，应该比较实用吧，刚刚把之前的代码贴到github上了，如果有帮助到您，希望帮我博客或github点个赞啊，助我找工作一臂之力！谢谢了.
前期准备参考博客
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
-
1
-
2
-
3
-
4
-
5
-
6
-
7
-
8
-
9
-
10
-
11
-
12
-
13
-
14
-
15
-
16
-
17
-
18
-
19
-
20
-
21
-
22
-
23
-
24
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();
}
}
-
1
-
2
-
3
-
4
-
5
-
6
-
7
-
8
-
9
-
10
-
11
-
12
-
13
-
14
-
15
-
16
-
17
-
18
-
19
-
20
-
21
-
22
-
23
-
24
-
25
-
26
-
27
-
28
-
29
-
30
-
31
-
32
-
33
-
34
-
35
-
36
-
37
-
38
-
39
-
40
-
41
-
42
-
43
-
44
-
45
-
46
-
47
-
48
-
49
-
50
-
51
-
52
-
53
-
54
-
55
-
56
-
57
-
58
-
59
-
60
-
61
-
62
-
63
此部分主要参考文章.该文章主要实现的功能： 
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
-
1
-
2
-
3
-
4
-
5
-
6
-
7
-
8
-
9
-
10
-
11
-
12
-
13
-
14
-
15
-
16
-
17
-
18
-
19
-
20
-
21
-
22
-
23
-
24
-
25
-
26
-
27
-
28
-
29
-
30
-
31
-
32
-
33
-
34
-
35
-
36
-
37
-
38
-
39
-
40
-
41
-
42
-
43
-
44
-
45
-
46
-
47
-
48
-
49
-
50
-
51
-
52
-
53
-
54
-
55
-
56
-
57
-
58
-
59
-
60
-
61
-
62
-
63
-
64
-
65
-
66
-
67
-
68
-
69
-
70
-
71
-
72
-
73
-
74
-
75
-
76
-
77
-
78
-
79
-
80
-
81
-
82
-
83
-
84
-
85
-
86
-
87
-
88
-
89
-
90
-
91
-
92
-
93
-
94
-
95
-
96
-
97
-
98
-
99
-
100
-
101
-
102
-
103
-
104
-
105
-
106
-
107
-
108
-
109
-
110
-
111
-
112
-
113
-
114
-
115
-
116
-
117
-
118
-
119
-
120
-
121
-
122
-
123
-
124
-
125
-
126
-
127
-
128
-
129
-
130
-
131
-
132
-
133
-
134
-
135
-
136
-
137
-
138
-
139
-
140
-
141
-
142
-
143
-
144
-
145
-
146
-
147
-
148
-
149
-
150
大致框架就是这个，ImuCommand.cpp 就按照这个框架写吧。此部分主要参考资料1资料2虽然最后并不是用这个方法实现的，但也学到了东西，备忘。
