机器人 / 系统课时

用 tf2 查询坐标系间的位姿

用 tf2 查询坐标系间的位姿

激光雷达的数据以 laser_frame 为参考,而底盘运动通常在 odom 中描述。本篇用 ROS 2 Jazzy 发布雷达安装的静态变换,模拟底盘的动态变换,再查询雷达在里程计坐标系中的位姿,并处理坐标系缺失和时间戳不匹配。

学习目标

  • 区分 /tf_static 与 /tf,判断查询方向。
  • 使用 Python Buffer.lookup_transform() 获取跨坐标系位姿。
  • 根据缺帧、断链和时间外推异常定位问题。

确认变换方向与坐标链

示例包含三个坐标系:odom 是里程计参考系,base_link 是底盘系,laser_frame 是雷达系。odom 到 base_link 的关系随运动变化,发布到 /tf;base_link 到 laser_frame 的安装关系不变,发布到 /tf_static。

假设底盘在 odom 中位于 (1.00, 0, 0) 米,雷达安装位置相对底盘为 (0.20, 0, 0.15) 米,两段均无旋转,则雷达在 odom 中位于 (1.20, 0, 0.15) 米。lookup_transform('odom', 'laser_frame', Time()) 返回的正是“源坐标系 laser_frame 在目标坐标系 odom 下”的变换,而不是反方向。

tf2 静态与动态变换链及查询方向

发布雷达的静态安装变换

环境使用 Ubuntu 24.04 与 ROS 2 Jazzy,以下均在 Bash 终端执行。终端 A 安装必要工具、加载 ROS 环境,并将雷达固定在底盘前方 0.20 米、上方 0.15 米。这里的平移单位是米,roll/pitch/yaw 单位是弧度。

bash
sudo apt update
sudo apt install -y ros-jazzy-tf2-ros ros-jazzy-tf2-tools
source /opt/ros/jazzy/setup.bash
ros2 run tf2_ros static_transform_publisher --x 0.20 --y 0 --z 0.15 --roll 0 --pitch 0 --yaw 0 --frame-id base_link --child-frame-id laser_frame

预期结果:发布进程持续运行,/tf_static 提供 base_link 到 laser_frame 的固定关系。不要关闭终端 A;它不是执行一次便退出的配置命令。

新开终端 B 查询雷达相对底盘的变换:

bash
source /opt/ros/jazzy/setup.bash
ros2 run tf2_ros tf2_echo base_link laser_frame

预期输出关键行(启动时可能先短暂等待):

text
- Translation: [0.200, 0.000, 0.150]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]

tf2_echo 使用“参考坐标系、被观察坐标系”的参数顺序;在本例中对应 Python 的目标系 base_link、源系 laser_frame。完整输出还包含时间、RPY 和变换矩阵。

发布动态变换并跨两段查询

保持终端 A 运行。将下面的完整程序保存为当前目录的 tf_pose_demo.py。程序每 0.5 秒模拟底盘向前移动 0.10 米,广播 odom 到 base_link 的动态变换;同一进程中的监听器缓存 /tf 和 /tf_static,查询雷达在 odom 下的位置。

python
import rclpy
from geometry_msgs.msg import TransformStamped
from rclpy.node import Node
from rclpy.time import Time
from tf2_ros import (
    Buffer, TransformBroadcaster, TransformListener,
    LookupException, ConnectivityException, ExtrapolationException
)

class TfPoseDemo(Node):
    def __init__(self):
        super().__init__('tf_pose_demo')
        self.buffer = Buffer()
        self.listener = TransformListener(self.buffer, self)
        self.broadcaster = TransformBroadcaster(self)
        self.x = 0.0
        self.timer = self.create_timer(0.5, self.step)

    def step(self):
        # 用递增的位置模拟动态里程计;真实机器人由里程计节点发布
        self.x += 0.1
        msg = TransformStamped()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = 'odom'
        msg.child_frame_id = 'base_link'
        msg.transform.translation.x = self.x
        msg.transform.rotation.w = 1.0
        self.broadcaster.sendTransform(msg)

        # 查询最新可用变换;首次接收消息前允许暂时失败
        try:
            result = self.buffer.lookup_transform(
                'odom', 'laser_frame', Time()
            )
        except (LookupException, ConnectivityException, ExtrapolationException) as exc:
            self.get_logger().warn(f'tf not ready: {exc}')
            return

        t = result.transform.translation
        self.get_logger().info(
            f'laser in odom: x={t.x:.2f}, y={t.y:.2f}, z={t.z:.2f}'
        )

def main():
    rclpy.init()
    node = TfPoseDemo()
    try:
        rclpy.spin(node)  # 必须处理订阅回调,Buffer 才会获得新变换
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

预期结果:启动并执行该完整程序后,监听器会定期打印雷达在 odom 下的位置;首次查询可能因缓冲区尚未收到变换而短暂报警。

在终端 B 停止先前的 tf2_echo(按 Ctrl+C)后运行:

bash
python3 tf_pose_demo.py

预期输出示例(时间和逐步增加的 x 数值因接收时刻而异):

text
[INFO] [<TIME>] [tf_pose_demo]: laser in odom: x=<X>, y=0.00, z=0.15

<TIME> 是日志时间,<X> 是变化的雷达横向位置;稳定收到变换后,它约等于最近一次缓存的底盘 x 加 0.20 米。验收后可按 Ctrl+C 停止程序;排查时让它继续运行。生产系统应由里程计节点发布动态 TF,应用节点仅创建 Buffer 与 TransformListener 进行查询。

排查缺帧、断链与时间错误

Time() 表示取缓冲区内最新可共同使用的变换,动态历史默认约保留 10 秒;把它换成当前时刻 self.get_clock().now(),可能因最新消息尚未到达而触发向未来外推。处理真实传感器数据时应按消息 header.stamp 查询对应时刻,而不是无条件使用最新位姿。

现象 常见根因 处理方法
LookupException,找不到 laser_frame 静态发布进程已退出、坐标系拼写错误 保持终端 A 运行,核对 frame_id 和 child_frame_id
ConnectivityException,两系无法连通 odom、base_link、laser_frame 没有连成一棵树 检查每条变换的父子关系,同一子坐标系不要交给多个发布者
ExtrapolationException,时间超出范围 查询时间早于缓冲历史或晚于最新动态变换 当前位姿先用 Time();带时间戳的传感器查询应等待对应变换,并保证时钟一致

在演示的两个发布者都运行时,从第三个已加载 Jazzy 环境的终端检查树:

bash
ros2 run tf2_tools view_frames

预期结果:当前目录生成 frames.pdf,其中能看到 odom、base_link、laser_frame 连成的父子链;若缺少一段,应先修复发布者,而不是反复延长查询等待时间。

要主动验证外推错误,可将程序中的 Time() 临时替换成 self.get_clock().now() + Duration(seconds=5.0),并增加导入 from rclpy.duration import Duration;由于发布者没有未来五秒的变换,查询应进入 ExtrapolationException 分支。验证后改回 Time()。连接 Gazebo 或 rosbag2 回放时,所有相关节点应统一 use_sim_time,并确保 /clock 可用,否则系统时间与仿真时间混用也会造成外推失败。

对于本例,只要静态边位于 /tf_static、动态边持续出现在 /tf,且查询时间落在可用历史内,lookup_transform('odom', 'laser_frame', Time()) 就能正确组合两段变换。接口用法可对照 ROS 2 Jazzy 静态发布教程 和 Python 监听器教程。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

用 tf2 查询坐标系间的位姿

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通