【ROS】ubuntu16.04下HANDSFREE_ROS_IMU调试_rostopic echo /handsfree/imu

最近在网上购买了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)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182

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()
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

官方使用的是python，编写了一个发布者和订阅者，通过订阅IMU发布的信息，并将其转换为角度信息，就得到了当前模块的调度信息，这款模块对于ROS而言，使用起来很方便