前言

在前面测试完了单独用激光雷达建图之后，一直想把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！
'''