TF2 时间旅行（C++）

目标： 了解 tf2 的高级时间旅行功能。

教程级别： 中级

预计时间： 10 分钟

背景

在之前的教程中，我们讨论了 tf2 与时间的基础知识。本教程将更进一步，揭示一个强大的 tf2 技巧：时间旅行。简而言之，tf2 库的关键特性之一是它能够在时间上和空间上变换数据。

这个 tf2 时间旅行功能可用于各种任务，例如长时间监控机器人的姿态，或构建一个跟随领航者”足迹”的跟随机器人。我们将利用时间旅行功能回顾过去的变换，编程使 turtle2 延迟 5 秒跟随 carrot1。

时间旅行

首先，让我们回到之前教程中结束的位置，进入你的 learning_tf2_cpp 功能包。

现在，不再让第二只 turtle 去胡萝卜当前所在的位置，而是让它去 5 秒前胡萝卜所在的位置。编辑 turtle_tf2_listener.cpp 文件中的 lookupTransform() 调用：

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

运行后，在前 5 秒内第二只 turtle 不知道该去哪里，因为我们还没有胡萝卜 5 秒前的姿态历史。但 5 秒之后会怎样呢？构建功能包，然后试一试：

ros2 launch learning_tf2_cpp turtle_tf2_fixed_frame_demo_launch.xml

turtlesim delay1

现在你应该会看到 turtle 像截图一样不受控制地到处移动。让我们来理解这种行为背后的原因。

在我们的代码中，我们向 tf2 提出了以下问题：“5 秒前 carrot1 相对于 5 秒前 turtle2 的姿态是什么？”这意味着我们根据第二只 turtle 5 秒前的位置和胡萝卜 5 秒前的位置来控制第二只 turtle。
然而，我们真正想问的是：“5 秒前 carrot1 相对于 turtle2 当前位置的姿态是什么？”

lookupTransform() 的高级 API

要向 tf2 提出这个特定的问题，我们将使用高级 API，它允许我们明确指定何时获取指定变换。实现方式是在调用 lookupTransform() 方法时传入附加参数。代码现在看起来像这样：

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

lookupTransform() 的高级 API 接受六个参数：

目标坐标系
变换的目标时间点
源坐标系
计算源坐标系所用的时间点
不随时间变化的坐标系，本例中是 world 坐标系
等待目标坐标系变为可用的时长

总结一下，tf2 在后台做了以下工作：在过去的时间点，计算从 carrot1 到 world 的变换；在 world 坐标系中，tf2 从过去时间旅行到当前；在当前时间点，tf2 计算从 world 到 turtle2 的变换。

检查结果

构建功能包，然后再次运行模拟，这次使用高级时间旅行 API：

ros2 launch learning_tf2_cpp turtle_tf2_fixed_frame_demo_launch.xml

turtlesim delay2

没错，第二只 turtle 被引导到了胡萝卜 5 秒前所在的位置！

总结

在本教程中，你看到了 tf2 的高级功能之一，了解到 tf2 可以在时间上变换数据，并通过 turtlesim 示例学习了如何实现。tf2 允许你回到过去，使用高级 lookupTransform() API 在 turtle 的旧姿态和当前姿态之间进行坐标系变换。