🔄 第11课：TF坐标变换

📋 课程目标

• 理解TF2坐标变换系统的原理
• 掌握TF树的发布与查询
• 学会使用StaticTransformPublisher和代码发布TF
• 掌握tf2_ros的Python/C++ API
• 学会TF调试工具的使用

🧠 为什么需要TF？

机器人有多个坐标系：底盘、传感器、世界地图等。TF2（Transform）系统负责维护这些坐标系之间的变换关系，让任何节点都可以查询"坐标系A中的点在坐标系B中的位置"。

📐 TF2架构

🐍 Python：发布TF变换

#!/usr/bin/env python3
"""TF发布节点 - 发布动态坐标变换"""

import math
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import TransformStamped
from tf2_ros import TransformBroadcaster


class DynamicTfPublisher(Node):
    """发布动态坐标变换"""

    def __init__(self):
        super().__init__('dynamic_tf_publisher')

        self.tf_broadcaster = TransformBroadcaster(self)
        self.timer = self.create_timer(0.05, self.publish_tf)  # 20Hz
        self.time_elapsed = 0.0

        self.get_logger().info('动态TF发布节点启动')

    def publish_tf(self):
        """发布odom→base_link变换（模拟运动）"""
        self.time_elapsed += 0.05

        # 圆周运动
        radius = 2.0
        omega = 0.3  # 角速度
        x = radius * math.cos(omega * self.time_elapsed)
        y = radius * math.sin(omega * self.time_elapsed)
        theta = omega * self.time_elapsed + math.pi / 2

        t = TransformStamped()
        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = 'odom'
        t.child_frame_id = 'base_link'

        t.transform.translation.x = x
        t.transform.translation.y = y
        t.transform.translation.z = 0.0

        # 四元数（从欧拉角转换）
        t.transform.rotation.x = 0.0
        t.transform.rotation.y = 0.0
        t.transform.rotation.z = math.sin(theta / 2)
        t.transform.rotation.w = math.cos(theta / 2)

        self.tf_broadcaster.sendTransform(t)


def main(args=None):
    rclpy.init(args=args)
    node = DynamicTfPublisher()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

🐍 Python：查询TF变换

#!/usr/bin/env python3
"""TF查询节点 - 坐标变换监听与查询"""

import rclpy
from rclpy.node import Node
from tf2_ros import Buffer, TransformListener
from geometry_msgs.msg import PointStamped


class TfListenerNode(Node):
    """TF监听与查询节点"""

    def __init__(self):
        super().__init__('tf_listener_node')

        # 创建TF缓冲区和监听器
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)

        # 定时查询
        self.timer = self.create_timer(1.0, self.lookup_transform)

        self.get_logger().info('TF监听节点启动')

    def lookup_transform(self):
        """查询坐标变换"""
        try:
            # 查询变换
            t = self.tf_buffer.lookup_transform(
                'odom',       # 目标坐标系
                'base_link',  # 源坐标系
                rclpy.time.Time()  # 最新变换
            )

            self.get_logger().info(
                f'base_link在odom中: '
                f'x={t.transform.translation.x:.2f}, '
                f'y={t.transform.translation.y:.2f}, '
                f'z={t.transform.translation.z:.2f}'
            )

            # 变换一个点
            point_in_base = PointStamped()
            point_in_base.header.frame_id = 'base_link'
            point_in_base.header.stamp = self.get_clock().now().to_msg()
            point_in_base.point.x = 1.0  # 机器人前方1米
            point_in_base.point.y = 0.0
            point_in_base.point.z = 0.0

            # 将点从base_link变换到odom
            point_in_odom = self.tf_buffer.transform(
                point_in_base, 'odom'
            )

            self.get_logger().info(
                f'机器人前方1m在odom中: '
                f'x={point_in_odom.point.x:.2f}, '
                f'y={point_in_odom.point.y:.2f}'
            )

        except Exception as ex:
            self.get_logger().warn(f'TF查询失败: {ex}')


def main(args=None):
    rclpy.init(args=args)
    node = TfListenerNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

🔧 静态变换发布

# 命令行发布静态变换
ros2 run tf2_ros static_transform_publisher \
  --x 0.25 --y 0 --z 0.1 \
  --roll 0 --pitch 0 --yaw 0 \
  --frame-id base_link --child-frame-id laser_link

# 在Launch中发布
from tf2_ros import StaticTransformBroadcaster

# 或使用Node
Node(
    package='tf2_ros',
    executable='static_transform_publisher',
    arguments=['--x', '0.25', '--y', '0', '--z', '0.1',
               '--frame-id', 'base_link', '--child-frame-id', 'laser_link']
)

🖥️ TF调试工具

# 查看TF树
ros2 run tf2_tools view_frames

# 查看特定变换
ros2 run tf2_ros tf2_echo odom base_link

# 监测TF延迟
ros2 run tf2_ros tf2_monitor

# 查看所有坐标系
ros2 topic echo /tf --no-arr

🎯 练习题

🏆 成就解锁