ROS 2边学边练（38）-- 时间之旅（C++）

前言

        前面的内容我们已经学习了tf2的一些基础知识，这一节我们即将进一步了解和学习关于tf2的一个牛逼功能 ---- 时间旅行。

动动手

时间旅行

        其实没标题那么玄乎，正常表述就是，获取历史转换数据（是不是感觉立马就变了）。

        我们还是利用learning_tf2_cpp包进行演练。

        打开turtle_tf2_listener.cpp,关注下面这块代码：

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) {

        原先turtle2是实时地跟随carrot1游动的，我们现在想让第二只海龟turtle2照着carrot1坐标系5秒之前的轨迹游动，也就是让tf2找到5秒之前carrot1的数据，让turtle2复现一下那时那景。上面的代码就是我们根据需求修改后的结果，看起来合情合理。

        我们重新构建下learning_tf2_cpp包并启动launch文件（同时打开另外一个终端运行turtle_teleop_key控制turtle1游动几下）：

$ros2 launch learning_tf2_cpp turtle_tf2_fixed_frame_demo_launch.py

        程序开始时，turtle2可能是没啥动静的，因为我们要求的是5秒钟前carrot1的数据，如果从启动开始没到5秒，tf2是查不到数据的，只有等待5秒钟之后，我们才能看看效果。

        turtle2不受控了！ 

        为什么会这样，代码写错了？参数给的不对？有点不理解。我们再仔细看看上面代码的实际意思，可能会恍然大悟（也可能迷中迷）：

1. 该代码实际向tf2查询的是，carrot1 5秒之前相对于turtle2 5秒之前的位姿；
2. 我们实际想问tf2查询的是，carrot1 5秒之前相对于turtle2当前的位姿；

        一切问题源自对lookupTransform函数理解不彻底，该函数的第三个参数when实际对源帧和目标帧均起作用了，这里的意思就变成目标帧5秒之前相对于源帧5秒之前的位姿，是无法实现我们真实的需求的。

扩展lookupTransform()函数

        我们需要明确告诉tf2，我们要carrot1 5秒之前相对于turtle2当前的位姿，而不是5秒之前相对5秒之前的位姿，这逻辑会陷入混乱，我们理解的也混乱。这就需要2个时间参数，一个针对源帧，一个针对目标帧，互不影响，如下所示：

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) {

• toFrameRel 是目标坐标系，/turtle2；
• now 是目标坐标系的时间点；
• fromFrameRel 是源坐标系，/carrot1；
• when 是源坐标系的时间点，这里是5秒前的时间点；
• "world" 是固定的父坐标系，用于解析变换关系链；
• 50ms 是查找变换关系的超时时间。

        为帮助理解，我们再看下几个坐标系之间的关系图，world是turtle1和turtle2的父坐标系，carrot1是turtle1的子坐标系，会与turtle1的位姿相关，同时，turtle2又以carrot1为目标，这是几者之间的关系。

        总结下上面代码背后完成的工作， 过去时(when)，tf2计算从carrot1到world的转换（关系图左半边），接着world帧承前启后（关系图中间），于当下(now)，tf2计算从world到turtle2的转换（关系图右半边）。

检查结果

        重新构建好后再次运行：

$ros2 launch learning_tf2_cpp turtle_tf2_fixed_frame_demo_launch.py

        并启动turtle_teleop_key控制turtle1的游动，我们发现一切又恢复正常了，turtle2会滞后于turtle1 5秒再随着之前turtle1的轨迹游动，我们从上帝视角参与了现在（turtle1）并追溯了时光（turtle2）。 

本篇完。