ROS2中IMU话题的发布及可视化

环境：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_euler

class ImuNode(Node):
    def __init__(self,name):
        super().__init__(name)
        self.get_logger().info("imu node init") 
        self.a = [0.0] * 3 # m/s^2
        self.w = [0.0] * 3 # rad/s
        self.angle = [0.0] * 3 # rad

        self.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] * 8
        GYROData = [0.0] * 8
        AngleData = [0.0] * 8
        for data in inputdata:  #在输入的数据进行遍历
            if FrameState == 0:   #当未确定状态的时候，进入以下判断
                if data == 0x55 and Bytenum == 0: #0x55位于第一位时候，开始读取数据，增大bytenum
                    CheckSum = data
                    Bytenum = 1
                    continue
                elif data == 0x51 and Bytenum == 1:#在byte不为0 且 识别到 0x51 的时候，改变frame
                    CheckSum += data
                    FrameState = 1
                    Bytenum = 2
                elif data == 0x52 and Bytenum == 1: #同理
                    CheckSum += data
                    FrameState = 2
                    Bytenum=2
                elif data == 0x53 and Bytenum == 1:
                    CheckSum += data
                    FrameState = 3
                    Bytenum = 2
            elif FrameState == 1: # acc_x   #已确定数据代表加速度   
                if Bytenum < 10:            # 读取8个数据
                    ACCData[Bytenum-2] = data # 从0开始
                    CheckSum += data
                    Bytenum += 1
                else:
                    if data == (CheckSum & 0xff):  #假如校验位正确
                        self.a = self.get_acc(ACCData)
                    CheckSum = 0 #各数据归零，进行新的循环判断
                    Bytenum = 0
                    FrameState = 0
            elif FrameState == 2: # gyro                
                if Bytenum < 10:
                    GYROData[Bytenum-2] = data
                    CheckSum += data
                    Bytenum += 1
                else:
                    if data == (CheckSum & 0xff):
                        self.w = self.get_gyro(GYROData)
                    CheckSum = 0
                    Bytenum = 0
                    FrameState = 0
            elif FrameState == 3: # angle               
                if Bytenum < 10:
                    AngleData[Bytenum-2] = data
                    CheckSum += data
                    Bytenum += 1
                else:
                    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 = 0
                    Bytenum = 0
                    FrameState = 0
     
    def 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_acc
        acc_y = (ayh << 8 | ayl) / 32768.0 * k_acc
        acc_z = (azh << 8 | azl) / 32768.0 * k_acc
        if acc_x >= k_acc:
            acc_x -= 2 * k_acc
        if acc_y >= k_acc:
            acc_y -= 2 * k_acc
        if acc_z >= k_acc:
            acc_z-= 2 * k_acc 
        g = 9.8
        acc_x *= g
        acc_y *= g
        acc_z *= g    
        return acc_x,acc_y,acc_z
     
    def 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_gyro
        gyro_y = (wyh << 8 | wyl) / 32768.0 * k_gyro
        gyro_z = (wzh << 8 | wzl) / 32768.0 * k_gyro
        if gyro_x >= k_gyro:
            gyro_x -= 2 * k_gyro
        if gyro_y >= k_gyro:
            gyro_y -= 2 * k_gyro
        if gyro_z >=k_gyro:
            gyro_z-= 2 * k_gyro
        gyro_x = gyro_x * 3.1415926 / 180.0
        gyro_y = gyro_z * 3.1415926 / 180.0
        gyro_z = gyro_y * 3.1415926 / 180.0
        return gyro_x,gyro_y,gyro_z
 
    def 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_angle
        angle_y = (ryh << 8 | ryl) / 32768.0 * k_angle
        angle_z = (rzh << 8 | rzl) / 32768.0 * k_angle
        if angle_x >= k_angle:
            angle_x -= 2 * k_angle
        if angle_y >= k_angle:
            angle_y -= 2 * k_angle
        if angle_z >=k_angle:
            angle_z -= 2 * k_angle
        angle_x = angle_x * 3.1415926 / 180.0
        angle_y = angle_y * 3.1415926 / 180.0
        angle_z = angle_z * 3.1415926 / 180.0
        return angle_x,angle_y,angle_z

def main(args=None):
    rclpy.init(args=args)
    wit_node = ImuNode("wit_node")
    rclpy.spin(wit_node)
    rclpy.shutdown()