【ROS2】中级:tf2- 使用时间 (C++)

目标：学习如何在特定时间获取变换，并使用 lookupTransform() 函数等待 tf2 树上的变换可用。

教程级别：中级

 时间：10 分钟

 目录

•  背景

•  任务

  • 1 更新侦听器节点

  • 2. 修复监听器节点

  • 3 检查结果

•  摘要

 背景

在之前的教程中，我们通过编写 tf2 广播器和 tf2 监听器重新创建了 turtle 演示。我们还学习了如何向变换树添加新帧，并了解了 tf2 如何跟踪坐标帧的树。这个树会随着时间变化，tf2 会为每个变换存储一个时间快照（默认情况下最多 10 秒）。到目前为止，我们使用 lookupTransform() 函数来获取 tf2 树中最新的可用变换，而不知道该变换是在什么时间记录的。本教程将教您如何在特定时间获取变换。

 任务

1 更新监听器节点

让我们回到添加框架教程结束的地方。转到 learning_tf2_cpp 包。打开 turtle_tf2_listener.cpp 并查看 lookupTransform() 调用：

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

您可以看到我们通过调用 tf2::TimePointZero 指定了一个等于 0 的时间。

 便条

tf2 包有自己的时间类型 tf2::TimePoint ，它不同于 rclcpp::Time 。包中的许多 API tf2_ros 会在 rclcpp::Time 和 tf2::TimePoint 之间自动转换。

rclcpp::Time(0, 0, this->get_clock()->get_clock_type()) 本可以在这里使用，但无论如何它都会被转换为 tf2::TimePointZero 。

对于 tf2，时间 0 表示缓冲区中“最新可用”的变换。现在，更改此行以获取当前时间的变换， this->get_clock()->now() ：

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

现在构建包并尝试运行启动文件。

ros2 launch learning_tf2_cpp turtle_tf2_demo_launch.py

您会注意到它失败并输出类似于以下内容：

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

您现在可以构建包并运行启动文件。

cxy@ubuntu2404-cxy:~$ cd ~/ros2_ws
cxy@ubuntu2404-cxy:~/ros2_ws$ colcon build --packages-select learning_tf2_cpp
Starting >>> learning_tf2_cpp
Finished <<< learning_tf2_cpp [19.0s]                       
 
 
Summary: 1 package finished [29.5s]
cxy@ubuntu2404-cxy:~/ros2_ws$ . install/setup.bash
cxy@ubuntu2404-cxy:~/ros2_ws$ ros2 launch learning_tf2_cpp turtle_tf2_demo_launch.py
[INFO] [launch]: All log files can be found below /home/cxy/.ros/log/2024-07-12-20-26-45-751031-ubuntu2404-cxy-11892
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [turtlesim_node-1]: process started with pid [11895]
[INFO] [turtle_tf2_broadcaster-2]: process started with pid [11896]
[INFO] [turtle_tf2_broadcaster-3]: process started with pid [11897]
[INFO] [turtle_tf2_listener-4]: process started with pid [11898]
[turtlesim_node-1] [INFO] [1720787207.560817235] [sim]: Starting turtlesim with node name /sim
[turtlesim_node-1] [INFO] [1720787207.570010819] [sim]: Spawning turtle [turtle1] at x=[5.544445], y=[5.544445], theta=[0.000000]
[turtlesim_node-1] [INFO] [1720787208.449319544] [sim]: Spawning turtle [turtle2] at x=[4.000000], y=[2.000000], theta=[0.000000]
[turtle_tf2_listener-4] [INFO] [1720787209.441088294] [listener]: Successfully spawned

cxy@ubuntu2404-cxy:~$ ros2 run turtlesim turtle_teleop_key

您应该注意到， lookupTransform() 实际上会阻塞，直到两个乌龟之间的变换变得可用（这通常需要几毫秒）。一旦达到超时时间（在这种情况下为五十毫秒），只有在变换仍然不可用时才会引发异常。

 摘要

在本教程中，您将学习如何在特定时间戳获取变换，以及在使用 lookupTransform() 函数时如何等待 tf2 树上的变换可用。

示例代码

以下是一个使用 lookupTransform 函数的示例代码：

#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/msg/transform_stamped.hpp>
 
 
class TransformListenerNode : public rclcpp::Node
{
public:
  TransformListenerNode()
  : Node("transform_listener_node")
  {
    tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
    tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
 
 
    rclcpp::Time now = this->get_clock()->now();
    try {
      geometry_msgs::msg::TransformStamped transform_stamped = tf_buffer_->lookupTransform(
        "target_frame", "source_frame", now, rclcpp::Duration::from_seconds(1.0));
      RCLCPP_INFO(this->get_logger(), "Transform: %f, %f, %f",
        transform_stamped.transform.translation.x,
        transform_stamped.transform.translation.y,
        transform_stamped.transform.translation.z);
    } catch (tf2::TransformException & ex) {
      RCLCPP_ERROR(this->get_logger(), "Could not transform: %s", ex.what());
    }
  }
 
 
private:
  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
};
 
 
int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<TransformListenerNode>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

这个示例代码创建了一个 ROS2 节点，并在节点中使用 lookupTransform 函数查找从 source_frame 到 target_frame 的变换关系。如果查找成功，会输出变换的平移信息；如果失败，会输出错误信息。