TF2 监听器（C++）

目标： 学习如何使用 tf2 获取坐标系变换。

教程级别： 中级

预计时间： 10 分钟

背景

在之前的教程中，我们创建了一个 tf2 广播器，将乌龟的位姿发布到 tf2。

在本教程中，我们将创建一个 tf2 监听器，开始实际使用 tf2。

前提条件

本教程假设你已经完成了 tf2 静态广播器教程（C++）和 tf2 广播器教程（C++）。 在之前的教程中，我们创建了一个 learning_tf2_cpp 功能包（package），接下来我们将继续在该功能包中开发。

任务

1 编写监听器节点

首先创建源文件。 进入之前教程中创建的 learning_tf2_cpp 功能包。 在 src 目录下，运行以下命令下载示例监听器代码：

Linux/macOS：

wget https://raw.githubusercontent.com/ros/geometry_tutorials/{DISTRO}/turtle_tf2_cpp/src/turtle_tf2_listener.cpp

Windows：

在 Windows 命令行提示符中：

curl -sk https://raw.githubusercontent.com/ros/geometry_tutorials/{DISTRO}/turtle_tf2_cpp/src/turtle_tf2_listener.cpp -o turtle_tf2_listener.cpp

或在 PowerShell 中：

curl https://raw.githubusercontent.com/ros/geometry_tutorials/{DISTRO}/turtle_tf2_cpp/src/turtle_tf2_listener.cpp -o turtle_tf2_listener.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.hpp"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "turtlesim_msgs/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_msgs::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, [this]() {return this->on_timer();});
  }

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_msgs/srv/Spawn
        auto request = std::make_shared<turtlesim_msgs::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_msgs::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_msgs::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;
}

1.1 代码解析

要了解生成乌龟所用服务的工作原理，请参阅”编写简单的服务与客户端（C++）“教程。

接下来看与获取坐标系变换相关的代码。 tf2_ros 提供了 TransformListener 类，大大简化了接收变换的工作。

#include "tf2_ros/transform_listener.h"

这里创建了一个 TransformListener 对象。 监听器一旦创建，就会在后台接收网络上的 tf2 变换，并缓存最多 10 秒。

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

注意：上面的构造函数（TransformListener(*tf_buffer_)）是简化版本，会在内部创建一个节点来管理订阅。

如果你正在编写一个可组合节点（composable node），或者需要变换监听器遵循节点专属的选项和话题重映射（例如 /tf 的命名空间或重映射），请将 this（或你节点的 NodeInterfaces）传递给构造函数：

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

这样可以确保订阅在现有节点上创建，并继承所有参数和话题配置。

最后，向监听器查询特定的变换。 调用 lookupTransform 方法，参数如下：

1. 目标坐标系（target frame）
2. 源坐标系（source frame）
3. 变换的目标时间点

传入 tf2::TimePointZero 即可获取最新的可用变换。 上述调用包裹在 try-catch 块中，以处理可能出现的异常。

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

得到的变换表示目标乌龟相对于 turtle2 的位置和朝向。 然后利用两只乌龟之间的角度计算速度指令，驱动 turtle2 跟随目标乌龟。 有关 tf2 的更多背景知识，请参阅概念说明部分中的 tf2 页面。

1.2 CMakeLists.txt

返回上一级进入 learning_tf2_cpp 目录，该目录下有 CMakeLists.txt 和 package.xml 文件。

打开 CMakeLists.txt，添加可执行文件并命名为 turtle_tf2_listener，稍后你将通过 ros2 run 来运行它。

add_executable(turtle_tf2_listener src/turtle_tf2_listener.cpp)
target_link_libraries(
    turtle_tf2_listener PUBLIC
    geometry_msgs::geometry_msgs
    rclcpp::rclcpp
    tf2::tf2
    tf2_ros::tf2_ros
    turtlesim_msgs::turtlesim_msgs
)

最后，添加 install(TARGETS…) 部分，以便 ros2 run 能找到你的可执行文件：

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

2 更新 launch 文件

使用文本编辑器打开 src/learning_tf2_cpp/launch 目录下名为 turtle_tf2_demo_launch 的 launch 文件（扩展名为 .py、.xml 或 .yaml），向 launch 描述中添加两个新节点和一个 launch 参数，并添加对应的导入语句。最终文件应如下所示：

Python：

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')}
            ]
        ),
    ])

XML：

<?xml version="1.0" encoding="UTF-8"?>
<launch>
  <node pkg="turtlesim" exec="turtlesim_node" name="sim" />
  <node pkg="learning_tf2_cpp" exec="turtle_tf2_broadcaster" name="broadcaster1">
    <param name="turtlename" value="turtle1" />
  </node>
  <arg name="target_frame" default="turtle1" description="Target frame name." />
  <node pkg="learning_tf2_cpp" exec="turtle_tf2_broadcaster" name="broadcaster2">
    <param name="turtlename" value="turtle2" />
  </node>
  <node pkg="learning_tf2_cpp" exec="turtle_tf2_listener" name="listener">
    <param name="target_frame" value="$(var target_frame)" />
  </node>
</launch>

YAML：

%YAML 1.2
---
launch:
  - node:
      pkg: "turtlesim"
      exec: "turtlesim_node"
      name: "sim"
  - node:
      pkg: "learning_tf2_cpp"
      exec: "turtle_tf2_broadcaster"
      name: "broadcaster1"
      param:
      - name: "turtlename"
        value: "turtle1"
  - arg:
      name: "target_frame"
      default: "turtle1"
      description: "Target frame name."
  - node:
      pkg: "learning_tf2_cpp"
      exec: "turtle_tf2_broadcaster"
      name: "broadcaster2"
      param:
      - name: "turtlename"
        value: "turtle2"
  - node:
      pkg: "learning_tf2_cpp"
      exec: "turtle_tf2_listener"
      name: "listener"
      param:
      - name: "target_frame"
        value: "$(var target_frame)"

这将声明一个 target_frame launch 参数，启动一个用于第二只乌龟的广播器，以及一个订阅这些变换的监听器节点。

3 构建

在工作空间根目录运行 rosdep 来检查缺失的依赖。

Linux：

rosdep install -i --from-path src --rosdistro {DISTRO} -y

macOS/Windows：

rosdep 仅在 Linux 上可用，可直接跳到下一步。

仍在工作空间根目录下构建功能包：

Linux/macOS：

colcon build --packages-select learning_tf2_cpp

Windows：

colcon build --merge-install --packages-select learning_tf2_cpp

打开一个新终端，导航到工作空间根目录，并 source 环境配置文件：

Linux/macOS：

. install/setup.bash

Windows：

在 Windows 命令行提示符中：

call install\setup.bat

或在 PowerShell 中：

.\install\setup.ps1

4 运行

现在可以启动完整的乌龟演示了：

XML：

ros2 launch learning_tf2_cpp turtle_tf2_demo_launch.xml

YAML：

ros2 launch learning_tf2_cpp turtle_tf2_demo_launch.yaml

Python：

ros2 launch learning_tf2_cpp turtle_tf2_demo_launch.py

你应该能看到带有两只乌龟的 turtlesim。 在第二个终端窗口中输入以下命令：

ros2 run turtlesim turtle_teleop_key

要验证是否正常工作，只需用方向键驱动第一只乌龟移动（确保当前活动窗口是运行 turtle_teleop_key 的终端，而不是仿真器窗口），你会看到第二只乌龟紧紧跟随第一只乌龟！

总结

在本教程中，你学习了如何使用 tf2 获取坐标系变换。 至此，你也完成了 tf2 简介教程中开始的 turtlesim 演示。