【ROS】ubuntu16.04下HANDSFREE_ROS_IMU调试
- 一、准备工作
-
- 1.下载驱动软件压缩包
- 2.安装 ros_imu能依赖包
- 3.创建工作空间
- 4.编译并设置环境变量
- 5.检查是否能检测到设备
- 二、IMU模块功能包的使用
- 三、IMU功能包代码解析
-
- 1.hfi_imu_ros.py
- 2.get_imu_rpy.py
最近在网上购买了ROS机器人IMU模块ARHS姿态传感器，购买的型号是HFI-B6，下面是官方给出的资料链接，这种信号的IMU是ROS操作系统专用的，能够直接读取当前模块的角度信息，并打印出来，而且传输速率非常快，相比较于传统的imu模块而言，使用起来更加的方便，因为官方已经完成了底层代码的编写，我们在使用上不需要考虑通信的问题
链接: https://gitee.com/HANDS-FREE/handsfree_ros_imu/blob/master/tutorials/doc.md.
如果你没有gitee账号的话，需要重新注册一个，才能下载gitee网站上的ROS驱动功能包
一、准备工作
1.下载驱动软件压缩包
博主的linux系统是ubuntu16.04，安装的是ros-kinetic版本，对应的python版本是python2，点击python2右边的下载驱动软件压缩包，即可完成驱动软件压缩包的下载
2.安装 ros_imu能依赖包
打开一个终端，在终端中输入，这里是针对kinetic版本的命令，如果你们是其他版本的话，还是安装官方教程来进行下载安装
sudo apt-get install ros-kinetic-imu-tools ros-kinetic-rviz-imu-plugin
sudo apt-get install python-visual
3.创建工作空间
在终端中输入以下指令
mkdir -p ~/handsfree/handsfree_ros_ws/src/
cd ~/handsfree/handsfree_ros_ws/src/
将之前下载好的软件驱动压缩包解压后，提取到 src 目录下，正常情况下提取出来的压缩包文件名是handsfree_ros_imu，如果不是请修改成 handsfree_ros_imu
4.编译并设置环境变量
cd ~/handsfree/handsfree_ros_ws/
catkin_make
cd ~/handsfree/handsfree_ros_ws/src/handsfree_ros_imu/scripts/
sudo chmod 777 *.py
echo “source ~/handsfree/handsfree_ros_ws/devel/setup.bash” >> ~/.bashrc
source ~/.bashrc
5.检查是否能检测到设备
连接 IMU 的 USB，检查电脑能否识别到 ttyUSB0，检测到 ttyUSB0 后，给 ttyUSB0 赋权限
ls /dev/ttyUSB0
sudo chmod 777 /dev/ttyUSB0
二、IMU模块功能包的使用
在下载的驱动软件功能包中，我们可以看到，一共有6个.py文件，其中，有四个是对应于不同IMU模块的.py文件，get_imu_rpy.py文件是读取当前模块的角度信息值，包括Roll（翻滚角），Pitch（俯仰角）和Yaw（偏航角）
如果我们要在终端下实时读取当前模块的角度信息，我们先要将IMU模块与ubuntu系统连接上，打开终端，输入以下命令
ls /dev/ttyUSB0
sudo chmod 777 /dev/ttyUSB0
然后在终端中输入roscore，启动ros_master，重新打开一个终端，在终端中输入命令rosrun handsfree_ros_imu hfi_b6_ros.py，这里要根据你购买的IMU模块的型号进行选择，然后在重新打开一个终端，在终端中输入rosrun handsfree_ros_imu get_imu_rpy.py，你就会看到实时打印的角度信息了
rosrun handsfree_ros_imu hfi_b6_ros.py
rosrun handsfree_ros_imu get_imu_rpy.py
相比于其他的一些可视化界面，读取当前模块的角度信息是最重要的，其他相关部分功能的，大家可以自行去官网上进行查看，如果要使用rviz来进行查看IMU模块的变化信息的话，也可以roslaunch功能包中的其他launch文件，也很方便
三、IMU功能包代码解析
1.hfi_imu_ros.py
#!/usr/bin/env python
# -*- coding:utf-8 -*-
import math
import serial
import struct
import time
import rospy
import serial.tools.list_ports
from geometry_msgs.msg import Quaternion
from sensor_msgs.msg import Imu
from sensor_msgs.msg import MagneticField
cov_orientation = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
cov_angular_velocity = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
cov_linear_acceleration = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
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
# 在缓冲数据中找到第一个包的起始位置
def find_first_package(buffer):
i = 0
while True:
if buffer[i] == 0x55 and (buffer[i + 1] & 0x50) == 0x50:
return i
if i + 2 >= len(buffer):
return -1
i += 1
# 检查校验和
def sb_sum_chech(byte_temp):
# if(len(byte_temp)==11) :
if (((byte_temp[0] + byte_temp[1] + byte_temp[2] + byte_temp[3] + byte_temp[4] + byte_temp[5] + byte_temp[6] +
byte_temp[7] + byte_temp[8] + byte_temp[9]) & 0xff) == byte_temp[10]):
# print('sum check ok!')
return True
else:
# print('sum check false!')
return False
# 查找 ttyUSB* 设备
def find_ttyUSB():
count = 0
port_list = []
for ser in serial.tools.list_ports.comports():
if str(ser.name).find("USB") == 3:
print("\033[32m找到了:" + str(ser.name) + " 设备\033[0m")
port_list.append(str(ser.name))
else:
count += 1
if count == len(list(serial.tools.list_ports.comports())):
print("\033[31m没有找到相关的 ttyUSB* 设备\033[0m")
exit(0)
return port_list
if __name__ == "__main__":
port_list = find_ttyUSB()
rospy.init_node("imu")
port = rospy.get_param("~port", "/dev/ttyUSB0")
baudrate = rospy.get_param("~baudrate", 921600)
try:
hf_imu = serial.Serial(port=port, baudrate=baudrate, timeout=0.5)
if hf_imu.isOpen():
rospy.loginfo("\033[32m串口打开成功...\033[0m")
else:
hf_imu.open()
rospy.loginfo("\033[32m打开串口成功...\033[0m")
except Exception as e:
print(e)
rospy.loginfo("\033[31m串口错误，其他因素\033[0m")
exit(0)
else:
imu_pub = rospy.Publisher("handsfree/imu", Imu, queue_size=10)
receive_buffer = bytearray()
linear_acceleration_x = 0
linear_acceleration_y = 0
linear_acceleration_z = 0
angular_velocity_x = 0
angular_velocity_y = 0
angular_velocity_z = 0
data_timeout = 0
while not rospy.is_shutdown():
if data_timeout < 1000:
data_timeout += 1
else:
print("\033[31m读取不到 imu 数据，当前 ttyUSB0 设备不是 imu\033[0m")
exit(0)
eul = []
try:
count = hf_imu.inWaiting()
except Exception as e:
print(e)
print ("\033[31mimu 失联\033[0m")
exit(0)
else:
if count > 0:
s = hf_imu.read(count)
receive_buffer += s
stamp = rospy.get_rostime()
dataLen = len(receive_buffer)
if dataLen >= 11:
# 去掉第1个包头前的数据
headerPos = find_first_package(receive_buffer)
if headerPos >= 0:
if headerPos > 0:
receive_buffer[0:headerPos] = b''
# 取 Config.minPackageLen 整数倍长度的数据
if dataLen - headerPos >= 11:
packageCount = int((dataLen - headerPos) / 11)
if packageCount > 0:
cutLen = packageCount * 11
temp = receive_buffer[0:cutLen]
# 按16进制字符串的形式显示收到的内容
receive_buffer[0:cutLen] = b''
# 解析数据,逐个数据包进行解析
for i in range(packageCount):
beginIdx = int(i * 11)
endIdx = int(i * 11 + 11)
byte_temp = temp[beginIdx:endIdx]
# 校验和通过了的数据包才进行解析
imu_msg = Imu()
imu_msg.header.stamp = stamp
imu_msg.header.frame_id = "base_link"
mag_msg = MagneticField()
mag_msg.header.stamp = stamp
mag_msg.header.frame_id = "base_link"
if sb_sum_chech(byte_temp):
data_timeout = 0
Data = list(struct.unpack("hhhh", byte_temp[2:10]))
# 加速度
if byte_temp[1] == 0x51:
linear_acceleration_x = Data[0] / 32768.0 * 16 * -9.8
linear_acceleration_y = Data[1] / 32768.0 * 16 * -9.8
linear_acceleration_z = Data[2] / 32768.0 * 16 * -9.8
# 角速度
if byte_temp[1] == 0x52:
imu_msg.orientation_covariance = cov_orientation
angular_velocity_x = Data[0] / 32768.0 * 2000 * math.pi / 180
angular_velocity_y = Data[1] / 32768.0 * 2000 * math.pi / 180
angular_velocity_z = Data[2] / 32768.0 * 2000 * math.pi / 180
# 姿态角
if byte_temp[1] == 0x53:
for i in range(3):
eul.append(Data[i] / 32768.0 * math.pi)
imu_msg.orientation = eul_to_qua(eul)
imu_msg.linear_acceleration.x = linear_acceleration_x
imu_msg.linear_acceleration.y = linear_acceleration_y
imu_msg.linear_acceleration.z = linear_acceleration_z
imu_msg.angular_velocity.x = angular_velocity_x
imu_msg.angular_velocity.y = angular_velocity_y
imu_msg.angular_velocity.z = angular_velocity_z
imu_msg.angular_velocity_covariance = cov_angular_velocity
imu_msg.linear_acceleration_covariance = cov_linear_acceleration
imu_pub.publish(imu_msg)
time.sleep(0.001)
2.get_imu_rpy.py
#!/usr/bin/env python
#coding=UTF-8
import rospy
import tf
from tf.transformations import *
from sensor_msgs.msg import Imu
def callback(data):
#这个函数是tf中的,可以将四元数转成欧拉角
(r,p,y) = tf.transformations.euler_from_quaternion((data.orientation.x,data.orientation.y,data.orientation.z,data.orientation.w))
#由于是弧度制，下面将其改成角度制看起来更方便
rospy.loginfo("Roll = %f, Pitch = %f, Yaw = %f",r*180/3.1415926,p*180/3.1415926,y*180/3.1415926)
def get_imu():
rospy.init_node('get_imu', anonymous=True)
rospy.Subscriber("/handsfree/imu", Imu, callback) #接受topic名称
rospy.spin()
if __name__ == '__main__':
get_imu()
官方使用的是python，编写了一个发布者和订阅者，通过订阅IMU发布的信息，并将其转换为角度信息，就得到了当前模块的调度信息，这款模块对于ROS而言，使用起来很方便