在前面测试完了单独用激光雷达建图之后，一直想把IMU的数据融合进去，由于经费的限制，忍痛在淘宝上买了一款便宜的IMU—GY95T，如下图所示
东西买回来了，但是还需要写一个读取数据的代码，商家并没有提供在ROS2上用python接收数据，并且将其转换为ROS2的数据格式的代码，于是自己只能手搓一遍。
读取IMU数据的代码倒是不难，但是要怎么将其转换为ROS2的数据格式是我之前完全没接触到的，把整个流程记录一下，便于回忆。
环境
ROS2 humble
Ubuntu22.04
IMU：GY-95T
我将整个流程分成了以下步骤
流程如下 ，利用python打开串口，然后接收数据即可。具体的过程后面一步步写出
首先通过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
在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() 方法可以用于检查当前串口缓冲区中等待读取的字节数，以便读取相应数量的字节。
我用流程图表述一下代码的读取原理
在接收到了IMU传过来的数据之后，我们就需要对其数据进行解析了。
解析这个部分，没有特别多要说的，我是根据卖家给出的数据手册，按照状态机的方式接收并检查数据是否准确。这里把状态机的代码贴出来
def Read_data(self):
'''
Author: Liu Yuxiang
Time: 2022.12.13
description: 读取IMU数据
'''
# 初始化数据
counter = 0
Recv_flag = 0
Read_buffer = []
# 接收数据至缓存区
Read_buffer=self.IMU_Usart.read(40) # 我们需要读取的是40个寄存器数据，即40个字节
# 状态机判断收包数据是否准确
while(1):
# 第1帧是否是帧头ID 0xA4
if (counter == 0):
if(Read_buffer[0] != 0xA4):
break
# 第2帧是否是读功能码 0x03
elif (counter == 1):
if(Read_buffer[1] != 0x03):
counter=0
break
# 第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=0
break
else:
if(len+5==counter):
#print('Recv done!')
Recv_flag=1
# 收包完毕
if(Recv_flag):
Recv_flag = 0
sum = 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.8
self.ACC_X = unpack_data[0]/2048 * g # unit m/s^2
self.ACC_Y = unpack_data[1]/2048 * g
self.ACC_Z = unpack_data[2]/2048 * g
self.GYRO_X = unpack_data[3]/16.4 # unit 度/s
self.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]/1000
self.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
break
else:
counter += 1 # 遍历整个接收数据的buffer
在跑通了整个读取数据的流程，并与卖家给的上位机返回的数据对比没问题之后，下一步就是需要将数据转换为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消息头，包含测量的时间戳和坐标系。
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 消息表示。
angular_velocity_covariance：一个包含9个元素的数组，表示角速度估计的协方差矩阵。矩阵以行优先顺序存储。协方差值应以(x、y、z、x-y、x-z、y-z)的顺序表示。
linear_acceleration：以IMU坐标系表示的IMU传感器的线性加速度，以 geometry_msgs/Vector3 消息表示。
linear_acceleration_covariance：一个包含9个元素的数组，表示线性加速度估计的协方差矩阵。矩阵以行优先顺序存储。协方差值应以(x、y、z、x-y、x-z、y-z)的顺序表示。
这个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.13
description: 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.0
self.Q1 :float = 0.0 # 四元数Q1
self.Q2 :float = 0.0 # 四元数Q2
self.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.13
description: 发送读取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.13
description: 读取IMU数据
'''
# 初始化数据
counter = 0
Recv_flag = 0
Read_buffer = []
# 接收数据至缓存区
Read_buffer=self.IMU_Usart.read(40) # 我们需要读取的是40个寄存器数据，即40个字节
# 状态机判断收包数据是否准确
while(1):
# 第1帧是否是帧头ID 0xA4
if (counter == 0):
if(Read_buffer[0] != 0xA4):
break
# 第2帧是否是读功能码 0x03
elif (counter == 1):
if(Read_buffer[1] != 0x03):
counter=0
break
# 第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=0
break
else:
if(len+5==counter):
#print('Recv done!')
Recv_flag=1
# 收包完毕
if(Recv_flag):
Recv_flag = 0
sum = 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.8
self.ACC_X = unpack_data[0]/2048 * g # unit m/s^2
self.ACC_Y = unpack_data[1]/2048 * g
self.ACC_Z = unpack_data[2]/2048 * g
self.GYRO_X = unpack_data[3]/16.4 # unit 度/s
self.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]/1000
self.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
break
else:
counter += 1 # 遍历整个接收数据的buffer
def 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_X
imu_data.linear_acceleration.y = self.ACC_Y
imu_data.linear_acceleration.z = self.ACC_Z
imu_data.angular_velocity.x = self.GYRO_X * 3.1415926 / 180.0 # unit transfer to rad/s
imu_data.angular_velocity.y = self.GYRO_Y * 3.1415926 / 180.0
imu_data.angular_velocity.z = self.GYRO_Z * 3.1415926 / 180.0
imu_data.orientation.x = self.Q0
imu_data.orientation.y = self.Q1
imu_data.orientation.z = self.Q2
imu_data.orientation.w = self.Q3
self.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！
'''
其实最关键的部分就是对其进行一个赋值并发布出去即可。
当我们有了数据之后，我们希望对其进行可视化，这样能够非常直观的看到我们的数据，并且符不符合ROS2的数据格式，因此，我们接下来用rviz2来可视化
首先，需要安装IMU的可视化工具，imu-tools
sudo apt install ros-humble-imu-tools
注意：如果你的ROS2的不是humble版本，就将其改为你自己的版本即可
安装完毕后，运行节点，并开启rviz2，点击add，在By topic中添加Imu就可以可视化IMU数据啦
