Handsfree_ros_imu：ROS机器人IMU模块的get_imu

上一篇博客写了关于Handsfree_ros_imu：ROS机器人IMU模块ARHS姿态传感器（A9）Liunx系统Ubuntu20.04学习启动和运行教程：

https://blog.csdn.net/qq_54900679/article/details/135539176?spm=1001.2014.3001.5502

这次带来get_imu_rpy.py文件的学习与数据记录改进：

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代码是一个ROS节点的代码示例，用于订阅名为/handsfree/imu的ROS话题（topic），该话题发布了传感器的IMU（惯性测量单元）数据。

以下是代码的主要功能和结构解释：

导入必要的Python库和ROS消息类型：
- rospy：ROS Python库，用于编写ROS节点。
- tf：ROS中的变换库，用于进行坐标变换。
- from tf.transformations import *：导入tf库中的变换函数。
- from sensor_msgs.msg import Imu：导入ROS消息类型Imu，用于订阅IMU数据。
callback函数：
- callback函数是一个回调函数，它会在接收到/handsfree/imu话题的新消息时被调用。
- 在回调函数内部，通过tf.transformations.euler_from_quaternion函数将四元数转换为欧拉角（Roll、Pitch、Yaw）。
- 使用rospy.loginfo函数将欧拉角以角度制格式打印到ROS日志中。
get_imu函数：
- get_imu函数是主函数，用于初始化ROS节点并设置话题订阅。
- rospy.init_node用于初始化ROS节点，其中'get_imu'是节点的名称，anonymous=True表示使节点名称唯一。
- rospy.Subscriber用于订阅/handsfree/imu话题，当有新消息发布到该话题时，将调用callback函数进行处理。
- rospy.spin()使节点保持运行状态，等待新消息的到来。
if __name__ == '__main__':：
- 这是Python的条件语句，用于检查脚本是否被直接运行而不是被导入到其他脚本中。
- 如果脚本被直接运行，则调用get_imu()函数启动ROS节点并开始订阅IMU数据。

总的来说，这个脚本是一个ROS节点，用于订阅IMU数据并将其转换为欧拉角形式，然后将结果打印到ROS日志中。这对于在ROS中处理IMU数据非常有用。

该文件在linux终端运行的代码和结果如下：

rosrun handsfree_ros_imu get_imu_rpy.py

改进部分

光在终端输出数据还不够，如果想要将其输出的这些rpy数据实时地保存成文本文件或者csv格式的文件，需要对原始代码进行改进，改进后的代码（imu_data_record.py）如下：

import rospy
import tf
from tf.transformations import *
from sensor_msgs.msg import Imu
import time

# 定义记录开始和结束时间
start_time = time.time()  # 记录开始时间
record_duration = 6  # 设置记录持续时间，这里设置为6秒，您可以根据需要进行调整
end_time = start_time + record_duration  # 记录结束时间

# 打开文件以写入数据
file_path = '/home/hjx/handsfree/imu_data_record/rpy_timer/imu_data_rpy_timer.txt'  # txt文件路径
file_path = '/home/hjx/handsfree/imu_data_record/rpy_timer/imu_data_rpy_timer.csv'  # csv文件路径
file = open(file_path, 'w')

def callback(data):
    if time.time() < end_time:  # 在规定的时间段内执行记录
        (r, p, y) = tf.transformations.euler_from_quaternion(
            (data.orientation.x, data.orientation.y, data.orientation.z, data.orientation.w))
        # 由于是弧度制，下面将其改成角度制看起来更方便
        imu_data = "Roll = %f, Pitch = %f, Yaw = %f" % (r * 180 / 3.1415926, p * 180 / 3.1415926, y * 180 / 3.1415926)
        rospy.loginfo(imu_data)

        # 将数据写入文件
        file.write(imu_data + '\n')
    else:
        file.close()  # 规定时间结束后关闭文件
        rospy.signal_shutdown("Recording completed.")  # 停止ROS节点


def get_imu():
    rospy.init_node('get_imu', anonymous=True)
    rospy.Subscriber("/handsfree/imu", Imu, callback)  # 接受topic名称
    rospy.spin()


if __name__ == '__main__':
    get_imu()

修改后的get_imu_rpy.py文件名称更名为imu_data_record.py

要在规定的时间段内停止数据的文本记录，您可以使用Python的time模块来实现。首先，您需要在get_imu函数中添加记录数据到文本文件的逻辑，并且在规定的时间段内记录数据，然后在时间结束后停止记录。

在上面的代码中，我们添加了一个计时器，以确保在规定的时间段内执行记录操作。当时间超过规定时间时，我们关闭文件并停止ROS节点。请注意，record_duration变量定义了记录的持续时间，您可以根据需要进行调整。

自定义imu_data_record文件的路径为（根据自己的喜好）：

下面我在pycharm编辑器中来运行imu_data_record.py这个文件：

配置好conda的环境和ros包的路径后，开始运行:

代码运行的结果如下(在6秒钟之后代码运行结束);

"WARNING: cannot load logging configuration file, logging is disabled" 这个警告通常不会导致程序出现实际错误，但它表明程序尝试加载日志配置文件时遇到了问题，因此无法进行日志记录。这通常是由于找不到或无法读取日志配置文件而引起的。

如果您的程序不依赖于日志记录，或者您不关心程序的日志输出，那么这个警告可以忽略。然而，如果您希望记录程序的日志，您可能需要检查以下事项：

日志配置文件路径：确保日志配置文件的路径在程序中是正确的，并且文件存在。
文件权限：检查日志配置文件是否具有读取权限。
配置文件格式：确保日志配置文件的格式是正确的。ROS通常使用YAML格式的配置文件来配置日志记录。
ROS参数：有时，您可以通过设置ROS参数来指定日志配置文件的路径，确保这些参数设置正确。

虽然这个警告不会影响程序的正常运行，但如果您希望记录日志或解决警告，您可以尝试检查以上事项，并确保日志配置文件正确配置和可访问。如果问题仍然存在，您可以查看ROS社区或相关论坛上是否有关于此问题的更多信息和解决方案。

我们继续来到数据保存的路径下查看文件：

发现rpy_timer文件夹下多出来了一个imu_data_rpy_timer.txt文件，点开查看;

这些正是imu_data_record.py文件在6秒内生产的数据文本。

下面我想将这个些数据保存为csv格式，只要将py文件的保存文件扩展名变为.csv即可

继续运行imu_data_record.py文件，得到：

以上的CSV格式数据还可以进一步处理，使得数据看起来更加简洁：

依靠如下代码：

# data_processor_regex
import re

# 定义一个函数，使用正则表达式来提取每行中的数字
def extract_numbers(line):
    # 正则表达式匹配所有的浮点数和负数
    return re.findall(r"[-+]?\d*\.\d+|\d+", line)

# 读取原始数据文件
with open('/home/hjx/handsfree/imu_data_record/rpy_timer/imu_data_rpy_timer.csv', 'r') as file:
    original_lines = file.readlines()

# 创建一个新的CSV文件用于保存处理后的数据
with open('/home/hjx/handsfree/imu_data_record/rpy_timer/imu_data_rpy_timer_processed_regex.csv', 'w') as output_file:
    for line in original_lines:
        numbers = extract_numbers(line)
        if len(numbers) >= 3:
            # 提取 Roll, Pitch, Yaw 的值
            roll, pitch, yaw = numbers[:3]
            # 写入提取的值到新的CSV文件
            output_file.write(f'{roll},{pitch},{yaw}\n')

print('数据处理完成，已保存为 imu_data_rpy_timer_processed_regex.csv 文件')

# 这段代码首先定义了一个函数 extract_numbers 来使用正则表达式从字符串中提取数字。
# 然后，它读取原始数据文件，对于文件中的每一行，使用这个函数来提取数字，
# 最后将提取的 Roll、Pitch 和 Yaw 值写入到一个新的CSV文件中。

可实现下面的最终效果：