目录
- 前言
- 接收IMU数据
- IMU的串口连接
- 问题
- python接收串口数据
- python解析数据
- ROS2发布IMU数据
- 可视化IMU数据
- 效果
前言
在前面测试完了单独用激光雷达建图之后,一直想把IMU的数据融合进去,由于经费的限制,忍痛在淘宝上买了一款便宜的IMU—GY95T,如下图所示
东西买回来了,但是还需要写一个读取数据的代码,商家并没有提供在ROS2上用python接收数据,并且将其转换为ROS2的数据格式的代码,于是自己只能手搓一遍。
读取IMU数据的代码倒是不难,但是要怎么将其转换为ROS2的数据格式是我之前完全没接触到的,把整个流程记录一下,便于回忆。
环境
ROS2 humble
Ubuntu22.04
IMU:GY-95T
我将整个流程分成了以下步骤
接收IMU数据
流程如下 ,利用python打开串口,然后接收数据即可。具体的过程后面一步步写出
IMU的串口连接
首先通过USB将IMU连接到电脑上。用lsusb
查看连接到Ubuntu上面的USB设备有哪些,我用的虚拟机测试,所以有一些虚拟机的设备
lsusb
# 返回以下信息Bus 001 Device 001: ID 1d6b:0002 Linux Foundation 2.0 root hub
Bus 002 Device 038: ID 1a86:7523 QinHeng Electronics CH340 serial converter
Bus 002 Device 029: ID 0e0f:0008 VMware, Inc. Virtual Bluetooth Adapter
Bus 002 Device 003: ID 0e0f:0002 VMware, Inc. Virtual USB Hub
Bus 002 Device 002: ID 0e0f:0003 VMware, Inc. Virtual Mouse
Bus 002 Device 001: ID 1d6b:0001 Linux Foundation 1.1 root hub
可以看到有一个叫QinHeng Electronics CH340 serial converter
的CH340设备,这就是IMU的串口连接设备了,再通过ls
指令查看其被映射为了什么端口
ls /dev/ttyUSB*# 返回
/dev/ttyUSB0
即这里的USB设备被映射为了/dev/ttyUSB0这个文件,后面还会用得上
问题
我在中途遇到了这么一个问题
能够找到USB设备,但是没有被映射为/dev/ttyUSB0
的问题
我首先查看了是否存在ch340的驱动
lsmod | grep ch34# 返回
ch341 24576 0
usbserial 57344 1 ch341
说明我的虚拟机是存在串口驱动的,但是没有找到设备,那通过这篇博客,我发现了是端口被占用了
Ubuntu22.04没有ttyUSB(无法访问 ‘/dev/ttyUSB‘: 没有那个文件或目录)问题解决
文中所说的方法如下:
sudo dmesg | grep brltty
若出现[ 7033.078452] usb 1-13: usbfs: interface 0 claimed by ch341 while 'brltty' sets config #1
则说明驱动被占用
使用sudo apt remove brltty
后,重新插拔设备即可
问题探究到底,这个brltty卸载后会不会对以后使用有影响呢?用chatgpt搜索这个软件后,给出的解释如下:
在Ubuntu中,Brltty 是一个用于盲人和视力受损人士的文本转换工具。它提供了一个用于显示在Braille终端上的用户界面,这些终端通常由触觉读取器件(如盲文显示器)或语音合成器(如屏幕阅读器)使用。
Brltty 可以将文本数据转换成 Braille 码,通过串口或并口将其发送到 Braille 终端。它支持多种不同类型的 Braille 终端,包括常见的 VarioUltra、BrailleNote 和 Focus 14/40。
除了将文本数据转换为 Braille 码之外,Brltty 还提供了一些其他的辅助功能,如语音反馈、支持多国语言、自动检测插入/拔出设备等。
因此对于非盲人和视力受损人士用处不大,因此卸载后无影响
注意:由于串口是具有权限的,为了确保在后续运行python代码的时候具有程序启动串口的权限,我们这里把端口的权限进行更改
sudo chmod 777 /dev/ttyUSB0
python接收串口数据
在Ubuntu连接了串口之后,我们就可以使用python来读取IMU的数据了
这里用到了python的serial库
sudo apt install python3-serial
为确保读取IMU数据的准确性,我们用python读取串口的方法是,在一个周期下,收取该周期内所有信息,每隔time,将数据返回一次
因此,我们需要按照如下设置我们的串口
# 串口初始化
IMU_Usart = serial.Serial(port = '/dev/ttyUSB0', # 串口baudrate=115200, # 波特率timeout = 0.001 # 由于后续使用read按照一个timeout周期时间读取数据# imu在波特率115200返回数据时间大概是1ms,9600下大概是10ms# 所以读取时间设置0.001s)
# ----读取IMU的内部数据-----------------------------------
try:count = IMU_Usart.inWaiting()if count > 0:# 接收数据至缓存区Read_buffer=self.IMU_Usart.read(40) # 我们需要读取的是40个寄存器数据,即40个字节
except KeyboardInterrupt:if serial != None:print("close serial port")self.IMU_Usart.close()#--------------------------------------------------------
其中.inWaiting()
函数的作用如下:
用于检查串口缓冲区中等待读取的字节数。
在串口通信中,发送方发送的数据可能需要一定时间才能到达接收方。因此,接收方需要先将接收到的数据存储在缓冲区中,等待读取。.inWaiting() 方法可以用于检查当前串口缓冲区中等待读取的字节数,以便读取相应数量的字节。
我用流程图表述一下代码的读取原理
python解析数据
在接收到了IMU传过来的数据之后,我们就需要对其数据进行解析了。
解析这个部分,没有特别多要说的,我是根据卖家给出的数据手册,按照状态机的方式接收并检查数据是否准确。这里把状态机的代码贴出来
def Read_data(self):'''Author: Liu Yuxiang Time: 2022.12.13description: 读取IMU数据'''# 初始化数据counter = 0 Recv_flag = 0Read_buffer = []# 接收数据至缓存区Read_buffer=self.IMU_Usart.read(40) # 我们需要读取的是40个寄存器数据,即40个字节# 状态机判断收包数据是否准确while(1):# 第1帧是否是帧头ID 0xA4if (counter == 0):if(Read_buffer[0] != 0xA4):break # 第2帧是否是读功能码 0x03 elif (counter == 1):if(Read_buffer[1] != 0x03):counter=0break# 第3帧判断起始帧 elif (counter == 2):if(Read_buffer[2] < 0x2c):start_reg=Read_buffer[2]else:counter=0 # 第4帧判断帧有多少数量 elif (counter == 3):if((start_reg+Read_buffer[3]) < 0x2C): # 最大寄存器为2C 大于0x2C说明数据肯定错了len=Read_buffer[3]else:counter=0break else:if(len+5==counter):#print('Recv done!')Recv_flag=1# 收包完毕if(Recv_flag):Recv_flag = 0sum = 0#print(Read_buffer) # Read_buffer中的是byte数据字节流,用struct包解包data_inspect = str(binascii.b2a_hex(Read_buffer)) # data是将数据转化为原本的按照16进制的数据try: # 如果接收数据无误,则执行数据解算操作for i in range(2,80,2): # 根据手册,检验所有帧之和低八位是否等于末尾帧sum += int(data_inspect[i:i+2],16)if (str(hex(sum))[-2:] == data_inspect[80:82]): # 如果数据检验没有问题,则进入解包过程#print('the Rev data is right')# 数据低八位在前,高八位在后#print(Read_buffer[4:-1]) unpack_data = struct.unpack('<hhhhhhhhhBhhhhhhhh',Read_buffer[4:-1])# 切片并将其解析为我们所需要的数据,切出我们所需要的数据部分g = 9.8self.ACC_X = unpack_data[0]/2048 * g # unit m/s^2self.ACC_Y = unpack_data[1]/2048 * g self.ACC_Z = unpack_data[2]/2048 * gself.GYRO_X = unpack_data[3]/16.4 # unit 度/sself.GYRO_Y = unpack_data[4]/16.4 self.GYRO_Z = unpack_data[5]/16.4 self.roll = unpack_data[6]/100 self.pitch = unpack_data[7]/100 self.yaw = unpack_data[8]/100 self.level = unpack_data[9]self.temp = unpack_data[10]/100 self.MAG_X = unpack_data[11]/1000 # unit Gaos self.MAG_Y = unpack_data[12]/1000 self.MAG_Z = unpack_data[13]/1000self.Q0 = unpack_data[14]/10000 self.Q1 = unpack_data[15]/10000 self.Q2 = unpack_data[16]/10000 self.Q3 = unpack_data[17]/10000#print(self.__dict__) except:print("Have Error in receiving data!!")counter=0 breakelse:counter += 1 # 遍历整个接收数据的buffer
ROS2发布IMU数据
在跑通了整个读取数据的流程,并与卖家给的上位机返回的数据对比没问题之后,下一步就是需要将数据转换为ROS2的数据格式,并将其发布出来,ROS2中IMU的数据格式类型如下
std_msgs/Header header # 时间戳和坐标系ID
geometry_msgs/Quaternion orientation # 四元数形式的方向
float64[9] orientation_covariance # 方向估计的协方差矩阵
geometry_msgs/Vector3 angular_velocity # 三维角速度
float64[9] angular_velocity_covariance # 角速度估计的协方差矩阵
geometry_msgs/Vector3 linear_acceleration # 三维线性加速度
float64[9] linear_acceleration_covariance # 线性加速度估计的协方差矩阵
header
:标准的ROS消息头,包含测量的时间戳和坐标系。- 时间戳没有单位,坐标系ID是字符串类型;
orientation
:以四元数形式表示的IMU传感器的方向,以geometry_msgs/Quaternion
消息表示。四元数表示从IMU坐标系到由header指定的参考坐标系的旋转。四元数应该被归一化。- 四元数表示的方向没有单位;
orientation_covariance
:一个包含9个元素的数组,表示方向估计的协方差矩阵。矩阵以行优先顺序存储。协方差值应以(x、y、z、x-y、x-z、y-z)的顺序表示。- 协方差矩阵是无单位的;
angular_velocity
:以IMU坐标系表示的IMU传感器的角速度,以geometry_msgs/Vector3
消息表示。- 角速度以弧度/秒(rad/s)为单位;
angular_velocity_covariance
:一个包含9个元素的数组,表示角速度估计的协方差矩阵。矩阵以行优先顺序存储。协方差值应以(x、y、z、x-y、x-z、y-z)的顺序表示。- 角速度估计的协方差矩阵是以 (rad/s)^2 为单位的;
linear_acceleration
:以IMU坐标系表示的IMU传感器的线性加速度,以 geometry_msgs/Vector3 消息表示。- 线性加速度以米/秒²(m/s²)为单位;
linear_acceleration_covariance
:一个包含9个元素的数组,表示线性加速度估计的协方差矩阵。矩阵以行优先顺序存储。协方差值应以(x、y、z、x-y、x-z、y-z)的顺序表示。- 线性加速度估计的协方差矩阵是以 (m/s²)^2 为单位的。
这个IMU是可以直接传回四元数,因此我可以直接进行传参,而不需要再进行四元数转换
注意,这里需要用到ROS2的serial
库,但是由于在ROS2 humble中并没有此库,因此,我们需要去下载这个库的源码,并手动安装,安装的教程参考我之前的这篇博客,地址如下:
ROS2安装serial库
这里把完整的代码贴出来
import rclpy # ROS2 Python Client Library
from rclpy.node import Node # ROS2 Node
from sensor_msgs.msg import Imu# Usart Library
import serial
import struct
import binascii'''Author: Liu Yuxiang Time: 2022.12.13description: IMU底层串口收发代码
'''# imu接收数据类型
class IMU(Node):send_data = []def __init__(self):super().__init__('imu_publisher')self.publisher_ = self.create_publisher(Imu, 'imu_data', 1)# 串口初始化self.IMU_Usart = serial.Serial(port = '/dev/ttyUSB0', # 串口baudrate=115200, # 波特率timeout = 0.001 # 由于后续使用read_all按照一个timeout周期时间读取数据# imu在波特率115200返回数据时间大概是1ms,9600下大概是10ms# 所以读取时间设置0.001s)# 接收数据初始化self.ACC_X:float = 0.0 # X轴加速度self.ACC_Y:float = 0.0 # Y轴加速度self.ACC_Z:float = 0.0 # Z轴加速度self.GYRO_X :float = 0.0 # X轴陀螺仪self.GYRO_Y :float = 0.0 # Y轴陀螺仪self.GYRO_Z :float = 0.0 # Z轴陀螺仪self.roll :float = 0.0 # 横滚角 self.pitch :float = 0.0 # 俯仰角self.yaw :float = 0.0 # 航向角self.leve :float = 0.0 # 磁场校准精度self.temp :float = 0.0 # 器件温度self.MAG_X :float = 0.0 # 磁场X轴self.MAG_Y :float = 0.0 # 磁场Y轴self.MAG_Z :float = 0.0 # 磁场Z轴self.Q0 :float = 0.0 # 四元数Q0.0self.Q1 :float = 0.0 # 四元数Q1self.Q2 :float = 0.0 # 四元数Q2self.Q3 :float = 0.0 # 四元数Q3# 判断串口是否打开成功if self.IMU_Usart.isOpen():print("open success")else:print("open failed")# 回调函数返回周期time_period = 0.001 self.timer = self.create_timer(time_period, self.timer_callback)# 发送读取指令self.Send_ReadCommand()def Send_ReadCommand(self):'''Author: Liu Yuxiang Time: 2022.12.13description: 发送读取IMU内部数据指令· 第一个寄存器0x08 最后一个读取寄存器0x2A 共35个· 读寄存器例子,读取模块内部温度,主站发送帧为:A4 03 1B 02 C4| A4 | 03 | 1B | 02 | C4| 帧头ID | 读功能码 |起始寄存器| 寄存器数量 |校验和低 8 位'''# 使用优雅的方式发送串口数据send_data = [0xA4,0x03,0x08,0x23,0xD2] #需要发送的串口包#send_data = [0xA4,0x03,0x08,0x1B,0xCA] #需要发送的串口包send_data=struct.pack("%dB"%(len(send_data)),*send_data) #解析成16进制print(send_data)self.IMU_Usart.write(send_data) #发送def Read_data(self):'''Author: Liu Yuxiang Time: 2022.12.13description: 读取IMU数据'''# 初始化数据counter = 0 Recv_flag = 0Read_buffer = []# 接收数据至缓存区Read_buffer=self.IMU_Usart.read(40) # 我们需要读取的是40个寄存器数据,即40个字节# 状态机判断收包数据是否准确while(1):# 第1帧是否是帧头ID 0xA4if (counter == 0):if(Read_buffer[0] != 0xA4):break # 第2帧是否是读功能码 0x03 elif (counter == 1):if(Read_buffer[1] != 0x03):counter=0break# 第3帧判断起始帧 elif (counter == 2):if(Read_buffer[2] < 0x2c):start_reg=Read_buffer[2]else:counter=0 # 第4帧判断帧有多少数量 elif (counter == 3):if((start_reg+Read_buffer[3]) < 0x2C): # 最大寄存器为2C 大于0x2C说明数据肯定错了len=Read_buffer[3]else:counter=0break else:if(len+5==counter):#print('Recv done!')Recv_flag=1# 收包完毕if(Recv_flag):Recv_flag = 0sum = 0#print(Read_buffer) # Read_buffer中的是byte数据字节流,用struct包解包data_inspect = str(binascii.b2a_hex(Read_buffer)) # data是将数据转化为原本的按照16进制的数据try: # 如果接收数据无误,则执行数据解算操作for i in range(2,80,2): # 根据手册,检验所有帧之和低八位是否等于末尾帧sum += int(data_inspect[i:i+2],16)if (str(hex(sum))[-2:] == data_inspect[80:82]): # 如果数据检验没有问题,则进入解包过程#print('the Rev data is right')# 数据低八位在前,高八位在后#print(Read_buffer[4:-1]) unpack_data = struct.unpack('<hhhhhhhhhBhhhhhhhh',Read_buffer[4:-1])# 切片并将其解析为我们所需要的数据,切出我们所需要的数据部分g = 9.8self.ACC_X = unpack_data[0]/2048 * g # unit m/s^2self.ACC_Y = unpack_data[1]/2048 * g self.ACC_Z = unpack_data[2]/2048 * gself.GYRO_X = unpack_data[3]/16.4 # unit 度/sself.GYRO_Y = unpack_data[4]/16.4 self.GYRO_Z = unpack_data[5]/16.4 self.roll = unpack_data[6]/100 self.pitch = unpack_data[7]/100 self.yaw = unpack_data[8]/100 self.level = unpack_data[9]self.temp = unpack_data[10]/100 self.MAG_X = unpack_data[11]/1000 # unit Gaos self.MAG_Y = unpack_data[12]/1000 self.MAG_Z = unpack_data[13]/1000self.Q0 = unpack_data[14]/10000 self.Q1 = unpack_data[15]/10000 self.Q2 = unpack_data[16]/10000 self.Q3 = unpack_data[17]/10000#print(self.__dict__) except:print("Have Error in receiving data!!")counter=0 breakelse:counter += 1 # 遍历整个接收数据的bufferdef timer_callback(self):# ----读取IMU的内部数据-----------------------------------try:count = self.IMU_Usart.inWaiting()if count > 0:self.Read_data()# 发布sensor_msgs/Imu 数据类型imu_data = Imu()imu_data.header.frame_id = "map"imu_data.header.stamp = self.get_clock().now().to_msg()imu_data.linear_acceleration.x = self.ACC_Ximu_data.linear_acceleration.y = self.ACC_Yimu_data.linear_acceleration.z = self.ACC_Zimu_data.angular_velocity.x = self.GYRO_X * 3.1415926 / 180.0 # unit transfer to rad/simu_data.angular_velocity.y = self.GYRO_Y * 3.1415926 / 180.0imu_data.angular_velocity.z = self.GYRO_Z * 3.1415926 / 180.0imu_data.orientation.x = self.Q0imu_data.orientation.y = self.Q1imu_data.orientation.z = self.Q2imu_data.orientation.w = self.Q3self.publisher_.publish(imu_data) # 发布imu的数据# --------------------------------------------------------#print('读取中')except KeyboardInterrupt:if serial != None:print("close serial port")self.IMU_Usart.close()#--------------------------------------------------------def main(args=None):# 变量初始化--------------------------------------------- rclpy.init(args=args)IMU_node = IMU()rclpy.spin(IMU_node)rclpy.shutdown()if __name__ == '__main__':main()'''
░░░░░░░░░░░░░░░░░░░░░░░░▄░░
░░░░░░░░░▐█░░░░░░░░░░░▄▀▒▌░
░░░░░░░░▐▀▒█░░░░░░░░▄▀▒▒▒▐░
░░░░░▄▄▀▒░▒▒▒▒▒▒▒▒▒█▒▒▄█▒▐░
░░░▄▀▒▒▒░░░▒▒▒░░░▒▒▒▀██▀▒▌░
░░▐▒▒▒▄▄▒▒▒▒░░░▒▒▒▒▒▒▒▀▄▒▒░
░░▌░░▌█▀▒▒▒▒▒▄▀█▄▒▒▒▒▒▒▒█▒▐
░▐░░░▒▒▒▒▒▒▒▒▌██▀▒▒░░░▒▒▒▀▄
░▌░▒▄██▄▒▒▒▒▒▒▒▒▒░░░░░░▒▒▒▒
▀▒▀▐████▌▄░▀▒▒░░░░░░░░░░▒▒▒
狗狗保佑代码无bug!
'''
其实最关键的部分就是对其进行一个赋值并发布出去即可。
可视化IMU数据
当我们有了数据之后,我们希望对其进行可视化,这样能够非常直观的看到我们的数据,并且符不符合ROS2的数据格式,因此,我们接下来用rviz2来可视化
首先,需要安装IMU的可视化工具,imu-tools
sudo apt install ros-humble-imu-tools
注意:如果你的ROS2的不是humble版本,就将其改为你自己的版本即可
安装完毕后,运行节点,并开启rviz2,点击add
,在By topic
中添加Imu
就可以可视化IMU数据啦