摘要：
1、 背景基本配置：ubuntu16.04，ROSDYNAMIC惯性导航模型：Vittek Intelligence WT61C提供的参考程序通过手动比较每个字节来正式确定数据包/数据帧。我个人认为它比较繁琐，因此使用Python的re和struct模块来简化其数据匹配和提取，并实现ROS中惯性导航数据的发布。
一、背景
基本配置：ubuntu 16.04，ROS Kinetic
惯导型号：维特智能 WT61C（六轴惯导）
维特智能官方提供的参考程序是通过手动比较各个字节来确定数据包/数据帧的，个人认为比较繁琐，因此采用Python的re（正则表达式）和struct（字节处理）模块简化其数据匹配和提取，并实现惯导数据在ROS中的发布。
二、程序
惯导程序如下：
#!/usr/bin/env python import math import re import serial import struct import time import rospy from geometry_msgs.msg import Quaternion, Vector3 from sensor_msgs.msg import Imu cov_orientation = [0.00174533, 0, 0, 0, 0.00174533, 0, 0, 0, 0.00174533] cov_angular_velocity, cov_linear_acceleration = [0.00104720, 0, 0, 0, 0.00104720, 0, 0, 0, 0.00104720], [0.0049, 0, 0, 0, 0.0049, 0, 0, 0, 0.0049] def eul_to_qua(Eular): Eular_Div = [0, 0, 0] Eular_Div[0], Eular_Div[1], Eular_Div[2] = Eular[0]/2.0, Eular[1]/2.0, Eular[2]/2.0 ca, cb, cc = math.cos(Eular_Div[0]), math.cos(Eular_Div[1]), math.cos(Eular_Div[2]) sa, sb, sc = math.sin(Eular_Div[0]), math.sin(Eular_Div[1]), math.sin(Eular_Div[2]) x = sa*cb*cc - ca*sb*sc y = ca*sb*cc + sa*cb*sc z = ca*cb*sc - sa*sb*cc w = ca*cb*cc + sa*sb*sc orientation = Quaternion() orientation.x, orientation.y, orientation.z, orientation.w = x, y, z, w return orientation accCali = "xffxaax67" feature = "UQ(.{6,6}).{3,3}UR(.{6,6}).{3,3}US(.{6,6}).{3,3}" fmt_B, fmt_h = "BBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBB", "<hhh" if __name__ == "__main__": rospy.init_node("imu") port = rospy.get_param("~port", "/dev/imu") imuHandle = serial.Serial(port=port, baudrate=115200, timeout=0.5) if imuHandle.isOpen(): rospy.loginfo("SUCCESS") else: rospy.loginfo("FAILURE") # Accelerometer Calibration time.sleep(0.5) imuHandle.write(accCali) time.sleep(0.5) imuPub = rospy.Publisher("imu", Imu, queue_size=10) while True: data = imuHandle.read(size=65) stamp = rospy.get_rostime() result = re.search(feature, data) if result: # check and get data frame = struct.unpack(fmt_B, result.group()) sum_Q, sum_R, sum_S = 0, 0, 0 for i in range(0, 10): sum_Q, sum_R, sum_S = sum_Q+frame[i], sum_R+frame[i+11], sum_S+frame[i+22] sum_Q, sum_R, sum_S = sum_Q&0x000000ff, sum_R&0x000000ff, sum_S&0x000000ff if (sum_Q==frame[10]) and (sum_R==frame[21]) and (sum_S==frame[32]): af, wf, ef = struct.unpack(fmt_h, result.group(1)), struct.unpack(fmt_h, result.group(2)), struct.unpack(fmt_h, result.group(3)) af_l, wf_l, ef_l = [], [], [] for i in range(0, 3): af_l.append(af[i]/32768.0*16*-9.8), wf_l.append(wf[i]/32768.0*2000*math.pi/180), ef_l.append(ef[i]/32768.0*math.pi) imuMsg = Imu() imuMsg.header.stamp, imuMsg.header.frame_id = stamp, "base_link" imuMsg.orientation = eul_to_qua(ef_l) imuMsg.orientation_covariance = cov_orientation imuMsg.angular_velocity.x, imuMsg.angular_velocity.y, imuMsg.angular_velocity.z = wf_l[0], wf_l[1], wf_l[2] imuMsg.angular_velocity_covariance = cov_angular_velocity imuMsg.linear_acceleration.x, imuMsg.linear_acceleration.y, imuMsg.linear_acceleration.z = af_l[0], af_l[1], af_l[2] imuMsg.linear_acceleration_covariance = cov_linear_acceleration imuPub.publish(imuMsg) time.sleep(0.01)
完整的ROS包地址：https://gitee.com/xjEzekiel/imu
三、分析
1、time.sleep(0.01)对应于原始数据频率100Hz；
2、WT61C一个数据包33字节，程序一次读取65字节，必然包含一个完整的数据包；
1）导致惯导数据发布频率降为100/2=50Hz，对于一般应用毫无问题；
2）结合1、可知，程序绝大多数时间在等待原始数据到来，因此在读取原始数据之后获取时间戳较为准确；
3）Python的串口读取内部采用select，不会堵塞；
3、Python中字节数组=str，UQRS对应于x55x51x52x53；
4、变量accCali是WT61C的加计校准指令；
5、函数eul_to_qua用于欧拉角（RPY）转四元数；
6、变量cov_*是*的协方差；
以上。