编写监听器 (C++)

目标： 了解如何使用 tf2 访问帧转换。

教程级别： 中级

时间： 10 分钟

背景 

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

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

先决条件 

本教程假设您已完成 tf2 静态广播器教程 (C++) 和 tf2 广播器教程 (C++)。

在之前的教程中，我们创建了一个 learning_tf2_cpp 包，我们将从这里继续工作。

任务 

1 编写监听器节点 

我们首先创建源文件。转到我们在上一个教程中创建的“learning_tf2_cpp”包。在“src”目录中输入以下命令下载示例监听器代码：

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

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

In a Windows command line prompt:

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

Or in powershell:

curl https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/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.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;
}

1.1 检查代码

要了解生成海龟背后的服务如何工作，请参阅：doc:`编写简单的服务和客户端（C++）<../../Beginner-Client-Libraries/Writing-A-Simple-Cpp-Service-And-Client>`教程。

现在，让我们看一下与获取帧转换访问权限相关的代码。 ``tf2_ros``包含一个``TransformListener``类，它使接收转换的任务更容易。

#include “tf2_ros/transform_listener.h”

在这里，我们创建一个``TransformListener``对象。一旦创建了侦听器，它就会开始通过网络接收 tf2 转换，并缓冲它们长达 10 秒。

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

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

目标帧
源帧
我们想要转换的时间

提供“tf2::TimePointZero”只会让我们获得最新的可用转换。所有这些都包装在 try-catch 块中以处理可能的异常。

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

由此产生的变换表示目标海龟相对于“turtle2”的位置和方向。然后使用海龟之间的角度来计算跟随目标海龟的速度命令。有关 tf2 的更多一般信息，另请参阅 :doc:“概念部分 <../../../Concepts/Intermediate/About-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)
ament_target_dependencies(
    turtle_tf2_listener
    geometry_msgs
    rclcpp
    tf2
    tf2_ros
    turtlesim
)

最后，添加“install（TARGETS…）”部分，以便“ros2 run”可以找到您的可执行文件：

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

2 更新启动文件 

使用文本编辑器打开“src/learning_tf2_cpp/launch”目录中名为“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')}
            ]
        ),
    ])

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

3 构建 

在工作区的根目录中运行“rosdep”以检查是否缺少依赖项。

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

rosdep only runs on Linux, so you will need to install geometry_msgs and turtlesim dependencies yourself

仍然在工作区的根目录中，构建您的包：

colcon build --packages-select learning_tf2_cpp

colcon build --packages-select learning_tf2_cpp

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

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

. install/setup.bash

. install/setup.bash

# CMD
call install\setup.bat

# Powershell
.\install\setup.ps1

4 运行 

现在您可以开始完整的海龟演示了：

ros2 launch learning_tf2_cpp turtle_tf2_demo.launch.py

您应该看到有两只海龟的海龟模拟器。在第二个终端窗口中输入以下命令：

ros2 run turtlesim turtle_teleop_key

要查看是否有效，只需使用箭头键绕着第一只海龟行驶（确保您的终端窗口处于活动状态，而不是模拟器窗口），您就会看到第二只海龟跟在第一只海龟后面！

摘要 

在本教程中，您学习了如何使用 tf2 来访问框架转换。您还完成了编写自己的 turtlesim 演示，该演示是您在 Introduction to tf2 教程中首次尝试的。