TF2 与时间（C++）

目标： 学习如何使用 lookupTransform() 函数获取特定时间的变换，以及如何在 tf2 树上等待变换变为可用。

教程级别： 中级

预计时间： 10 分钟

背景

在之前的教程中，我们通过编写 tf2 广播器和监听器重新创建了 turtlesim 演示程序，还学习了如何向变换树中添加新坐标系，并了解了 tf2 如何维护坐标系树。这棵树会随时间变化，tf2 为每个变换存储时间快照（默认最多缓存 10 秒）。到目前为止，我们使用 lookupTransform() 函数获取 tf2 树中最新可用的变换，但并不关心该变换是在什么时间记录的。本教程将教你如何获取特定时间点的变换。

任务

1 更新监听器节点

让我们回到添加坐标系教程中结束的位置。进入 learning_tf2_cpp 功能包，打开 turtle_tf2_listener.cpp，查看 lookupTransform() 调用：

   try {
       t = tf_buffer_->lookupTransform(
          toFrameRel,
          fromFrameRel,
          tf2::TimePointZero);
   } catch (const tf2::TransformException & ex) {

可以看到，我们通过 tf2::TimePointZero 将时间指定为 0。

注意：tf2 包有自己的时间类型 tf2::TimePoint，与 rclcpp::Time 不同。tf2_ros 包中的许多 API 会自动在 rclcpp::Time 和 tf2::TimePoint 之间转换。这里也可以使用 rclcpp::Time(0, 0, this->get_clock()->get_clock_type())，但它最终也会被转换为 tf2::TimePointZero。

对于 tf2，时间 0 表示缓冲区中“最新可用”的变换。现在，将此行改为获取当前时间的变换，即 this->get_clock()->now()：

   rclcpp::Time now = this->get_clock()->now();
   try {
       t = tf_buffer_->lookupTransform(
           toFrameRel, fromFrameRel,
           now);
   } catch (const tf2::TransformException & ex) {

现在构建功能包并尝试运行 launch 文件。

$ ros2 launch learning_tf2_cpp turtle_tf2_demo_launch.xml # .py 或 .yaml 也可以
[INFO] [1629873136.345688064] [listener]: Could not transform turtle2 to turtle1: Lookup would
require extrapolation into the future.  Requested time 1629873136.345539 but the latest data
is at time 1629873136.338804, when looking up transform from frame [turtle1] to frame [turtle2]

输出提示坐标系不存在，或者数据属于未来时间。

要理解为什么会出现这种情况，需要了解缓冲区的工作原理。首先，每个监听器都有一个缓冲区，用于存储来自不同广播器的所有坐标变换。其次，当广播器发送一个变换时，该变换需要经过一些时间才能到达缓冲区（通常是几毫秒）。因此，当你请求“当前”时刻的坐标系变换时，应该等待几毫秒让信息到达。

2 修复监听器节点

tf2 提供了一个实用工具，可以等待直到变换变为可用。你可以通过向 lookupTransform() 添加超时参数来使用它。按如下所示编辑代码（添加最后一个超时参数）：

   rclcpp::Time now = this->get_clock()->now();
   try {
       t = tf_buffer_->lookupTransform(
           toFrameRel,
           fromFrameRel,
           now,
           50ms);
   } catch (const tf2::TransformException & ex) {

lookupTransform() 可以接受四个参数，其中最后一个是可选的超时参数。它会阻塞最多该持续时间来等待变换变为可用，直到超时。

3 检查结果

现在可以构建功能包并运行 launch 文件。

ros2 launch learning_tf2_cpp turtle_tf2_demo_launch.xml

你应该会注意到 lookupTransform() 实际上会阻塞，直到两只 turtle 之间的变换变为可用（通常只需要几毫秒）。只有当达到超时时间（本例中为 50 毫秒）后变换仍然不可用时，才会抛出异常。

总结

在本教程中，你学习了如何获取特定时间戳的变换，以及如何在使用 lookupTransform() 函数时等待 tf2 树中的变换变为可用。