ROS 2边学边练（35）-- 写一个监听（C++）

前言

        在前面章节我们一直接触的是广播发布者，只管将turtle1的位姿信息广播给tf2系统，然后就没有然后了，发布到tf2系统中的这些数据的着落我们似乎并没有去关心，有发就有收，有来就有往，存在即合理嘛。此篇即给这些数据一个应用着落，去使用tf2中的这些数据。

动动手

编写一个监听节点

        在learning_tf2_cpp包的src路径下，执行下面命令下载turtle_tf2_listener.cpp:

$wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_cpp/src/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. 通过服务通信机制创建一个客户端，发送孵化小海龟请求(海龟名字turtle2 + x/y/theta值)给服务端，具体孵化海龟的事情由服务端执行并返回结果给客户端；
2. 监听并获取某个时间从坐标系turtle1到坐标系turtle2的tf2转换数据；
3. 创建一个发布者，发布经过处理的tf2转换数据到主题/turtle2/cmd_vel，从而控制turtle2的游动。

#include "tf2_ros/transform_listener.h"

         包含此头文件是为了使用tf2_ros下的TransformListener类的监听获取转换数据功能。

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

        一旦创建了监听器，它就开始通过连线接收tf2转换数据，且保存的缓冲时间最高到长达10秒。

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

        最后通过lookupTransform函数查找获取满足条件的转换数据，其中的3个条件参数含义如下：

• toFrameRel:目标帧
• fromFrameRel:源帧
• TimePointZero:我们想要转换的时间的数据，这里提供最新的可用的转换数据 

帧与坐标系概念解释

        在ROS（Robot Operating System，机器人操作系统）中，frame（帧）和坐标系是两个密切相关的概念，它们在机器人感知、定位、导航和控制等任务中发挥着重要作用。

        首先，frame通常指的是一个特定的参考系，用于描述机器人或其他物体在特定时刻的位置和姿态。这个位置和姿态可以包括在三维空间中的位置坐标（x, y, z）以及绕这三个轴的旋转角度（roll, pitch, yaw）。每个frame都有自己的原点、坐标轴和旋转方向，这些属性定义了frame的特性和相对于其他frame的关系。

而坐标系则是用于描述物体位置和姿态的数学工具。在ROS中，常见的坐标系包括map、odom和base_link等。这些坐标系具有不同的特性和用途：

        map坐标系是一个固定的世界坐标系，可以在机器人所在的环境中随意指定一个点作为原点，并规定其具体的朝向。这个坐标系用于统一描述分布在不同地方的机器人和其他设备（如激光雷达、相机等）的坐标。
        odom坐标系是机器人运动的参考坐标系，也是固定的。在这个坐标系中，机器人的位姿是相对于起始点来描述的。通过odom坐标系，可以获取机器人相对于起始点的位置和姿态信息。
        base_link坐标系则与机器人的底盘直接连接，其位置和方向都是任意的。这个坐标系用于描述机器人自身的运动和姿态变化。

在ROS中，这些坐标系和frame之间有着密切的关系。通过定义和转换不同坐标系之间的关系，ROS可以实现对机器人运动状态和环境信息的精确描述和处理。例如，通过map和odom之间的变换，可以得到机器人相对于世界坐标系的位姿；通过base_link坐标系，可以获取机器人自身的运动信息。

        总的来说，frame和坐标系在ROS中都是用于描述机器人和环境状态的重要工具。它们之间的关系和转换是实现机器人感知、定位、导航和控制等任务的基础。

CMakeLists.txt

        打开CMakeLists.txt，需要指定构建的可执行文件名称及依赖，还有安装的路径。将如下内容添加进去。

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
    turtle_tf2_listener
    DESTINATION lib/${PROJECT_NAME})

更新launch文件

        打开上一节我们创建的turtle_tf2_demo_launch.py，我们需要添加2给节点描述，添加一个启动参数，还有需要导入的模块，完整文件内容如下：

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启动参数，为我们将孵化生成的第二只海龟和订阅这些转换数据的监听器启动一个广播程序。

        可以简单概括下整个流程：

1. 海龟1号turtle1的位姿数据通过广播器1（发布）turtle_tf2_broadcaster1广播到tf2系统；
2. 监听器节点turtle_tf2_listener通过服务方式孵化出海龟2号turtle2；
3. 监听器节点turtle_tf2_listener监听获取tf2系统中turtle1帧->turtle2帧的位姿转换数据；
4. 海龟2号turtle2通过广播器2 turtle_tf2_broadcaster2广播其位姿数据到tf2系统
5. 监听器节点turtle_tf2_listener根据获取的turtle1帧->turtle2帧的数据经过处理后发布到主题turtle2/cmd_vel，从而控制turtle2跟随turtle1的步伐游动;
6. tf2系统在其中就是充当了转换各个不同参考系或坐标系下位姿的角色，十分方便和重要。

构建

        进入工作空间根路径下，分别执行如下命令进行依赖检查和最终包构建。

$rosdep install -i --from-path src --rosdistro iron -y

$colcon build --packages-select learning_tf2_cpp

运行

        首先source下环境：

$. install/setup.bash

        接着，启动我们的launch文件：

$ros2 launch learning_tf2_cpp turtle_tf2_demo_launch.py

        出现了两只小海龟（红色为turtle1，黄色为turtle2），其中turtle2是孵化的,由于指定了初始位置，所以根据上面的逻辑会首先游动到turtle1的位置。

        我们再打开另外一个终端，启动控制节点，控制下第一只小海龟，看看后面会发生什么有意思的事情（是否如前所述）：

$ros2 run turtlesim turtle_teleop_key

        确实夫唱妇随了。

本篇完。