Handsfree_ros_imu:ROS机器人IMU模块的hfi_a9.py文件学习记录

16 篇文章 0 订阅
10 篇文章 6 订阅

之前的博客写了关于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(惯性测量单元)相关的串口通信。以下是对代码的主要功能和组件的详细分析:

  1. 导入库

    • serial:用于串口通信。
    • struct:解析打包的二进制数据。
    • platform:检测操作系统信息,用于判断Python版本。
    • serial.tools.list_ports:列出计算机的串口设备。
    • math:提供数学函数,例如开方。
  2. 查找ttyUSB设备(find_ttyUSB函数)

    • 打印默认IMU串口设备(/dev/ttyUSB0),提示用户在识别多个设备时需要在配置文件中修改。
    • 列出连接到计算机的所有USB串口设备。
  3. CRC校验(checkSum函数)

    • 使用CRC16校验算法来验证数据的完整性。
  4. 16进制转IEEE浮点数(hex_to_ieee函数)

    • 将16进制数据转换为IEEE标准的浮点数。
    • 对Python 2和3使用不同的方法处理数据。
  5. 处理串口数据(handleSerialData函数)

    • 解析串口接收到的原始数据。
    • 检查数据的起始位和数据长度。
    • 根据数据类型(角速度、加速度、磁场、欧拉角)进行处理。
    • 使用CRC校验判断数据是否有效。
    • 计算并打印加速度、角速度、欧拉角和磁场的值。
  6. 全局变量

    • 定义用于存储数据和标记的全局变量。
  7. 主函数(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')

 下面给出以上修改后的代码解析:

  1. 导入模块

    • serial:用于处理串口通信。
    • struct:用于处理二进制数据。
    • time:用于处理时间相关的功能。
    • platform:用于获取操作系统信息。
    • serial.tools.list_ports:用于列出系统中的串口设备。
    • math:用于数学运算。
  2. 函数定义

    • find_ttyUSB:检测连接到电脑的USB串口设备,并打印设备列表。
    • checkSum:执行CRC校验,验证数据的完整性。
    • hex_to_ieee:将16进制数据转换为IEEE浮点数。
    • handleSerialData:处理从串口接收到的原始数据。
  3. 主要逻辑

    • 初始化一些全局变量。
    • __main__部分,脚本首先检测Python版本,然后搜索串口设备,并尝试打开特定的串口(默认为/dev/ttyUSB0)。
    • 如果串口打开成功,脚本会记录IMU数据到指定文件中(默认路径为/home/hjx/handsfree/imu_data_record/hfi_a9_timer/imu_data_a9_timer.csv)。
    • 数据记录包括加速度、角速度、欧拉角和磁场信息。
    • 脚本会在指定时间(默认3秒)后停止记录数据。
  4. 数据处理

    • handleSerialData函数接收串口数据,根据特定的格式解析数据,包括加速度、角速度、欧拉角和磁场等信息。
    • 进行CRC校验确保数据完整性,然后将数据转换为浮点数进行记录。
  5. 异常处理

    • 在尝试打开串口或处理数据时,脚本包含异常处理逻辑,以确保在出现错误时能够优雅地处理。

        总之,这个脚本主要用于通过串口从IMU设备收集数据,并将其转换为可用的格式进行分析和记录。

配置好conda的环境和ros包的路径后,开始在pycharm中运行:

其中  duration = 3  # 保存3秒数据,可根据需要调整

查看经过3秒后保存好的csv文件:

    

  • 22
    点赞
  • 26
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 1
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

随机惯性粒子群

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值