话不多说，先上视频看看效果！
用的维特智能的WT61CTTL模块，我用的是这个，大家不一定，看你自己情况！这里先把我的开发记录贴出来！
功能：
通过读取WT61C TTL传感器的加速度、角速度、欧拉角，发布imu数据，在RVIZ下进行可视化显示。
详细组成如下：
cfg：配置文件
存放rviz的启动配置文件imu_display.rviz，在启动RVIZ进行测试的时候可以直接使用我配置好的内容。
include：无内容，无需考虑。
launch：启动文件
imu_driver.launch启动两个节点，其中WT61CTTL节点负责从IMU传感器获取数据，以及进行校准服务；
imu_data.launch节点负责订阅WT61CTTL节点处理好发过来的数据，转换成sensor_msgs/IMU格式的数据发布。
imu_rviz.launch启动调试，可将传感器在现实空间下的姿态进行事实显示。
msg：话题类型文件
WT61CTTL.msg存放WT61C TTL传感器发出的数据类型，里面存储9个float64类型的数据。这是传感器的原始数据。
float64 x_acc <!--X轴加速度-->
float64 y_acc <!--Y轴加速度-->
float64 z_acc <!--Z轴加速度-->
float64 x_gyro <!--X轴角速度-->
float64 y_gyro <!--Y轴角速度-->
float64 z_gyro <!--Z轴角速度-->
float64 roll <!--伏仰角（X轴）-->
float64 pitch <!--翻滚角（Y轴）-->
float64 yaw <!--航向角（Z轴）-->srv：服务类型文件
setAccZero.srv服务负责加速度数据校准，内容如下：
---
int32 status传入任意参数，返回校准结果。
setYawZero.srv服务负责航向角数据校准，内容如下：
---
int32 status传入任意参数，返回校准结果。
随着运行时间的增加，加速度数值可能出现漂移，这时候造成的误差是比较大的，此时让机器人处于静止状态，调用该服务实现加速度数值校准。
航向角数字也是通过数学积分计算得到的，随着时间的增加会出现误差值，调用该服务实现Yaw数值置0。
scripts：脚本文件
demo.py是由厂商提供的示例程序，详情请查看
WT61CTTL.py是我根据示例程序修改得到的，具体请看下面详细注释。
#!/usr/bin/env python3
#coding:utf-8
#声明环境是Python3,编码格式为utf-8
import rospy #导入ros相关文件
from jhr_imu.msg import WT61CTTL #导入自定义话题类型文件
from jhr_imu.srv import * #导入自定义服务类型文件
import serial #导入串口通讯相关文件
#从串口读取的原始数据
ACCData=[0.0]*8
GYROData=[0.0]*8
AngleData=[0.0]*8
FrameState = 0 #通过0x后面的值判断属于哪一种情况
Bytenum = 0 #读取到这一段的第几位
CheckSum = 0 #求和校验位
#转换后的数据存储
a = [0.0]*3 #加速度
w = [0.0]*3 #角速度
Angle = [0.0]*3 #欧拉角
‘’‘
*函数名称：DueData
*函数功能：处理串口的数据，转换成WT61CTTL格式的数据，并发布该消息
*输入参数：inputdata，串口读取的原始数据
*输出参数：无
’‘’
def DueData(inputdata): #新增的核心程序，对读取的数据进行划分，各自读到对应的数组里
global FrameState #在局部修改全局变量，要进行global的定义
global Bytenum
global CheckSum
global a
global w
global Angle
for data in inputdata: #在输入的数据进行遍历
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 #已确定数据代表加速度
if Bytenum<10: # 读取8个数据
ACCData[Bytenum-2]=data # 从0开始
CheckSum+=data
Bytenum+=1
else:
if data == (CheckSum&0xff): #假如校验位正确
a = 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):
w = 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):
Angle = get_angle(AngleData)
d = list(a)+list(w)+list(Angle) #拼装数据
#print("a(g):%10.3f %10.3f %10.3f w(deg/s):%10.3f %10.3f %10.3f Angle(deg):%10.3f %10.3f %10.3f"%d)
jhr_imu = WT61CTTL() #建立WT61CTTL类型的数据jhr_imu
jhr_imu.x_acc = d[0] #提取X轴加速度值
jhr_imu.y_acc = d[1] #提取Y轴加速度值
jhr_imu.z_acc = d[2] #提取Z轴加速度值
jhr_imu.x_gyro = d[3] #提取X轴角速度值
jhr_imu.y_gyro = d[4] #提取Y轴角速度值
jhr_imu.z_gyro = d[5] #提取Z轴角速度值
jhr_imu.roll = d[6] #提取俯仰角值
jhr_imu.pitch = d[7] #提取翻滚角值
jhr_imu.yaw = d[8] #提取航向角值
wt61cttlpub.publish(jhr_imu) #发布数据
rate.sleep()
#标识数据初始化
CheckSum=0
Bytenum=0
FrameState=0
‘’‘
*函数名称：get_acc
*函数功能：通过串口原始数据，计算得到加速度标准数据
*输出参数：加速度数值部分的串口原始数据
*输出参数：X、Y、Z三个的加速度值
*特殊说明：函数get_gyro、函数get_angle和这里一样，具体计算请查阅数据手册
’‘’
def get_acc(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
return acc_x,acc_y,acc_z
def get_gyro(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
return gyro_x,gyro_y,gyro_z
def get_angle(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
return angle_x,angle_y,angle_z
‘’‘
*函数名称：set_acc_zero
*函数功能：通过串口发送对应的指令，实现加速度数值的校准
*输入参数：无
*输出参数：返回status值
’‘’
def set_acc_zero(req):
ser.write(b'\xFF\xAA\x67')
status = 0
return setAccZeroResponse(status)
‘’‘
*函数名称：set_yaw_zero
*函数功能：通过串口发送对应的指令，实现航向角数值的校准
*输入参数：无
*输出参数：返回status值
’‘’
def set_yaw_zero(req):
ser.write(b'\xFF\xAA\x52')
status = 0
return setYawZeroResponse(status)
if __name__=='__main__':
rospy.init_node('WT61CTTL') #初始化节点WT61CTTL
wt61cttlpub = rospy.Publisher('WT61CTTL', WT61CTTL, queue_size=10) #发布WT61CTTL数据
setacczero = rospy.Service('set_acc_zero', setAccZero, set_acc_zero) #创建加速度校准服务
setyawzero = rospy.Service('set_yaw_zero', setYawZero, set_yaw_zero) #创建航向角校准服务
rate = rospy.Rate(20) #以20Hz的频率发布
ser = serial.Serial('/dev/ttyUSB0',9600, timeout=0.5) #创建串口并打开
print(ser.is_open) #输出串口当前状态
while not rospy.is_shutdown(): #只要ros不关闭
datahex = ser.read(33) #串口读取数据
DueData(datahex) #处理数据校准通过发起服务请求完成。
imu_node.py是订阅WT61CTTL的数据，以sensor_msgs/IMU的格式发布，具体请看下面详细注释。
#!/usr/bin/env python2
#coding:utf-8
#声明环境为Python2,编码格式为utf-8
#因为出现了tf2在Python3不兼容的问题，所以降为使用Python2
import rospy #导入ros相关文件
import string
import math
import time
import sys
from tf.transformations import quaternion_from_euler #导入tf计算相关文件
from std_msgs.msg import Float32 #导入std_msgs的Float32话题类型文件
from sensor_msgs.msg import Imu #导入sensor_msgs的Imu话题类型文件
from jhr_imu.msg import WT61CTTL #导入jhr_imu的WT61CTTL话题类型文件
degrees2rad = math.pi/180.0 #角度值转弧度值参数
imu_yaw_calibration = 0.0 #yaw校准预留，未使用可删除
accel_factor = 9.806/256.0 #加速度转换计算
seq = 0 #计数
‘’‘
*函数名称：WT61CTTL_callback
*函数功能：订阅回调函数，当订阅到WT61CTTL话题时执行，将WT61CTTL的数据格式转成Imu的数据格式，并发布
*输入参数：WT61CTTL消息耳的内容data
*输出参数：无
’‘’
def WT61CTTL_callback(data):
yaw_deg = data.yaw #获取yaw
yaw_deg = yaw_deg + imu_yaw_calibration #yaw校准，加0,老版本使用的，没有修改
yaw = yaw_deg*degrees2rad //yaw转弧度
pitch = float(data.pitch)*degrees2rad //获取pitch转弧度
roll = float(data.roll)*degrees2rad //获取roll转弧度
#Imu的加速度
imuMsg.linear_acceleration.x = float(data.x_acc)*accel_factor
imuMsg.linear_acceleration.y = float(data.y_acc)*accel_factor
imuMsg.linear_acceleration.z = float(data.z_acc)*accel_factor
#Imu的角速度
imuMsg.angular_velocity.x = float(data.x_gyro)*degrees2rad
imuMsg.angular_velocity.y = float(data.y_gyro)*degrees2rad
imuMsg.angular_velocity.z = float(data.z_gyro)*degrees2rad
#将欧拉角转成四元数
q = quaternion_from_euler(roll, pitch, yaw)
imuMsg.orientation.x = q[0]
imuMsg.orientation.y = q[1]
imuMsg.orientation.z = q[2]
imuMsg.orientation.w = q[3]
#四元数协方差估计矩阵
# Orientation covariance estimation
imuMsg.orientation_covariance = [
0.0025 , 0 , 0,
0, 0.0025, 0,
0, 0, 0.0025
]
#角速度协方差估计矩阵
# Angular velocity covariance estimation
imuMsg.angular_velocity_covariance = [
0.02, 0 , 0,
0 , 0.02, 0,
0 , 0 , 0.02
]
#加速度协方差估计矩阵
# linear acceleration covariance estimation
imuMsg.linear_acceleration_covariance = [
0.04 , 0 , 0,
0 , 0.04, 0,
0 , 0 , 0.04
]
#上面参数我是抄ROS小课堂张志杰老师的，有兴趣你可以自己算算，我这里能用
#Imu数据头部标识
global seq
imuMsg.header.stamp= rospy.Time.now()
imuMsg.header.frame_id = "base_imu_link"
imuMsg.header.seq = seq
seq = seq + 1
data_pub.publish(imuMsg) #发布Imu数据
rate.sleep()
rospy.init_node("imu_node") #初始化节点并命名为imu_node
data_pub = rospy.Publisher("imu_data", Imu, queue_size=1) #定义imu数据发布
rospy.Subscriber('WT61CTTL', WT61CTTL, WT61CTTL_callback) #定义WT61CTTL数据订阅
imuMsg = Imu() #实例化Imu类似数据
rate = rospy.Rate(20) #以20Hz的频率处理
rospy.spin()以上两部分为核心代码，WT61CTTL获取数据给imu_data，imu_data处理后以Imu的格式发布出来。
校准的功能我写在WT61CTTL里面的，因为校准是通过串口发送数据来实现的，WT61CTTL可以理解为IMU传感器的驱动功能包，而imu_data则是IMU传感器数据的处理功能包。后期维护请注意你要修改的是什么，不要同时都修改！！！
CMakeList.txt和packahe.xml的内容自行阅读。
遗留的BUG：
IMU启动大约20分钟左右会出现数据延迟2s的情况，重新校准加速度即可，延迟2s不影响使用，但是希望得到解决。
IMU的Yaw（Z轴）数值启动后存在误差，且随着时间推移误差值十分明显，特别是在剧烈运动时，猜测是积分计算的丢失，希望得到解决。
怎么说呢，抄了ROS小课堂张志杰老师的代码，挺高效的！传送门
感谢张老师！大家都是写代码的，都不容易！张老师写的比我的完善也更加准确，支持正版！
这里为大家分享一下我的一些思路！
唔！从视频来看的话，效果挺不错的，实时的姿态在RVIZ下显示，这玩意在小车啊、无人机啊、潜艇啊等等，都是比较好玩的！
最最最主要的是，看着有逼格！适合ZB！！！
思路的话，就是发布出来一个sensor_msgs下的Imu类型的话题！
其他的话，比如什么校准啊、置零啊什么的，看你自己需求！
sensor_msgs的Imu类型具体是啥，大家去ROS的官网找下吧！传送门
# This is a message to hold data from an IMU (Inertial Measurement Unit)
#这是一个保存来自IMU的数据的消息(惯性测量单元)
#
# Accelerations should be in m/s^2 (not in g's), and rotational velocity should be in rad/sec
#加速度的单位应该是m/s^2(不是g)，旋转速度的单位应该是rad/sec
#
# If the covariance of the measurement is known, it should be filled in (if all you know is the
# variance of each measurement, e.g. from the datasheet, just put those along the diagonal)
# A covariance matrix of all zeros will be interpreted as "covariance unknown", and to use the
# data a covariance will have to be assumed or gotten from some other source
##如果测量的协方差是已知的，就应该填写(如果你只知道
#每个测量值的方差，例如数据表的方差，把它们放在对角线上)
#所有零的协方差矩阵将被解释为“协方差未知”，并使用
# data a covariance必须是假设的，或者是从其他来源获得的
#
# If you have no estimate for one of the data elements (e.g. your IMU doesn't produce an orientation estimate), please set element 0 of the associated covariance matrix to -1
# If you are interpreting this message, please check for a value of -1 in the first element of each
# covariance matrix, and disregard the associated estimate.
#如果你对其中一个数据元素没有估计(例如，你的IMU没有产生方向估计)，请将相关协方差矩阵的元素0设置为-1
#如果你正在解释这个消息，请检查每个元素的第一个元素的值为-1
#协方差矩阵，并忽略相关的估计。
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 其实也就是线性加速度、角速度、四元数！这些工作的话，Arduino+MPU6050完全可以完成，但是就是四元数这个玩意不好搞，Arduino的MPU6050库里面有DMP的算法，貌似不是很准确，有机会的话试试，有兴趣的小伙伴可以按照这个思路试试！
就是按照sensor_msgs的Imu数据类型发布个msg就可以了！！！协方差矩阵这玩意，我也不会算，这里抄张志杰老师，能使！
维特智能这家的陀螺仪吧，内部带了算法，数据准确度还是可以的，不过要求高的话，还是再找找吧！之前用过北京一家叫做三驰惯导的，他们家的陀螺仪还不错，就是加个不亲民，得几千块钱！
这里没有打广告，凭心而论！
我这里用的是WT61CTTL，串口通讯，输出加速度、角速度、欧拉角，还有就是可以置零欧拉角的Yaw航向角数值，因为这个数值是积分出来的，会漂移，融合地磁也就是九轴的陀螺仪会好些！还有就是加速度数值的置零，这里也差不多算是校准吧，虽然这个模块自带了校准，但是准不准另算！出问题的时候手动搞下还是可以的！
置零也简单，官方提供的资料很详细，串口发送命令就可以了，我这里做的是服务调用，因为我得知道结果是什么样的！返回值这里我偷懒了，只是简单的返回了一个结果！后期改进的话，应该会将校准置零后的数据给反馈出来，便于参考！
需要代码的话，在古月居公众号（微信）要吧，贴百度网盘老是被和谐，也容易过期！
突然发现，博客可以加表情了，不错不错！社区tql！！！
最后吧，我还是说一句话~
ROS = 通讯机制+工具箱+功能包+社区
ROS只是一个工具，它不是核心。你的思想、你的代码才是项目的核心，一定一定一定不要本末倒置。
初入门者，切记！
前两天也在社区看到了大佬们的语句，这一句我确实印象深刻而且将自己对号入座！
随着编程年龄的增长，慢慢的开始坚信一句话：“你写的代码，永远比不上开源那些大牛们，所以放弃抵抗，拥抱开源吧。”我从不写代码，我只是代码的搬运工！（玩笑话，有感而发，不要认真！）
评论（7）
您还未登录，请登录后发表或查看评论