具身智能学习笔记

使用 TF2 管理坐标系

5 min read

什么是 TF2

在 ROS 2（Robot Operating System 2）中，TF2（Transform 2）是一个非常重要的工具，用于处理机器人中不同坐标系之间的变换关系。它允许开发者轻松地管理、查询和广播坐标系之间的相对位置和姿态。

  1. 坐标系（Frames）

    • 坐标系是 TF2 的核心概念之一。每个坐标系都有一个唯一的名称，例如“base_link”（机器人位置）、“camera_link”（相机位置）、“odom”（里程计坐标系）等。这些名称用于标识机器人或环境中的不同部分。

  2. 变换（Transforms）

    • 变换是两个坐标系之间的相对位置和姿态关系。变换通常用一个 的齐次变换矩阵表示，也可以用四元数和平移向量表示。TF2 使用这些变换来计算不同坐标系之间的相对位置和姿态。

  3. 广播（Broadcasting）

    • 广播是指将一个坐标系相对于另一个坐标系的变换关系发送到 TF2 系统中。通常，一个节点会定期广播它所知道的变换关系。例如，一个 IMU（惯性测量单元）节点可能会广播从“base_link”到“imu_link”的变换。
    • 内容：
      • 位置：xyz
      • 姿态：欧拉角 zyx / 四元数
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import TransformStamped
from tf2_ros import TransformBroadcaster
 
class FramePublisher(Node):
    def __init__(self):
        super().__init__('frame_publisher')
        self.tf_broadcaster = TransformBroadcaster(self)
        self.timer = self.create_timer(0.1, self.broadcast_transform)
 
    def broadcast_transform(self):
        t = TransformStamped()            # 详见5.时间戳
        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = 'base_link'   # 参考坐标系
        t.child_frame_id = 'camera_link'  # 目标坐标系
        t.transform.translation.x = 0.1
        t.transform.translation.y = 0.2
        t.transform.translation.z = 0.3
        t.transform.rotation.x = 0.0
        t.transform.rotation.y = 0.0
        t.transform.rotation.z = 0.0
        t.transform.rotation.w = 1.0
        self.tf_broadcaster.sendTransform(t)
 
def main(args=None):
    rclpy.init(args=args)
    frame_publisher = FramePublisher()
    rclpy.spin(frame_publisher)
    frame_publisher.destroy_node()
    rclpy.shutdown()
 
if __name__ == '__main__':
    main()
  1. 查询（Querying）
    • 查询是指从 TF2 系统中获取两个坐标系之间的变换关系。一个节点可以通过查询来获取它需要的变换关系，从而将数据从一个坐标系转换到另一个坐标系。例如，一个视觉处理节点可能需要将相机数据从“camera_link”转换到“base_link”。
    • TransformListener 的职责是监听TF2系统中的变换信息，并将这些信息存储到 Buffer 中；TransformListener 在定义时自动启动。
import rclpy
from rclpy.node import Node
from tf2_ros import Buffer, TransformListener
 
class FrameListener(Node):
    def __init__(self):
        super().__init__('frame_listener')
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer)
        self.timer = self.create_timer(1.0, self.lookup_transform)
 
    def lookup_transform(self):
        try:
            trans = self.tf_buffer.lookup_transform(
            	'base_link', 'camera_link', rclpy.time.Time())
            self.get_logger().info(
            	f'Transform from base_link to camera_link: {trans}')
        except Exception as e:
            self.get_logger().info(f'Lookup failed: {e}')
 
def main(args=None):
    rclpy.init(args=args)
    frame_listener = FrameListener()
    rclpy.spin(frame_listener)
    frame_listener.destroy_node()
    rclpy.shutdown()
 
if __name__ == '__main__':
    main()

  1. 时间戳（Timestamps）

    • TF2 中的变换是时间戳化的，这意味着每个变换都有一个时间戳，表示该变换在特定时间点的有效性。这使得 TF2 能够处理动态变换，例如机器人在移动时的坐标系变换。
    • 内容：
      • 父坐标系名称
      • 子坐标系名称
      • 父坐标系和子坐标系之间的关系

  2. 缓冲区（Buffer）

    • TF2 维护一个缓冲区，用于存储最近的变换关系。这个缓冲区允许节点查询过去某个时间点的变换关系，而不仅仅是当前的变换关系。这对于处理延迟和时间同步问题非常有用。

  3. 工具和库

    • tf2_ros：提供了 ROS2 的 TF2 接口，包括广播和查询功能。
      • tf2_monitor：查看所有的发布者和频率。
      • tf2_geometry_msgs：提供了将 ROS2 的几何消息（如 geometry_msgs/Pose）与 TF2 的变换关系进行转换的工具。
      • tf2_eigen：提供了将 Eigen 库的矩阵与 TF2 的变换关系进行转换的工具。
    • rqt_tf_tree：可视化 tf 更新信息 sudo apt install ros-humble-rqt-tf-tree

Graph View

Backlinks