hfi_a9 IMU数据接收与发布

概述

由于目前手上的IMU型号为hfi_a9，该型号IMU没有C++版本的驱动，找到的只有python版本的驱动，所以基于python版本的驱动进行代码编写。找到的demo可以直接运行并将IMU的数据打印到终端上，如下图所示：

需要完成的任务是将各个数据以字符串的形式发布出去。我的做法：首先是将数据写进一个.txt文件中，再打开该文件，通过readline()函数，将数据整行保存进byt中，再将byt中的数据以字符串的形式放进msg.data中，最后通过话题/command将msg.data中的数据发布出去。

将数据写进.txt文件中的程序

data = []
data=open("/home/wjf/桌面/data.txt",'w+')
print("%.2f" %float(acceleration[0] * -9.8 / acc_k), "%.2f" % float(acceleration[1] * -9.8 / acc_k), "%.2f" % float(acceleration[2] * -9.8 / acc_k),
"%.2f" % angularVelocity[0], "%.2f" % angularVelocity[1], "%.2f" % angularVelocity[2],
"%.2f" %angle_degree[0], "%.2f" % angle_degree[1], "%.2f" % angle_degree[2],
"%.2f" % magnetometer[0], "%.2f" % magnetometer[1],"%.2f" % magnetometer[2],file=data)
data.close()

读写数据并发布的程序

msg.data = str(byt)
self.command_publisher_.publish(msg) 
self.get_logger().info(f'发布了指令：{msg.data}') #打印一下发布的数据msg = String()f = open("/home/wjf/桌面/data.txt")

#读取一行数据

byt = f.readline()

print(byt)

具体操作步骤

1.创建功能包和节点

cd chapt3/chapt3_ws/src/
ros2 pkg create example_topic_rclpy  --build-type ament_python --dependencies rclpy

2.创建节点文件

cd example_topic_rclpy/example_topic_rclpy
touch topic_subscribe_02.py
touch topic_publisher_02.py

3.编写代码

代码在下面的topic_publisher_02.py中有全部展示。

4.运行节点

cd imu01_ws/
colcon build
source install/setup.bash
ros2 run example_topic_rclpy topic_subscribe_02

5.结果

topic_publisher_02.py

#!/usr/bin/env python
# -*- coding:utf-8 -*-
import serial
import struct
import platform
import serial.tools.list_ports
import math
import rclpy
from rclpy.node import Node
from std_msgs.msg import String

python_version = platform.python_version()[0]

# 查找 ttyUSB* 设备
def find_ttyUSB():
print('imu 默认串口为 /dev/ttyUSB0, 若识别多个串口设备, 请在 launch 文件中修改 imu 对应的串口')
posts = [port.device for port in serial.tools.list_ports.comports() if 'USB' in port.device]
print('当前电脑所连接的 {} 串口设备共 {} 个: {}'.format('USB', len(posts), posts))

# crc 校验
def checkSum(list_data, check_data):
data = bytearray(list_data)
crc = 0xFFFF
for pos in data:
crc ^= pos
for i in range(8):
if (crc & 1) != 0:
crc >>= 1
crc ^= 0xA001
else:
crc >>= 1
return hex(((crc & 0xff) << 8) + (crc >> 8)) == hex(check_data[0] << 8 | check_data[1])


# 16 进制转 ieee 浮点数
def hex_to_ieee(raw_data):
ieee_data = []
raw_data.reverse()
for i in range(0, len(raw_data), 4):
data2str =hex(raw_data[i] | 0xff00)[4:6] + hex(raw_data[i + 1] | 0xff00)[4:6] + hex(raw_data[i + 2] | 0xff00)[4:6] + hex(raw_data[i + 3] | 0xff00)[4:6]
if python_version == '2':
ieee_data.append(struct.unpack('>f', data2str.decode('hex'))[0])
if python_version == '3':
ieee_data.append(struct.unpack('>f', bytes.fromhex(data2str))[0])
ieee_data.reverse()
return ieee_data


# 处理串口数据
def handleSerialData(raw_data):

#print(raw_data)
global buff, key, angle_degree, magnetometer, acceleration, angularVelocity, pub_flag
if python_version == '2':
buff[key] = ord(raw_data)
if python_version == '3':
buff[key] = raw_data



key += 1
if buff[0] != 0xaa:
key = 0
return
if key < 3:
return
if buff[1] != 0x55:
key = 0
return
if key < buff[2] + 5: # 根据数据长度位的判断, 来获取对应长度数据
return

else:
data_buff = list(buff.values()) # 获取字典所以 value


if buff[2] == 0x2c and pub_flag[0]:
if checkSum(data_buff[2:47], data_buff[47:49]):
data = hex_to_ieee(data_buff[7:47])
angularVelocity = data[1:4]
acceleration = data[4:7]
magnetometer = data[7:10]
else:
print('校验失败')
pub_flag[0] = False
elif buff[2] == 0x14 and pub_flag[1]:
if checkSum(data_buff[2:23], data_buff[23:25]):
data = hex_to_ieee(data_buff[7:23])
angle_degree = data[1:4]
else:
print('校验失败')
pub_flag[1] = False
else:
print("该数据处理类没有提供该 " + str(buff[2]) + " 的解析")
print("或数据错误")
buff = {}
key = 0

buff = {}
key = 0
if pub_flag[0] == True or pub_flag[1] == True:
return
pub_flag[0] = pub_flag[1] = True
acc_k = math.sqrt(acceleration[0] ** 2 + acceleration[1] ** 2 + acceleration[2] ** 2)

# print('''
# 加速度(m/s²)：
# x轴：%.2f
# y轴：%.2f
# z轴：%.2f

# 角速度(rad/s)：
# x轴：%.2f
# y轴：%.2f
# z轴：%.2f

# 欧拉角(°)：
# x轴：%.2f
# y轴：%.2f
# z轴：%.2f

# 磁场：
# x轴：%.2f
# y轴：%.2f
# z轴：%.2f
# ''' % (acceleration[0] * -9.8 / acc_k, acceleration[1] * -9.8 / acc_k, acceleration[2] * -9.8 / acc_k,
# angularVelocity[0], angularVelocity[1], angularVelocity[2],
# angle_degree[0], angle_degree[1], angle_degree[2],
# magnetometer[0], magnetometer[1], magnetometer[2]
# ))

data = []
data=open("/home/wjf/桌面/data.txt",'w+')
print("%.2f" %float(acceleration[0] * -9.8 / acc_k), "%.2f" % float(acceleration[1] * -9.8 / acc_k), "%.2f" % float(acceleration[2] * -9.8 / acc_k),
"%.2f" % angularVelocity[0], "%.2f" % angularVelocity[1], "%.2f" % angularVelocity[2],
"%.2f" %angle_degree[0], "%.2f" % angle_degree[1], "%.2f" % angle_degree[2],
"%.2f" % magnetometer[0], "%.2f" % magnetometer[1],"%.2f" % magnetometer[2],file=data)
data.close()
key = 0
flag = 0
buff = {}
angularVelocity = [0, 0, 0]
acceleration = [0, 0, 0]
magnetometer = [0, 0, 0]
angle_degree = [0, 0, 0]
pub_flag = [True, True]

class NodePublisher02(Node):
def __init__(self,name):
super().__init__(name)
self.get_logger().info("大家好，我是%s!" % name)
self.command_publisher_ = self.create_publisher(String,"command", 10) 
self.timer = self.create_timer(0.5, self.timer_callback)
def timer_callback(self):
"""
定时器回调函数
"""
#python_version = platform.python_version()[0]

find_ttyUSB()
port = "/dev/ttyUSB0"
baudrate = 921600

try:
hf_imu = serial.Serial(port=port, baudrate=baudrate, timeout=0.5)
if hf_imu.isOpen():
print("\033[32m串口打开成功...\033[0m")
else:
hf_imu.open()
print("\033[32m打开串口成功...\033[0m")
except Exception as e:
print(e)
print("\033[31m串口打开失败\033[0m")
exit(0)
else:
while True:
try:
buff_count = hf_imu.inWaiting()
except Exception as e:
print("exception:" + str(e))
print("imu 失去连接，接触不良，或断线")
exit(0)
else:
if buff_count > 0:
buff_data = hf_imu.read(buff_count)
for i in range(0, buff_count):
handleSerialData(buff_data[i]) 
f = open("/home/wjf/桌面/data.txt")

#读取一行数据

byt = f.readline()

print(byt)
#acc_k = math.sqrt(acceleration[0] ** 2 + acceleration[1] ** 2 + acceleration[2] ** 2)
msg = String()
#msg.data =str("%.2f" %float(acceleration[0] * -9.8 / acc_k))+" "+str("%.2f" % float(acceleration[1] * -9.8 / acc_k))+" "+str("%.2f" % float(acceleration[2] * -9.8 / acc_k))+" "+str("%.2f" % angularVelocity[0])+" "+str("%.2f" % angularVelocity[1])+" "+str("%.2f" % angularVelocity[2])+" "+str("%.2f" %angle_degree[0])+" "+str("%.2f" % angle_degree[1])+" "+str("%.2f" % angle_degree[2])+" "+str("%.2f" % magnetometer[0])+" "+str("%.2f" % magnetometer[1])+" "+str("%.2f" % magnetometer[2])
msg.data = str(byt)
self.command_publisher_.publish(msg) 
self.get_logger().info(f'发布了指令：{msg.data}') #打印一下发布的数据




def main(args=None):
rclpy.init(args=args) # 初始化rclpy
node = NodePublisher02("topic_publisher_02") # 新建一个节点
rclpy.spin(node) # 保持节点运行，检测是否收到退出指令（Ctrl+C）
rclpy.shutdown() # 关闭rclpy

setup.py

from setuptools import setup

package_name = 'example_topic_rclpy'

setup(
name=package_name,
version='0.0.0',
packages=[package_name],
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='wjf',
maintainer_email='wjf@todo.todo',
description='TODO: Package description',
license='TODO: License declaration',
tests_require=['pytest'],
entry_points={
'console_scripts': [
"topic_publisher_02 = example_topic_rclpy.topic_publisher_02:main",
"topic_subscribe_02 = example_topic_rclpy.topic_subscribe_02:main"
],
},
)