【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而言，使用起來很友善