环境：Ubuntu 20.04，ROS2 Foxy
传感器：维特智能BWT901CL

代码是从维特智能的示例代码修改的，实现基本的加速度、角速度和角度读取，发布IMU消息。这个传感器还支持磁场输出等功能，后面再加上吧。
IMU消息的说明可以看这里sensor_msgs/Imu Message

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
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

用sudo chmod 777 /dev/ttyUSB0 修改串口权限。
RViz2进行可视化，需用sudo apt-get install ros-foxy-imu-tools安装用于IMU消息显示的插件，之后在 Add -> By topic 中选择IMU消息即可。

import rclpy    # ROS2 Python Client Library
from rclpy.node import Node    # ROS2 Node
from sensor_msgs.msg import Imu
import serial # Usart Library       
from tf_transformations import quaternion_from_eulerclass ImuNode(Node):def __init__(self,name):super().__init__(name)self.get_logger().info("imu node init") self.a = [0.0] * 3 # m/s^2self.w = [0.0] * 3 # rad/sself.angle = [0.0] * 3 # radself.ser = serial.Serial('/dev/ttyUSB0', 115200, timeout=0.5)if self.ser.is_open:print("open success")else:print("open failed")self.publisher = self.create_publisher(Imu, 'imu_data', 1)self.timer = self.create_timer(0.001, self.timer_callback)def timer_callback(self):datahex = self.ser.read(33)self.DueData(datahex)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.a[0]imu_data.linear_acceleration.y = self.a[1]imu_data.linear_acceleration.z = self.a[2]imu_data.angular_velocity.x = self.w[0]imu_data.angular_velocity.y = self.w[1]imu_data.angular_velocity.z = self.w[2]q = quaternion_from_euler(self.angle[0], self.angle[1], self.angle[2])imu_data.orientation.x = q[0]imu_data.orientation.y = q[1]imu_data.orientation.z = q[2]imu_data.orientation.w = q[3]self.publisher.publish(imu_data) def DueData(self,inputdata):   #新增的核心程序，对读取的数据进行划分，各自读到对应的数组里FrameState = 0  #通过0x后面的值判断属于哪一种情况Bytenum = 0     #读取到这一段的第几位CheckSum = 0    #求和校验位ACCData = [0.0] * 8GYROData = [0.0] * 8AngleData = [0.0] * 8for data in inputdata:  #在输入的数据进行遍历if FrameState == 0:   #当未确定状态的时候，进入以下判断if data == 0x55 and Bytenum == 0: #0x55位于第一位时候，开始读取数据，增大bytenumCheckSum = dataBytenum = 1continueelif data == 0x51 and Bytenum == 1:#在byte不为0 且 识别到 0x51 的时候，改变frameCheckSum += dataFrameState = 1Bytenum = 2elif data == 0x52 and Bytenum == 1: #同理CheckSum += dataFrameState = 2Bytenum=2elif data == 0x53 and Bytenum == 1:CheckSum += dataFrameState = 3Bytenum = 2elif FrameState == 1: # acc_x   #已确定数据代表加速度   if Bytenum < 10:            # 读取8个数据ACCData[Bytenum-2] = data # 从0开始CheckSum += dataBytenum += 1else:if data == (CheckSum & 0xff):  #假如校验位正确self.a = self.get_acc(ACCData)CheckSum = 0 #各数据归零，进行新的循环判断Bytenum = 0FrameState = 0elif FrameState == 2: # gyro                if Bytenum < 10:GYROData[Bytenum-2] = dataCheckSum += dataBytenum += 1else:if data == (CheckSum & 0xff):self.w = self.get_gyro(GYROData)CheckSum = 0Bytenum = 0FrameState = 0elif FrameState == 3: # angle               if Bytenum < 10:AngleData[Bytenum-2] = dataCheckSum += dataBytenum += 1else:if data == (CheckSum & 0xff):self.angle = self.get_angle(AngleData)#d = self.a + self.w + self.angle#print("a(m/s^2):%10.3f %10.3f %10.3f w(rad/s):%10.3f %10.3f %10.3f Angle(rad):%10.3f %10.3f %10.3f"%d)CheckSum = 0Bytenum = 0FrameState = 0def get_acc(self,datahex):  axl = datahex[0]                                        axh = datahex[1]ayl = datahex[2]                                        ayh = datahex[3]azl = datahex[4]                                        azh = datahex[5]       k_acc = 16.0    acc_x = (axh << 8 | axl) / 32768.0 * k_accacc_y = (ayh << 8 | ayl) / 32768.0 * k_accacc_z = (azh << 8 | azl) / 32768.0 * k_accif acc_x >= k_acc:acc_x -= 2 * k_accif acc_y >= k_acc:acc_y -= 2 * k_accif acc_z >= k_acc:acc_z-= 2 * k_acc g = 9.8acc_x *= gacc_y *= gacc_z *= g    return acc_x,acc_y,acc_zdef get_gyro(self,datahex):                                      wxl = datahex[0]                                        wxh = datahex[1]wyl = datahex[2]                                        wyh = datahex[3]wzl = datahex[4]                                        wzh = datahex[5]k_gyro = 2000.0    gyro_x = (wxh << 8 | wxl) / 32768.0 * k_gyrogyro_y = (wyh << 8 | wyl) / 32768.0 * k_gyrogyro_z = (wzh << 8 | wzl) / 32768.0 * k_gyroif gyro_x >= k_gyro:gyro_x -= 2 * k_gyroif gyro_y >= k_gyro:gyro_y -= 2 * k_gyroif gyro_z >=k_gyro:gyro_z-= 2 * k_gyrogyro_x = gyro_x * 3.1415926 / 180.0gyro_y = gyro_z * 3.1415926 / 180.0gyro_z = gyro_y * 3.1415926 / 180.0return gyro_x,gyro_y,gyro_zdef get_angle(self,datahex):                                 rxl = datahex[0]                                        rxh = datahex[1]ryl = datahex[2]                                        ryh = datahex[3]rzl = datahex[4]                                        rzh = datahex[5]k_angle = 180.0     angle_x = (rxh << 8 | rxl) / 32768.0 * k_angleangle_y = (ryh << 8 | ryl) / 32768.0 * k_angleangle_z = (rzh << 8 | rzl) / 32768.0 * k_angleif angle_x >= k_angle:angle_x -= 2 * k_angleif angle_y >= k_angle:angle_y -= 2 * k_angleif angle_z >=k_angle:angle_z -= 2 * k_angleangle_x = angle_x * 3.1415926 / 180.0angle_y = angle_y * 3.1415926 / 180.0angle_z = angle_z * 3.1415926 / 180.0return angle_x,angle_y,angle_zdef main(args=None):rclpy.init(args=args)wit_node = ImuNode("wit_node")rclpy.spin(wit_node)rclpy.shutdown()