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

发布时间:2024年01月12日

上一篇博客写了关于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(惯性测量单元)数据。

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

  1. 导入必要的Python库和ROS消息类型:

    • rospy:ROS Python库,用于编写ROS节点。
    • tf:ROS中的变换库,用于进行坐标变换。
    • from tf.transformations import *:导入tf库中的变换函数。
    • from sensor_msgs.msg import Imu:导入ROS消息类型Imu,用于订阅IMU数据。
  2. callback函数:

    • callback函数是一个回调函数,它会在接收到/handsfree/imu话题的新消息时被调用。
    • 在回调函数内部,通过tf.transformations.euler_from_quaternion函数将四元数转换为欧拉角(Roll、Pitch、Yaw)。
    • 使用rospy.loginfo函数将欧拉角以角度制格式打印到ROS日志中。
  3. get_imu函数:

    • get_imu函数是主函数,用于初始化ROS节点并设置话题订阅。
    • rospy.init_node用于初始化ROS节点,其中'get_imu'是节点的名称,anonymous=True表示使节点名称唯一。
    • rospy.Subscriber用于订阅/handsfree/imu话题,当有新消息发布到该话题时,将调用callback函数进行处理。
    • rospy.spin()使节点保持运行状态,等待新消息的到来。
  4. 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" 这个警告通常不会导致程序出现实际错误,但它表明程序尝试加载日志配置文件时遇到了问题,因此无法进行日志记录。这通常是由于找不到或无法读取日志配置文件而引起的。

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

  1. 日志配置文件路径:确保日志配置文件的路径在程序中是正确的,并且文件存在。

  2. 文件权限:检查日志配置文件是否具有读取权限。

  3. 配置文件格式:确保日志配置文件的格式是正确的。ROS通常使用YAML格式的配置文件来配置日志记录。

  4. ROS参数:有时,您可以通过设置ROS参数来指定日志配置文件的路径,确保这些参数设置正确。

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

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

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

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

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

继续运行imu_data_record.py文件,得到

文章来源:https://blog.csdn.net/qq_54900679/article/details/135550752
本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。