《ROS中级课程》第16课 tf2之使用时间（C++）

使用时间（C++）

参考资料：https://docs.ros.org/en/iron/Tutorials/Intermediate/Tf2/Learning-About-Tf2-And-Time-Cpp.html。

目标

学习如何在指定时间获取转换、如何使用lookupTransform函数等待tf2树上的转换可用。

背景

在前面的课程，我们通过编写tf2广播器和监听器重建了海龟示例。我们也学习了如何在转换树上添加新的坐标系、学习了tf2是如何跟踪坐标系树的。这棵树随时间改变，tf2为每次转换（默认最多10秒）都存储了时间快照。到目前为止，我们使用lookupTransform()函数来访问tf2树中最新可用的转换，而不知道该转换是在什么时候记录的。

本节课将介绍如何获取指定时间的转换。

任务

1、tf2和时间

回到上一节添加坐标系的课程。进入learning_tf2_cpp功能包，打开turtle_tf2_listener.cpp文件，查看lookupTransform()调用：

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

可以看到，我们指定了等于0的时间tf2::TimePointZero。

注意：

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意味着在缓存中的最新可用转换。现在，改变这一行代码来获取在当前时间的转换：

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.py

会发现有如下错误打印：

[INFO] [1629873136.345688064] [listener]: Could not transform turtle1 to turtle2: 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]

这个打印告诉我们坐标系不存在或数据在未来。

为了理解这种情况发生的原因，我们需要理解缓存是如何工作的。首先，每个监听者都有一个缓存，用于存放所有的来自不同的tf2广播器的坐标系转换，其次，当一个广播器发送一个转换时，在该转换数据进入缓存前需花费一些时间（通常是几毫秒）。结果，当你请求一个当前时间的坐标系转换时，就需要等待几毫秒以便信息到达缓存。

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.py

你会注意到，lookupTransform()实际上会阻塞直至两只海龟之间的转换变的可用。一旦到达超时时间且转换仍不可用，就会触发一个异常。

总结

在本节课中，学习了如何获取指定时间戳的转换、如何使用lookupTransform函数等待tf2树中的转换可用。