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

说明:

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

概述：

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

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

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

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

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

步骤：

• 新建turtle_tf2_listener_travel.cpp

cd ~/tf2_ws/src/learning_tf2_cpp/src
cp turtle_tf2_listener_timeout.cpp turtle_tf2_listener_travel.cpp
vim turtle_tf2_listener_travel.cpp

• 修改为如下内容

rclcpp::Time when = this->get_clock()->now() - rclcpp::Duration(5, 0);
transformStamped = tf_buffer_->lookupTransform(
    toFrameRel,
    fromFrameRel,
    when,
    50ms);

• 修改CMakeLists.txt文件，再原来的内容基础上添加turtle_tf2_listener_travel.cpp的内容

add_executable(turtle_tf2_listener_travel src/turtle_tf2_listener_travel.cpp)
ament_target_dependencies(
  turtle_tf2_listener_travel
  geometry_msgs
  rclcpp
  tf2
  tf2_ros
  turtlesim
)
install(TARGETS
  turtle_tf2_listener_travel
  DESTINATION lib/${PROJECT_NAME})

• 创建turtle_tf2_demo_travel.launch.py

cd ~/tf2_ws/src/learning_tf2_cpp/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_cpp',
    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_cpp/launch
cp turtle_tf2_fixed_frame_demo_timetout.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_cpp'), 'launch'),
      '/turtle_tf2_demo_travel.launch.py']),
    launch_arguments={'target_frame': target_frame}.items()
  )

• 构建

colcon build --symlink-install --packages-select learning_tf2_cpp

• 加载工作空间

. ~/tf2_ws/install/local_setup.bash

测试：

• 现在，如果你运行这个，在前 5 秒内，第二只海龟将不知道去哪里，因为我们还没有 5 秒的carrot1姿势历史。
• 但是在这 5 秒之后会发生什么？ 让我们试一试：

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

lookupTransform()高级API

• 通过调用lookupTransform()带有附加参数的方法来完成的

rclcpp::Time now = this->get_clock()->now();
rclcpp::Time when = now - rclcpp::Duration(5, 0);
transformStamped = tf_buffer_->lookupTransform(
    toFrameRel,
    now,
    fromFrameRel,
    when,
    "world",
    50ms);

• 综上所述，tf2 在后台做了以下工作。过去，它计算从carrot1到 的变换world。
• 在world坐标系中，tf2 时间从过去到现在。
• 并且在当前时间，tf2 计算从world到 的变换turtle2
• 使用高级时间旅行 API

ros2 launch learning_tf2_cpp turtle_tf2_fixed_frame_demo_travel.launch.py

参考：

• 资料来源