编写监听器（C++）

目标 了解如何使用 tf2 获取帧变换。

辅导水平： 中级

时间 10 分钟

背景介绍

在之前的教程中，我们创建了一个 tf2 广播器，用于向 tf2 发布乌龟的姿势。

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

先决条件

本教程假定您已经完成了 tf2 静态广播员教程（C++） 和 TF2 广播员教程（C++）.在上一教程中，我们创建了一个 learning_tf2_cpp 这也是我们将继续开展工作的基础。

任务

1 写入监听节点

首先创建源文件。转到 learning_tf2_cpp 软件包。在 来源 输入以下命令，下载监听器示例代码：

wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_cpp/src/turtle_tf2_listener.cpp

使用您喜欢的文本编辑器打开文件。

#include 时间顺序<chrono>；
#include 功能强大；
#include <内存>；
#include <字符串>；

#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"；

使用 命名空间 标准::计时器;

类 帧监听器 : 公 rclcpp::节点
{
公:
  帧监听器()
  : 节点("turtle_tf2_frame_listener"；),
    乌龟产卵服务准备就绪(错误),
    乌龟产卵(错误)
  {
    // 声明并获取 `target_frame` 参数
    目标框架 = 此->；declare_parameter<；标准::字符串>；(target_frame"；, "turtle1"；);

    tf_buffer_ =
      标准::make_unique<；tf2_ros::缓冲器>；(此->；获取时钟());
    tf_listener_ =
      标准::共享<；tf2_ros::变换监听器>；(*tf_buffer_);

    // 创建一个客户端来生成乌龟
    产卵器_ =
      此->；创建客户端<；海龟::服务::再生>；("spawn"；);

    // 创建龟 2 速度发布器
    出版商_ =
      此->；创建出版商<；几何参数::信息::扭转>；("turtle2/cmd_vel"；, 1);

    // 每秒调用一次 on_timer 函数
    定时器 = 此->；创建隔离墙计时器(
      1s, [此]() {返回 此->；on_timer();});
  }

私人:
  空白 on_timer()
  {
    // 在变量中存储帧名，这些变量将用于
    // 计算变换
    标准::字符串 fromFrameRel = 目标框架.c_str();
    标准::字符串 toFrameRel = "turtle2"；;

    如果 (乌龟产卵服务准备就绪) {
      如果 (乌龟产卵) {
        几何参数::信息::TransformStamped t;

        // 查找 target_frame 和 turtle2 帧之间的变换
        // 为乌龟 2 发送速度指令，使其到达目标帧
        尝试 {
          t = tf_buffer_->；查找变换(
            toFrameRel, fromFrameRel,
            tf2::时间点零);
        } 捕捉 (缢 tf2::转换异常 及样品； 从) {
          RCLCPP_INFO(
            此->；get_logger(), "无法将 %s 转换为 %s： %s"；,
            toFrameRel.c_str(), fromFrameRel.c_str(), 从.什么());
          返回;
        }

        几何参数::信息::扭转 信息;

        天电 缢 双人 缩放旋转率 = 1.0;
        信息.角度.z = 缩放旋转率 * atan2(
          t.变.译文.y,
          t.变.译文.x);

        天电 缢 双人 缩放前进速度 = 0.5;
        信息.线形.x = 缩放前进速度 * 平方(
          啪(t.变.译文.x, 2) +
          啪(t.变.译文.y, 2));

        出版商_->；发布(信息);
      } 不然 {
        RCLCPP_INFO(此->；get_logger(), "成功生成"；);
        乌龟产卵 = 真;
      }
    } 不然 {
      // 检查服务是否准备就绪
      如果 (产卵器_->；服务已就绪()) {
        // 用乌龟名称和坐标初始化请求
        // 请注意，x、y 和 theta 在 turtlesim/srv/Spawn 中定义为浮点数。
        汽车 要求 = 标准::共享<；海龟::服务::再生::要求>；();
        要求->；x = 4.0;
        要求->；y = 2.0;
        要求->；θ = 0.0;
        要求->；名字 = "turtle2"；;

        // 调用请求
        使用 服务响应未来 =
          rclcpp::客户<；海龟::服务::再生>:：共享未来;
        汽车 响应接收回调 = [此](服务响应未来 未来) {
            汽车 结果 = 未来.获取();
            如果 (strcmp(结果->；名字.c_str(), "turtle2"；) == 0) {
              乌龟产卵服务准备就绪 = 真;
            } 不然 {
              RCLCPP_ERROR(此->；get_logger(), 服务回调结果不匹配"；);
            }
          };
        汽车 结果 = 产卵器_->；异步发送请求(要求, 响应接收回调);
      } 不然 {
        RCLCPP_INFO(此->；get_logger(), 服务尚未准备就绪"；);
      }
    }
  }

  // 用布尔值来存储信息
  // 如果可以提供产卵龟服务
  bool 乌龟产卵服务准备就绪;
  // 如果乌龟成功产卵
  bool 乌龟产卵;
  rclcpp::客户<；海龟::服务::再生>:：SharedPtr 产卵器_{nullptr};
  rclcpp::定时器基数::SharedPtr 定时器{nullptr};
  rclcpp::出版商<；几何参数::信息::扭转>:：SharedPtr 出版商_{nullptr};
  标准::共享_ptr<；tf2_ros::变换监听器>； tf_listener_{nullptr};
  标准::唯一参数<；tf2_ros::缓冲器>； tf_buffer_;
  标准::字符串 目标框架;
};

int 主要(int 参数, 烧焦 * 参数[])
{
  rclcpp::启动(参数, 参数);
  rclcpp::后旋(标准::共享<；帧监听器>；());
  rclcpp::关闭();
  返回 0;
}

1.1 检查代码

要了解产卵龟背后的服务是如何运作的，请参阅 编写简单的服务和客户端（C++） 教程。

现在，让我们来看看访问帧变换的相关代码。代码 tf2_ros 包含一个 变换监听器 类，使接收转换任务变得更容易。

#include "tf2_ros/transform_listener.h"；

在这里，我们创建一个 变换监听器 对象。一旦创建了监听器，它就会开始通过线路接收 tf2 变换，并将其缓冲长达 10 秒。

tf_listener_ =
  标准::共享<；tf2_ros::变换监听器>；(*tf_buffer_);

最后，我们向监听器查询特定的转换。我们调用 查找变换 方法，参数如下

  1. 目标框架

  2. 来源框架

  3. 我们希望转换的时间

提供 tf2::TimePointZero 将只获取最新的可用转换。所有这些都包裹在一个 try-catch 块中，以处理可能出现的异常。

t = tf_buffer_->；查找变换(
  toFrameRel, fromFrameRel,
  tf2::时间点零);

由此产生的变换代表了目标海龟相对于 乌龟2.然后利用海龟之间的角度计算出速度指令，以跟踪目标海龟。有关 tf2 的更多一般信息，请参阅 概念部分的 tf2 页面.

1.2 CMakeLists.txt

导航一级回到 learning_tf2_cpp 目录中的 CMakeLists.txt 和 package.xml 文件的位置。

现在打开 CMakeLists.txt 添加可执行文件并命名 乌龟_tf2_监听器，稍后将与 玫瑰2 运行.

add_executable(turtle_tf2_listener src/turtle_tf2_listener.cpp)
ament_target_dependencies(
    乌龟_tf2_监听器
    几何参数
    rclcpp
    tf2
    tf2_ros
    海龟
)

最后，添加 install(TARGETS...) 因此 玫瑰2 运行 能找到你的可执行文件：

install(TARGETS
    乌龟_tf2_监听器
    DESTINATION lib/${PROJECT_NAME})

2 更新启动文件

打开名为 turtle_tf2_demo_launch.py 在 src/learning_tf2_cpp/launch 目录下，在启动说明中添加两个新节点，添加一个启动参数，并添加导入。生成的文件应该如下所示

从 启动 舶来品 启动说明
从 launch.actions 舶来品 声明启动参数
从 launch.substitutions 舶来品 启动配置

从 launch_ros.actions 舶来品 节点


捍卫 生成发射描述():
    返回 启动说明([
        节点(
            包装='turtlesim'；,
            可执行='turtlesim_node',
            名字='sim'；
        ),
        节点(
            包装='learning_tf2_cpp',
            可执行='turtle_tf2_broadcaster'；,
            名字='broadcaster1',
            参数=[
                {'turtlename'；: 'turtle1'；}
            ]
        ),
        声明启动参数(
            'target_frame', 默认值='turtle1'；,
            描述='目标帧名称；
        ),
        节点(
            包装='learning_tf2_cpp',
            可执行='turtle_tf2_broadcaster'；,
            名字='broadcaster2',
            参数=[
                {'turtlename'；: 'turtle2'；}
            ]
        ),
        节点(
            包装='learning_tf2_cpp',
            可执行='turtle_tf2_listener',
            名字='听众'；,
            参数=[
                {'target_frame': 启动配置('target_frame')}
            ]
        ),
    ])

这将声明一个 target_frame 启动参数，为我们将生成的第二只海龟启动一个广播器，并启动一个监听器来订阅这些转换。

3 建

运行 rosdep 在工作区的根目录下，检查是否存在缺失的依赖项。

rosdep install -i --from-path src --rosdistro jazzy -y

还是在工作区的根目录下，构建你的软件包：

colcon build --packages-select learning_tf2_cpp

打开一个新终端，导航到工作区的根目录，然后获取设置文件：

install/setup.bash

4 运行

现在你可以开始完整的乌龟演示了：

ros2 launch learning_tf2_cpp turtle_tf2_demo_launch.py

你会看到海龟模拟器上有两只海龟。在第二个终端窗口中键入以下命令：

ros2 run turtlesim turtle_teleop_key

要查看是否正常，只需使用箭头键绕过第一只乌龟（确保终端窗口处于活动状态，而不是模拟器窗口），就会看到第二只乌龟跟在第一只乌龟后面！

摘要

在本教程中，您学会了如何使用 tf2 访问帧变换。您还完成了自己编写的 turtlesim 演示，该演示在 tf2 简介 教程。