ROS2 中 TF 变换发布与订阅：实现 base_link 和 test_link 实时可视化显示

视频讲解

安装环境依赖

sudo apt-get install ros-humble-tf2-ros ros-humble-tf2-geometry-msgs ros-humble-tf-transformations

创建一个包名为tf_test_pkg的包

ros2 pkg create --build-type ament_python tf_test_pkg --dependencies rclpy tf2_ros geometry_msgs

依赖为tf2_ros，geometry_msgs

在tf_test_pkg/tf_test_pkg中创建tf_publisher.py

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

class TFPublisher(Node):
    def __init__(self):
        super().__init__('tf_publisher')
        self.tf_broadcaster = tf2_ros.TransformBroadcaster(self)
        self.timer = self.create_timer(0.1, self.broadcast_tf)
        self.pitch = 0.0

    def broadcast_tf(self):
        t = TransformStamped()
        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = '/base_link'
        t.child_frame_id = '/test_link'

        # 设置平移
        t.transform.translation.x = 1.0
        t.transform.translation.y = 0.0
        t.transform.translation.z = 0.0

        # 增加pitch角度
        self.pitch += 0.2
        if self.pitch > 2 * math.pi:
            self.pitch -= 2 * math.pi

        # 设置旋转（仅pitch变化）
        from tf_transformations import quaternion_from_euler
        q = quaternion_from_euler(0, self.pitch, 0)
        t.transform.rotation.x = q[0]
        t.transform.rotation.y = q[1]
        t.transform.rotation.z = q[2]
        t.transform.rotation.w = q[3]

        # 发布TF变换
        self.tf_broadcaster.sendTransform(t)

def main(args=None):
    rclpy.init(args=args)
    tf_publisher = TFPublisher()
    rclpy.spin(tf_publisher)
    tf_publisher.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

同样的，创建tf_subscriber.py

import rclpy
from rclpy.node import Node
import tf2_ros
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener

class TFSubscriber(Node):
    def __init__(self):
        super().__init__('tf_subscriber')
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)
        self.timer = self.create_timer(1.0, self.lookup_transform)

    def lookup_transform(self):
        try:
            trans = self.tf_buffer.lookup_transform(
                'base_link', 'test_link', rclpy.time.Time())
            self.get_logger().info(f"Translation: {trans.transform.translation}")
            self.get_logger().info(f"Rotation: {trans.transform.rotation}")
        except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
            self.get_logger().warn("Could not find transform between /base_link and /test_link")

def main(args=None):
    rclpy.init(args=args)
    tf_subscriber = TFSubscriber()
    rclpy.spin(tf_subscriber)
    tf_subscriber.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

修改setup.py，增加内容如下：

entry_points={
        'console_scripts': [
            'tf_publisher = tf_test_pkg.tf_publisher:main',
            'tf_subscriber = tf_test_pkg.tf_subscriber:main',
        ],
    },

编译运行

colcon build --packages-select tf_test_pkg

source install/setup.bash 
ros2 run tf_test_pkg tf_publisher
ros2 run tf_test_pkg tf_subscriber

rviz2配置

设置固定坐标系

启动 Rviz 后，在 Rviz 窗口左侧的 “Displays” 面板中，找到 “Global Options” 部分，点击 “Fixed Frame” 右侧的下拉框，选择 /base_link 作为固定坐标系。这一步很关键，因为 Rviz 会以固定坐标系为参考来显示其他坐标系的位置和姿态。

添加 TF 显示插件

在 Rviz 窗口左侧 “Displays” 面板的上方，点击 “Add” 按钮，在弹出的 “Add Display” 对话框中，选择 “TF” 选项，然后点击 “OK”。此时，“Displays” 面板中会新增一个 “TF” 条目。

调整 TF 显示设置

在 “Displays” 面板中展开 “TF” 条目

• Marker Scale：用于调整坐标系箭头的大小。你可以根据实际情况增大或减小该值，使显示效果更清晰。
• Show Axes：勾选该选项会显示每个坐标系的坐标轴（X 轴为红色，Y 轴为绿色，Z 轴为蓝色），方便直观地了解坐标系的方向。
• Show Names：勾选此选项会在每个坐标系旁边显示其名称（如 /base_link 和 /test_link），便于识别不同的坐标系。