ROS使用（9）tf2

最新推荐文章于 2024-05-29 16:54:03 发布

蓝天巨人

最新推荐文章于 2024-05-29 16:54:03 发布

阅读量625

点赞数

分类专栏： ROS 文章标签： python 开发语言

本文链接：https://blog.csdn.net/qq_44632658/article/details/130131286

版权

ROS 专栏收录该内容

10 篇文章 1 订阅

订阅专栏

许多tf2教程可用于C++和Python。本教程经过了精简，以完成C++或Python。如果你想同时学习C++和Python，你应该分别学习一次C++和一次Python的教程。

工作区设置

Introduction to tf2.
tf2的介绍

本教程将给予你一个很好的想法tf2可以为你做什么。它在一个使用turtlesim的多机器人示例中展示了tf2的一些功能。这也介绍了使用tf2_echo、view_frames和rviz。
编写一个静态广播器（Python）（C++）.

本教程将教您如何将静态坐标框架广播到tf2。
写一个广播器（Python）（C++）.

本教程教你如何将机器人的状态广播到tf2。
编写监听器（Python）（C++）。

本教程将教您如何使用tf2访问帧变换。
添加坐标（Python）（C++）。

本教程教你如何添加一个额外的固定帧到tf2。
使用时间（Python）（C++）。

本教程教您使用lookup_transform函数中的超时来等待TF2树上可用的变换。
T时间旅行（Python）（C++）。

本教程教你tf2的高级时间旅行功能。

Debugging tf2

Quaternion fundamentals.
四元数基础。

This tutorial teaches you basics of quaternion usage in ROS 2.
本教程教你在ROS 2中使用四元数的基础知识。
Debugging tf2 problems.
调试tf2问题。

This tutorial teaches you about a systematic approach for debugging tf2 related problems.
本教程教你一个调试tf2相关问题的系统方法。

Using sensor message with tf2

在tf2_ros::MessageFilter中使用stamped数据类型

本教程教你如何使用tf2_ros::MessageFilter来处理带戳的数据类型。

Writing a static broadcaster

发布静态变换对于定义机器人基座与其传感器或非移动部件之间的关系非常有用。例如，在激光扫描仪的中心处的帧中的激光扫描测量是最容易推理的。

这是一个独立的教程，涵盖了静态转换的基础知识，它由两部分组成。在第一部分中，我们将编写代码将静态转换发布到tf2。在第二部分中，我们将解释如何在static_transform_publisher中使用命令行tf2_ros可执行工具。

在接下来的两个教程中，我们将编写代码，从 TF2简介教程。之后，下面的教程将重点介绍如何使用更高级的tf2特性来扩展演示。

在前面的教程中，您学习了如何创建工作区和创建包。

首先，我们将创建一个用于本教程和后续教程的包。名为learning_tf2_cpp的包将依赖于geometry_msgs、rclcpp、tf2、tf2_ros和turtlesim。本教程的代码存储在此处。

打开一个新的终端并源化你的ROS 2安装，这样ros2命令就可以工作了。导航到工作区的src文件夹并创建一个新包：

ros2 pkg create --build-type ament_cmake --dependencies geometry_msgs rclcpp tf2 tf2_ros turtlesim -- learning_tf2_cpp

您的终端将返回一条消息，验证您的软件包learning_tf2_cpp及其所有必要的文件和文件夹的创建。

write the static broadcaster node

让我们首先创建源文件。在src/learning_tf2_cpp/src目录中，通过输入以下命令下载示例静态广播器代码：

使用首选的文本编辑器打开文件。

#include <memory>

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_ros/static_transform_broadcaster.h"

class StaticFramePublisher : public rclcpp::Node
{
public:
  explicit StaticFramePublisher(char * transformation[])
  : Node("static_turtle_tf2_broadcaster")
  {
    tf_static_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);

    // Publish static transforms once at startup
    this->make_transforms(transformation);
  }

private:
  void make_transforms(char * transformation[])
  {
    geometry_msgs::msg::TransformStamped t;

    t.header.stamp = this->get_clock()->now();
    t.header.frame_id = "world";
    t.child_frame_id = transformation[1];

    t.transform.translation.x = atof(transformation[2]);
    t.transform.translation.y = atof(transformation[3]);
    t.transform.translation.z = atof(transformation[4]);
    tf2::Quaternion q;
    q.setRPY(
      atof(transformation[5]),
      atof(transformation[6]),
      atof(transformation[7]));
    t.transform.rotation.x = q.x();
    t.transform.rotation.y = q.y();
    t.transform.rotation.z = q.z();
    t.transform.rotation.w = q.w();

    tf_static_broadcaster_->sendTransform(t);
  }

  std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tf_static_broadcaster_;
};

int main(int argc, char * argv[])
{
  auto logger = rclcpp::get_logger("logger");

  // Obtain parameters from command line arguments
  if (argc != 8) {
    RCLCPP_INFO(
      logger, "Invalid number of parameters\nusage: "
      "$ ros2 run learning_tf2_cpp static_turtle_tf2_broadcaster "
      "child_frame_name x y z roll pitch yaw");
    return 1;
  }

  // As the parent frame of the transform is `world`, it is
  // necessary to check that the frame name passed is different
  if (strcmp(argv[1], "world") == 0) {
    RCLCPP_INFO(logger, "Your static turtle name cannot be 'world'");
    return 1;
  }

  // Pass parameters and initialize node
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<StaticFramePublisher>(argv));
  rclcpp::shutdown();
  return 0;
}

2.1 Examine the code

现在让我们看看与将静态乌龟姿势发布到tf2相关的代码。第一行包括所需的头文件。首先，我们包含 geometry_msgs/msg/transform_stamped.hpp以访问 TransformStamped消息类型，我们将其发布到转换树。

#include "geometry_msgs/msg/transform_stamped.hpp"

之后，包含 rclcpp，因此可以使用其 rclcpp::Node类。

#include "rclcpp/rclcpp.hpp"

tf2::Quaternion是一个四元数类，它提供了将欧拉角转换为四元数的方便函数，反之亦然。我们还包括tf2_ros/static_transform_broadcaster.h，以使用StaticTransformBroadcaster来简化静态转换的发布。

#include "tf2/LinearMath/Quaternion.h"
#include "tf2_ros/static_transform_broadcaster.h"

StaticFramePublisher类构造函数使用名称static_turtle_tf2_broadcaster初始化节点。然后，创建StaticTransformBroadcaster，它将在启动时发送一个静态转换。

tf_static_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);

this->make_transforms(transformation);

这里我们创建了一个TransformStamped对象，它将是我们在填充后发送的消息。在传递实际的转换值之前，我们需要给予它适当的元数据。

We need to give the transform being published a timestamp and we’ll just stamp it with the current time, this->get_clock()->now()
我们需要为发布的转换提供一个时间戳，我们将使用当前时间标记它，this->get_clock()->now()
Then we need to set the name of the parent frame of the link we’re creating, in this case world
然后我们需要设置我们正在创建的链接的父框架的名称，在本例中为world
Finally, we need to set the name of the child frame of the link we’re creating
最后，我们需要设置正在创建的链接的子帧的名称

geometry_msgs::msg::TransformStamped t;

t.header.stamp = this->get_clock()->now();
t.header.frame_id = "world";
t.child_frame_id = transformation[1];

这里我们填充海龟的6D姿势（平移和旋转）。

t.transform.translation.x = atof(transformation[2]);
t.transform.translation.y = atof(transformation[3]);
t.transform.translation.z = atof(transformation[4]);
tf2::Quaternion q;
q.setRPY(
atof(transformation[5]),
atof(transformation[6]),
atof(transformation[7]));
t.transform.rotation.x = q.x();
t.transform.rotation.y = q.y();
t.transform.rotation.z = q.z();
t.transform.rotation.w = q.w();

最后，我们使用sendTransform()函数广播静态转换。

tf_static_broadcaster_->sendTransform(t);

2.2 Add dependencies

导航一级回到src/learning_tf2_cpp目录，其中已为您创建了CMakeLists.txt和package.xml文件。

如创建包教程中所述，请确保填写<description>、<maintainer>和<license>标记：

<description>Learning tf2 with rclcpp</description>
<maintainer email="you@email.com">Your Name</maintainer>
<license>Apache License 2.0</license>

2.3 CMakeLists.txt

将可执行文件添加到CMakeLists.txt并将其命名为static_turtle_tf2_broadcaster，稍后将与ros2 run一起使用。

add_executable(static_turtle_tf2_broadcaster src/static_turtle_tf2_broadcaster.cpp)
ament_target_dependencies(
   static_turtle_tf2_broadcaster
   geometry_msgs
   rclcpp
   tf2
   tf2_ros
)
Finally, add the install(TARGETS…) section so ros2 run can find your executable:
最后，添加install(TARGETS…)部分，以便ros2 run可以找到您的可执行文件：

install(TARGETS
   static_turtle_tf2_broadcaster
   DESTINATION lib/${PROJECT_NAME})

Writing a broadcaster (C++)

在接下来的两个教程中，我们将编写代码，从 TF2简介教程。之后，下面的教程将重点介绍如何使用更高级的tf2特性来扩展演示，包括在转换查找和时间旅行中使用超时

本教程假设您具有ROS 2的工作知识，并且您已经完成了tf2教程介绍和tf2静态广播器教程（C++）。在前面的教程中，您学习了如何创建工作区和创建包。您还创建了learning_tf2_cpp包，这是我们将继续工作的地方。

Task

write the broadcaster node

让我们首先创建源文件。转到我们在上一个教程中创建的learning_tf2_cpp包。在src目录中，通过输入以下命令下载示例广播器代码：

#include <functional>
#include <memory>
#include <sstream>
#include <string>

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_ros/transform_broadcaster.h"
#include "turtlesim/msg/pose.hpp"

class FramePublisher : public rclcpp::Node
{
public:
  FramePublisher()
  : Node("turtle_tf2_frame_publisher")
  {
    // Declare and acquire `turtlename` parameter
    turtlename_ = this->declare_parameter<std::string>("turtlename", "turtle");

    // Initialize the transform broadcaster
    tf_broadcaster_ =
      std::make_unique<tf2_ros::TransformBroadcaster>(*this);

    // Subscribe to a turtle{1}{2}/pose topic and call handle_turtle_pose
    // callback function on each message
    std::ostringstream stream;
    stream << "/" << turtlename_.c_str() << "/pose";
    std::string topic_name = stream.str();

    subscription_ = this->create_subscription<turtlesim::msg::Pose>(
      topic_name, 10,
      std::bind(&FramePublisher::handle_turtle_pose, this, std::placeholders::_1));
  }

private:
  void handle_turtle_pose(const std::shared_ptr<turtlesim::msg::Pose> msg)
  {
    geometry_msgs::msg::TransformStamped t;

    // Read message content and assign it to
    // corresponding tf variables
    t.header.stamp = this->get_clock()->now();
    t.header.frame_id = "world";
    t.child_frame_id = turtlename_.c_str();

    // Turtle only exists in 2D, thus we get x and y translation
    // coordinates from the message and set the z coordinate to 0
    t.transform.translation.x = msg->x;
    t.transform.translation.y = msg->y;
    t.transform.translation.z = 0.0;

    // For the same reason, turtle can only rotate around one axis
    // and this why we set rotation in x and y to 0 and obtain
    // rotation in z axis from the message
    tf2::Quaternion q;
    q.setRPY(0, 0, msg->theta);
    t.transform.rotation.x = q.x();
    t.transform.rotation.y = q.y();
    t.transform.rotation.z = q.z();
    t.transform.rotation.w = q.w();

    // Send the transformation
    tf_broadcaster_->sendTransform(t);
  }

  rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr subscription_;
  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
  std::string turtlename_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<FramePublisher>());
  rclcpp::shutdown();
  return 0;
}

现在，让我们看一下与将乌龟姿势发布到tf2相关的代码。首先，我们定义并获取一个参数turtlename，它指定了一个海龟的名字，例如turtle1或turtle2。

turtlename_ = this->declare_parameter<std::string>("turtlename", "turtle");

之后，节点订阅主题turtleX/pose并对每个传入消息运行函数handle_turtle_pose。

subscription_ = this->create_subscription<turtlesim::msg::Pose>(
  topic_name, 10,
  std::bind(&FramePublisher::handle_turtle_pose, this, _1));

现在，我们创建一个TransformStamped对象并给予适当的元数据。

我们需要给予发布的转换一个时间戳，我们将通过调用this->get_clock()->now()来标记当前时间。这将返回Node使用的当前时间。
然后我们需要设置我们正在创建的链接的父框架的名称，在本例中为world。
最后，我们需要设置我们正在创建的链接的子节点的名称，在本例中，这是turtle本身的名称。

海龟姿态消息的处理函数广播这个海龟的平移和旋转，并将其发布为从帧world到帧turtleX的变换。

geometry_msgs::msg::TransformStamped t;

// Read message content and assign it to
// corresponding tf variables
t.header.stamp = this->get_clock()->now();
t.header.frame_id = "world";
t.child_frame_id = turtlename_.c_str();
// Turtle only exists in 2D, thus we get x and y translation
// coordinates from the message and set the z coordinate to 0
t.transform.translation.x = msg->x;
t.transform.translation.y = msg->y;
t.transform.translation.z = 0.0;

// For the same reason, turtle can only rotate around one axis
// and this why we set rotation in x and y to 0 and obtain
// rotation in z axis from the message
tf2::Quaternion q;
q.setRPY(0, 0, msg->theta);
t.transform.rotation.x = q.x();
t.transform.rotation.y = q.y();
t.transform.rotation.z = q.z();
t.transform.rotation.w = q.w();

最后，我们把我们构造的转换传递给sendTransform的TransformBroadcaster方法，它将负责广播。

// Send the transformation
tf_broadcaster_->sendTransform(t);

1.2 CMakeLists.txt

导航一级返回到learning_tf2_cpp目录，其中包含CMakeLists.txt和package.xml文件。

现在打开CMakeLists.txt添加可执行文件并将其命名为turtle_tf2_broadcaster，稍后您将与ros2 run一起使用。

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

Writing a listener (C++)

在之前的教程中，我们创建了一个tf2广播器，将乌龟的姿势发布到tf2。在本教程中，我们将创建一个tf2侦听器来开始使用tf2。

本教程假设您已经完成了 tf2静态广播器教程（C++）和tf2广播器教程（C++）。在上一个教程中，我们创建了一个learning_tf2_cpp包，这是我们将继续工作的地方。

#include <chrono>
#include <functional>
#include <memory>
#include <string>

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2/exceptions.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "turtlesim/srv/spawn.hpp"

using namespace std::chrono_literals;

class FrameListener : public rclcpp::Node
{
public:
  FrameListener()
  : Node("turtle_tf2_frame_listener"),
    turtle_spawning_service_ready_(false),
    turtle_spawned_(false)
  {
    // Declare and acquire `target_frame` parameter
    target_frame_ = this->declare_parameter<std::string>("target_frame", "turtle1");

    tf_buffer_ =
      std::make_unique<tf2_ros::Buffer>(this->get_clock());
    tf_listener_ =
      std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);

    // Create a client to spawn a turtle
    spawner_ =
      this->create_client<turtlesim::srv::Spawn>("spawn");

    // Create turtle2 velocity publisher
    publisher_ =
      this->create_publisher<geometry_msgs::msg::Twist>("turtle2/cmd_vel", 1);

    // Call on_timer function every second
    timer_ = this->create_wall_timer(
      1s, std::bind(&FrameListener::on_timer, this));
  }

private:
  void on_timer()
  {
    // Store frame names in variables that will be used to
    // compute transformations
    std::string fromFrameRel = target_frame_.c_str();
    std::string toFrameRel = "turtle2";

    if (turtle_spawning_service_ready_) {
      if (turtle_spawned_) {
        geometry_msgs::msg::TransformStamped t;

        // Look up for the transformation between target_frame and turtle2 frames
        // and send velocity commands for turtle2 to reach target_frame
        try {
          t = tf_buffer_->lookupTransform(
            toFrameRel, fromFrameRel,
            tf2::TimePointZero);
        } catch (const tf2::TransformException & ex) {
          RCLCPP_INFO(
            this->get_logger(), "Could not transform %s to %s: %s",
            toFrameRel.c_str(), fromFrameRel.c_str(), ex.what());
          return;
        }

        geometry_msgs::msg::Twist msg;

        static const double scaleRotationRate = 1.0;
        msg.angular.z = scaleRotationRate * atan2(
          t.transform.translation.y,
          t.transform.translation.x);

        static const double scaleForwardSpeed = 0.5;
        msg.linear.x = scaleForwardSpeed * sqrt(
          pow(t.transform.translation.x, 2) +
          pow(t.transform.translation.y, 2));

        publisher_->publish(msg);
      } else {
        RCLCPP_INFO(this->get_logger(), "Successfully spawned");
        turtle_spawned_ = true;
      }
    } else {
      // Check if the service is ready
      if (spawner_->service_is_ready()) {
        // Initialize request with turtle name and coordinates
        // Note that x, y and theta are defined as floats in turtlesim/srv/Spawn
        auto request = std::make_shared<turtlesim::srv::Spawn::Request>();
        request->x = 4.0;
        request->y = 2.0;
        request->theta = 0.0;
        request->name = "turtle2";

        // Call request
        using ServiceResponseFuture =
          rclcpp::Client<turtlesim::srv::Spawn>::SharedFuture;
        auto response_received_callback = [this](ServiceResponseFuture future) {
            auto result = future.get();
            if (strcmp(result->name.c_str(), "turtle2") == 0) {
              turtle_spawning_service_ready_ = true;
            } else {
              RCLCPP_ERROR(this->get_logger(), "Service callback result mismatch");
            }
          };
        auto result = spawner_->async_send_request(request, response_received_callback);
      } else {
        RCLCPP_INFO(this->get_logger(), "Service is not ready");
      }
    }
  }

  // Boolean values to store the information
  // if the service for spawning turtle is available
  bool turtle_spawning_service_ready_;
  // if the turtle was successfully spawned
  bool turtle_spawned_;
  rclcpp::Client<turtlesim::srv::Spawn>::SharedPtr spawner_{nullptr};
  rclcpp::TimerBase::SharedPtr timer_{nullptr};
  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_{nullptr};
  std::shared_ptr<tf2_ros::TransformListener> tf_listener_{nullptr};
  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
  std::string target_frame_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<FrameListener>());
  rclcpp::shutdown();
  return 0;
}

要了解产卵海龟背后的服务是如何工作的，请参考编写简单的服务和客户端（C++）教程。现在，让我们看一下与访问框架转换相关的代码。 tf2_ros包含一个TransformListener头文件实现，它使接收转换的任务变得更容易。

#include "tf2_ros/transform_listener.h"

在这里，我们创建一个TransformListener对象。一旦创建了侦听器，它就开始通过连接接收tf2转换，并将它们缓冲最多10秒。

tf_listener_ =
  std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);

最后，我们向侦听器查询特定的转换。我们使用以下参数调用lookup_transform方法：

Target frame目标帧
Source frame源帧
The time at which we want to transform我们想要转变的时间

提供tf2::TimePointZero()只会让我们得到最新的可用转换。所有这些都被包装在一个try-catch块中，以处理可能的异常。

t = tf_buffer_->lookupTransform(
  toFrameRel, fromFrameRel,
  tf2::TimePointZero);

使用文本编辑器打开名为turtle_tf2_demo.launch.py的启动文件，向启动描述中添加两个新节点，添加一个启动参数，然后添加导入。生成的文件应该如下所示：

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration

from launch_ros.actions import Node


def generate_launch_description():
    return LaunchDescription([
        Node(
            package='turtlesim',
            executable='turtlesim_node',
            name='sim'
        ),
        Node(
            package='learning_tf2_cpp',
            executable='turtle_tf2_broadcaster',
            name='broadcaster1',
            parameters=[
                {'turtlename': 'turtle1'}
            ]
        ),
        DeclareLaunchArgument(
            'target_frame', default_value='turtle1',
            description='Target frame name.'
        ),
        Node(
            package='learning_tf2_cpp',
            executable='turtle_tf2_broadcaster',
            name='broadcaster2',
            parameters=[
                {'turtlename': 'turtle2'}
            ]
        ),
        Node(
            package='learning_tf2_cpp',
            executable='turtle_tf2_listener',
            name='listener',
            parameters=[
                {'target_frame': LaunchConfiguration('target_frame')}
            ]
        ),
    ])

Adding a frame (C++)

在前面的教程中，我们通过编写tf2广播器和tf2侦听器重新创建了turtle演示。本教程将教你如何添加额外的固定和动态帧到转换树。事实上，在tf2中添加帧与创建tf2广播器非常相似，但此示例将向您展示tf2的一些附加功能。

对于许多与转换相关的任务，在局部框架内思考更容易。例如，在激光扫描仪的中心处的帧中的激光扫描测量是最容易推理的。 tf2允许您为系统中的每个传感器、链接或关节定义局部框架。当从一个帧变换到另一个帧时，tf2将处理引入的所有隐藏中间帧变换。

tf2 Tree

TF2建立帧的树结构，并且因此不允许帧结构中的闭环。这意味着一个框架只有一个父框架，但它可以有多个子框架。目前，我们的tf2树包含三个帧：world、turtle1和turtle2。这两个turtle帧是world帧的子帧。如果我们想在tf2中添加一个新的帧，现有的三个帧中的一个需要是父帧，新的帧将成为它的子帧。

1 Write the fixed frame broadcaster

在我们的turtle示例中，我们将添加一个新帧carrot1，它将是turtle1的子帧。这一帧将作为第二只海龟的目标。

让我们首先创建源文件。转到我们在前面的教程中创建的learning_tf2_cpp包。输入以下命令下载固定帧广播器代码：

#include <chrono>
#include <functional>
#include <memory>

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_broadcaster.h"

using namespace std::chrono_literals;

class FixedFrameBroadcaster : public rclcpp::Node
{
public:
  FixedFrameBroadcaster()
  : Node("fixed_frame_tf2_broadcaster")
  {
    tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
    timer_ = this->create_wall_timer(
      100ms, std::bind(&FixedFrameBroadcaster::broadcast_timer_callback, this));
  }

private:
  void broadcast_timer_callback()
  {
    geometry_msgs::msg::TransformStamped t;

    t.header.stamp = this->get_clock()->now();
    t.header.frame_id = "turtle1";
    t.child_frame_id = "carrot1";
    t.transform.translation.x = 0.0;
    t.transform.translation.y = 2.0;
    t.transform.translation.z = 0.0;
    t.transform.rotation.x = 0.0;
    t.transform.rotation.y = 0.0;
    t.transform.rotation.z = 0.0;
    t.transform.rotation.w = 1.0;

    tf_broadcaster_->sendTransform(t);
  }

rclcpp::TimerBase::SharedPtr timer_;
  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<FixedFrameBroadcaster>());
  rclcpp::shutdown();
  return 0;
}

2 Write the dynamic frame broadcaster

我们在本教程中发布的额外帧是一个固定的帧，它不会随时间的推移而相对于父帧发生变化。然而，如果你想发布一个移动的帧，你可以编写广播程序来随着时间的推移改变帧。让我们改变我们的carrot1帧，以便它随着时间的推移相对于turtle1帧而改变。现在，输入以下命令下载动态帧广播器代码：

#include <chrono>
#include <functional>
#include <memory>

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_broadcaster.h"

using namespace std::chrono_literals;

const double PI = 3.141592653589793238463;

class DynamicFrameBroadcaster : public rclcpp::Node
{
public:
  DynamicFrameBroadcaster()
  : Node("dynamic_frame_tf2_broadcaster")
  {
    tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
    timer_ = this->create_wall_timer(
      100ms, std::bind(&DynamicFrameBroadcaster::broadcast_timer_callback, this));
  }

private:
  void broadcast_timer_callback()
  {
    rclcpp::Time now = this->get_clock()->now();
    double x = now.seconds() * PI;

    geometry_msgs::msg::TransformStamped t;
    t.header.stamp = now;
    t.header.frame_id = "turtle1";
    t.child_frame_id = "carrot1";
    t.transform.translation.x = 10 * sin(x);
    t.transform.translation.y = 10 * cos(x);
    t.transform.translation.z = 0.0;
    t.transform.rotation.x = 0.0;
    t.transform.rotation.y = 0.0;
    t.transform.rotation.z = 0.0;
    t.transform.rotation.w = 1.0;

    tf_broadcaster_->sendTransform(t);
  }

  rclcpp::TimerBase::SharedPtr timer_;
  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<DynamicFrameBroadcaster>());
  rclcpp::shutdown();
  return 0;
}

Using time (C++)

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

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

1 tf2 and time

让我们回到最后的部分添加框架教程 . 转到learning_tf2_cpp包。打开turtle_tf2_listener.cpp并查看lookupTransform()调用：

transformStamped = tf_buffer_->lookupTransform(
   toFrameRel,
   fromFrameRel,
   tf2::TimePointZero);

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

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

rclcpp::Time now = this->get_clock()->now();
transformStamped = tf_buffer_->lookupTransform(
   toFrameRel,
   fromFrameRel,
   now);

要理解为什么会发生这种情况，我们需要了解缓冲区是如何工作的。首先，每个接收器具有缓冲器，在该缓冲器中存储来自不同tf2广播器的所有坐标变换。其次，当广播器发送转换时，转换进入缓冲区需要一些时间（通常是几毫秒）。因此，当您在时间“now”请求帧转换时，您应该等待几毫秒以等待该信息到达。

2 Wait for transforms

tf2提供了一个很好的工具，它会一直等到转换可用。您可以通过向lookupTransform()添加一个超时参数来使用它。要修复此问题，请按如下所示编辑代码（添加最后一个超时参数）：

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

lookupTransform()可以接受四个参数，其中最后一个是可选的超时。它将阻塞该持续时间，等待超时。

你应该注意到lookupTransform()实际上会阻塞，直到两个turtle之间的转换可用（这通常需要几毫秒）。一旦超时（在本例中为50毫秒），只有转换仍然不可用时才会引发异常。