ROS2与tf2入门教程-使用tf2进行Time travel(Python)

说明：

• 介绍tf2的高级时间旅行功能

概述：

• 在之前的教程中，我们讨论了 tf2 和 time 的基础知识。

• 本教程将带我们更进一步，并揭示一个强大的 tf2 技巧：时间旅行。

• 简而言之，tf2 库的关键特性之一是它能够在时间和空间上转换数据。

• 此 tf2 时间旅行功能可用于各种任务，例如长时间监控机器人的姿势或构建跟随领导者“步骤”的跟随机器人。

• 我们将使用该时间旅行功能及时查找变换，并编程turtle2 使其落后于carrot1 5 秒。

步骤：

• 新建turtle_tf2_listener_travel.py

cd ~/tf2_ws/src/learning_tf2_py/learning_tf2_py
cp turtle_tf2_listener_timeout.py turtle_tf2_listener_travel.py
vim turtle_tf2_listener_travel.py

• 将文件中的lookup_transform()修改为如下部分

when = self.get_clock().now() - rclpy.time.Duration(seconds=5.0)
trans = self._tf_buffer.lookup_transform(
    to_frame_rel,
    from_frame_rel,
    when,
    timeout=Duration(seconds=0.05))

• 现在，如果你运行它，在前 5 秒内，第二只乌龟将不知道去哪里，因为我们还没有乌龟的 5 秒姿势历史
  　
• 增加权限　

sudo chmod +x turtle_tf2_listener_travel.py

• 添加入口，修改setup.py在'console_scripts': [里添加

'turtle_tf2_listener_travel = learning_tf2_py.turtle_tf2_listener_travel:main',

• 新增为turtle_tf2_demo_travel.launch.py

cd ~/tf2_ws/src/learning_tf2_py/launch
cp turtle_tf2_demo_timeout.launch.py turtle_tf2_demo_travel.launch.py
vim turtle_tf2_demo_travel.launch.py

• 修改内容如下

Node(
  package='learning_tf2_py',
  executable='turtle_tf2_listener_travel',
  name='listener',
  parameters=[
      {'target_frame': LaunchConfiguration('target_frame')}
  ]
),   

• 新增为turtle_tf2_fixed_frame_demo_travel.launch.py

cd ~/tf2_ws/src/learning_tf2_py/launch
cp turtle_tf2_fixed_frame_demo_timeout.launch.py turtle_tf2_fixed_frame_demo_travel.launch.py
vim turtle_tf2_fixed_frame_demo_travel.launch.py

• 修改内容如下

demo_nodes = IncludeLaunchDescription(
  PythonLaunchDescriptionSource([os.path.join(
        get_package_share_directory('learning_tf2_py'), 'launch'),
        '/turtle_tf2_demo_travel.launch.py']),
  launch_arguments={'target_frame': target_frame}.items()
)

• 构建

colcon build --symlink-install --packages-select learning_tf2_py

• 加载工作空间

. ~/tf2_ws/install/local_setup.bash

测试：

• 启动turtle_tf2_fixed_frame_demo_travel.launch.py

. ~/tf2_ws/install/local_setup.bash
ros2 launch learning_tf2_py turtle_tf2_fixed_frame_demo_travel.launch.py

• 你现在应该注意到你的乌龟像这个截图一样不受控制地四处行驶。 让我们试着理解这种行为背后的原因。

• 在我们的代码中，我们向 tf2 询问了以下问题：“carrot1 5 秒前相对于 turtle2 5 秒前的姿势是什么？”。

• 这意味着我们根据 5 秒前的位置以及第一个carrot1 5 秒前的位置来控制第二个海龟。

• 然而，我们真正想问的是：“carrot1 5 秒前的pose，相对于turtle2 的当前位置是什么？”。

lookup_transform()高级API

• 此函数没测试通过，　暂作记录
• 为了向 tf2 提出这个特定问题，我们将使用一个高级 API，它使我们能够明确说明何时获取指定的转换。
• 这是通过使用附加参数调用lookup_transform_full() 方法来完成的。
• 您的代码现在看起来像这样：

when = self.get_clock().now() - rclpy.time.Duration(seconds=5.0)
trans = self._tf_buffer.lookup_transform_full(
        target_frame=to_frame_rel,
        target_time=rclpy.time.Time(),
        source_frame=from_frame_rel,
        source_time=when,
        fixed_frame='world',
        timeout=Duration(seconds=0.05))

• lookup_transform_full() 的高级 API 采用六个参数：

目标框架
转变为的时间
源框架
评估源帧的时间
不随时间改变的框架，在这种情况下是世界框架
等待目标帧可用的时间

• 综上所述，tf2 在后台做了以下事情。
• 过去，它计算从carrot1到world的变换。
• 在world框架中，tf2 时间从过去穿越到现在。
• 在当前时间，tf2 计算从world 到 turtle2 的变换。
• 再次运行模拟，这次使用高级时间旅行 API

ros2 launch learning_tf2_py turtle_tf2_fixed_frame_demo_travel.launch.py

参考：

• 资料来源