Point Cloud Library (PCL) for Python - pclpy 指南 (4) - ROS 输出 TUM 格式姿态数据

Point Cloud Library (PCL) for Python - pclpy 指南 (3) - SLAM 姿态数据格式 TUM 介绍

在 ROS（Robot Operating System）中，输出 TUM 格式的姿态数据文件（pose_tum.txt）通常需要从 ROS 中的位姿数据（Pose）话题中订阅并保存这些数据到文件。以下是一个示例代码，它订阅 ROS 中的 /odom 话题，并将位姿数据保存为 TUM 格式的文件。
 

示例代码

首先，确保已经安装并配置好 ROS 和相应的工作空间。然后创建一个新的 ROS 包或在现有的包中添加以下代码。

创建节点文件：pose_to_tum.py

#!/usr/bin/env python

import rospy
import tf
from nav_msgs.msg import Odometry
import time

def quaternion_to_tum_format(quaternion):
    """
    Convert quaternion to the TUM format.
    """
    qx, qy, qz, qw = quaternion
    return qx, qy, qz, qw

def pose_callback(msg):
    """
    Callback function to process pose data and write it to a file in TUM format.
    """
    # Get the current time
    timestamp = time.time()

    # Get position
    position = msg.pose.pose.position
    x = position.x
    y = position.y
    z = position.z

    # Get orientation
    orientation = msg.pose.pose.orientation
    qx, qy, qz, qw = quaternion_to_tum_format([orientation.x, orientation.y, orientation.z, orientation.w])

    # Write to file
    with open('pose_tum.txt', 'a') as f:
        f.write(f"{timestamp} {x} {y} {z} {qx} {qy} {qz} {qw}\n")

def main():
    rospy.init_node('pose_to_tum', anonymous=True)

    # Subscribe to the /odom topic
    rospy.Subscriber('/odom', Odometry, pose_callback)

    rospy.spin()

if __name__ == '__main__':
    main()

 

步骤说明

1. 创建并初始化ROS节点：

   rospy.init_node('pose_to_tum', anonymous=True)
   

2. 订阅 /odom 话题：

   rospy.Subscriber('/odom', Odometry, pose_callback)
   

3. 在回调函数中处理位姿数据并写入文件：

   def pose_callback(msg):
       # 获取当前时间戳
       timestamp = time.time()
   
       # 获取位置信息
       position = msg.pose.pose.position
       x = position.x
       y = position.y
       z = position.z
   
       # 获取四元数
       orientation = msg.pose.pose.orientation
       qx, qy, qz, qw = quaternion_to_tum_format([orientation.x, orientation.y, orientation.z, orientation.w])
   
       # 写入文件
       with open('pose_tum.txt', 'a') as f:
           f.write(f"{timestamp} {x} {y} {z} {qx} {qy} {qz} {qw}\n")
   

4. 四元数格式转换函数：

   def quaternion_to_tum_format(quaternion):
       qx, qy, qz, qw = quaternion
       return qx, qy, qz, qw
   

 

运行步骤

1. 创建一个ROS包（如果还没有的话）：

   cd ~/catkin_ws/src
   catkin_create_pkg pose_to_tum rospy std_msgs nav_msgs tf
   

2. 将上述Python文件放到包的src目录下：

   cd ~/catkin_ws/src/pose_to_tum/src
   touch pose_to_tum.py
   chmod +x pose_to_tum.py
   

3. 编译工作空间：

   cd ~/catkin_ws
   catkin_make
   

4. 运行节点：

   source devel/setup.bash
   rosrun pose_to_tum pose_to_tum.py
   

 

总结

此脚本会订阅 /odom 话题的消息，并将位姿数据以TUM格式写入 pose_tum.txt 文件。确保 /odom 话题上有数据发布，通常机器人仿真环境或真实机器人会发布该话题。