Handsfree_ros_imu：ROS机器人IMU模块的hfi_a9.py文件学习记录
之前的博客写了关于Handsfree_ros_imu：ROS机器人IMU模块ARHS姿态传感器（A9）Liunx系统Ubuntu20.04学习启动和运行教程：
https://blog.csdn.net/qq_54900679/article/details/135539176?spm=1001.2014.3001.5502
与Handsfree_ros_imu：ROS机器人IMU模块的get_imu_rpy.py文件学习记录：
https://blog.csdn.net/qq_54900679/article/details/135550752?spm=1001.2014.3001.5502
这次带来hfi_a9.py文件的学习与数据记录改进：
hfi_a9.py文件位置如下：
对应的代码如下：
#!/usr/bin/env python
# -*- coding:utf-8 -*-
import serial
import struct
import platform
import serial.tools.list_ports
import math
# 查找 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):
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]
))
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]
if __name__ == "__main__":
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])
这段Python代码主要用于处理与IMU（惯性测量单元）相关的串口通信。以下是对代码的主要功能和组件的详细分析：
-
导入库：
serial：用于串口通信。struct：解析打包的二进制数据。platform：检测操作系统信息，用于判断Python版本。serial.tools.list_ports：列出计算机的串口设备。math：提供数学函数，例如开方。
-
查找ttyUSB设备（
find_ttyUSB函数）：- 打印默认IMU串口设备（
/dev/ttyUSB0），提示用户在识别多个设备时需要在配置文件中修改。 - 列出连接到计算机的所有USB串口设备。
- 打印默认IMU串口设备（
-
CRC校验（
checkSum函数）：- 使用CRC16校验算法来验证数据的完整性。
-
16进制转IEEE浮点数（
hex_to_ieee函数）：- 将16进制数据转换为IEEE标准的浮点数。
- 对Python 2和3使用不同的方法处理数据。
-
处理串口数据（
handleSerialData函数）：- 解析串口接收到的原始数据。
- 检查数据的起始位和数据长度。
- 根据数据类型（角速度、加速度、磁场、欧拉角）进行处理。
- 使用CRC校验判断数据是否有效。
- 计算并打印加速度、角速度、欧拉角和磁场的值。
-
全局变量：
- 定义用于存储数据和标记的全局变量。
-
主函数（
if __name__ == "__main__"）：- 检测Python版本。
- 查找并设置串口。
- 循环读取串口数据并调用
handleSerialData函数处理。
这段代码主要用于从IMU设备接收数据，并通过串口将其转换为可用的浮点数，用于进一步的数据分析或应用。代码在兼容性方面对Python 2和3都进行了考虑，并且具有较强的错误处理和数据校验功能。
改进部分
要将角速度、加速度和磁场数据保存到文本文件或CSV文件中，可以在上述代码的基础上进行一些修改。除了保存到对应路径下的文件中，还要考虑实时地保存数据这个要求，从开始保存数据到结束保存数据的时间是可以自定义的，比如要实现保存3秒时间段内实时的数据。
改进后的代码（imu_data_a9_record.py）如下：
import serial
import struct
import time
import platform
import serial.tools.list_ports
import math
# 其他必要的导入模块和全局变量
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):
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]
# ))
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]
if __name__ == "__main__":
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:
# 定义保存数据的文件名和持续时间
file_name = '/home/hjx/handsfree/imu_data_record/hfi_a9_timer/imu_data_a9_timer.csv' # 可以根据需要更改文件名和路径
duration = 3 # 保存3秒数据，可根据需要调整
start_time = time.time()
end_time = start_time + duration
# 在保存数据之前，添加一个标题行
with open(file_name, 'w') as file:
file.write(
'Timestamp,X_Acceleration,Y_Acceleration,Z_Acceleration,X_AngularVelocity,Y_AngularVelocity,Z_AngularVelocity,Euler_X,Euler_Y,Euler_Z,X_Magnetometer,Y_Magnetometer,Z_Magnetometer\n')
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])
current_time = time.time()
if current_time >= end_time:
# 保存数据并退出循环
with open(file_name, 'a') as file:
file.write(','.join(map(str, [current_time, acceleration[0], acceleration[1], acceleration[2],
angularVelocity[0], angularVelocity[1], angularVelocity[2],
angle_degree[0], angle_degree[1], angle_degree[2],
magnetometer[0], magnetometer[1], magnetometer[2]])) + '\n')
break
else:
# 保存数据并继续接收和处理
with open(file_name, 'a') as file:
file.write(','.join(map(str, [current_time, acceleration[0], acceleration[1], acceleration[2],
angularVelocity[0], angularVelocity[1], angularVelocity[2],
angle_degree[0], angle_degree[1], angle_degree[2],
magnetometer[0], magnetometer[1], magnetometer[2]])) + '\n')
下面给出以上修改后的代码解析：
-
导入模块：
serial：用于处理串口通信。struct：用于处理二进制数据。time：用于处理时间相关的功能。platform：用于获取操作系统信息。serial.tools.list_ports：用于列出系统中的串口设备。math：用于数学运算。
-
函数定义：
find_ttyUSB：检测连接到电脑的USB串口设备，并打印设备列表。checkSum：执行CRC校验，验证数据的完整性。hex_to_ieee：将16进制数据转换为IEEE浮点数。handleSerialData：处理从串口接收到的原始数据。
-
主要逻辑：
- 初始化一些全局变量。
- 在
__main__部分，脚本首先检测Python版本，然后搜索串口设备，并尝试打开特定的串口（默认为/dev/ttyUSB0）。 - 如果串口打开成功，脚本会记录IMU数据到指定文件中（默认路径为
/home/hjx/handsfree/imu_data_record/hfi_a9_timer/imu_data_a9_timer.csv）。 - 数据记录包括加速度、角速度、欧拉角和磁场信息。
- 脚本会在指定时间（默认3秒）后停止记录数据。
-
数据处理：
handleSerialData函数接收串口数据，根据特定的格式解析数据，包括加速度、角速度、欧拉角和磁场等信息。- 进行CRC校验确保数据完整性，然后将数据转换为浮点数进行记录。
-
异常处理：
- 在尝试打开串口或处理数据时，脚本包含异常处理逻辑，以确保在出现错误时能够优雅地处理。
总之，这个脚本主要用于通过串口从IMU设备收集数据，并将其转换为可用的格式进行分析和记录。
配置好conda的环境和ros包的路径后，开始在pycharm中运行:
其中 duration = 3 # 保存3秒数据，可根据需要调整
查看经过3秒后保存好的csv文件：
