ROS2高效学习第十章 -- ros2 高级组件其三之 TF2

1 前言与资料

本文我们介绍 ros2 高级组件之 TF2，TF 即 transform，坐标变换。早在 ros1 系列课程中，我们用了三篇博客，系统介绍了坐标变换的背景知识以及 API 编程。因此本文不再详述坐标变换的背景知识，读者可以自行阅读：ROS高效进阶第一章 – ROS高级组件之 TF坐标变换 其一 进行补充。
本文将专注于 ros2 的 TF2 库的 API 编程，同样实现小乌龟2跟随小乌龟1样例，这个样例在：ROS高效进阶第一章 – ROS高级组件之 TF坐标变换 其二中已经有实现。借此机会，我们可以比较两个 ros 版本的 TF 库 API 接口的不同之处。
除此之外，为了增加本文样例内容的丰富度，我们将采用之前学习的 ros2 component 机制：ROS2高效学习第十章 – ros2 高级组件其一之 component 合并进程启动，巩固已有的学习成果。
本文参考资料如下：
（1）ROS高效进阶第一章 – ROS高级组件之 TF坐标变换 其一
（2）ROS高效进阶第一章 – ROS高级组件之 TF坐标变换 其二
（3）ROS2高效学习第十章 – ros2 高级组件其一之 component 合并进程启动
（4）Introduction-To-Tf2
（5）Writing-A-Tf2-Broadcaster-Cpp
（6）Writing-A-Tf2-Listener-Cpp
（7）Writing-A-Tf2-Broadcaster-Py
（8）Writing-A-Tf2-Listener-Py
本系列博客汇总：ROS2 高效学习系列。

2 正文

2.1 learning_tf2_cpp

（1）创建 learning_tf2_cpp 软件包和相关文件

cd ~/colcon_ws/src
ros2 pkg create --build-type ament_cmake --license Apache-2.0 learning_tf2_cpp --dependencies rclcpp_components geometry_msgs rclcpp tf2 tf2_ros turtlesim
cd learning_tf2_cpp
mkdir launch
touch launch/turtle_tf2_demo_launch.py
touch include/learning_tf2_cpp/turtle_tf_broadcaster.hpp
touch include/learning_tf2_cpp/turtle_tf_listener.hpp
touch include/learning_tf2_cpp/visibility_control.h
touch src/turtle_tf_broadcaster.cpp src/turtle_tf_listener.cpp

（2）编写 tf 广播器头文件：turtle_tf_broadcaster.hpp

#ifndef LEARNING_TF2_CPP__TURTLE_TF_BROADCASTER_HPP_
#define LEARNING_TF2_CPP__TURTLE_TF_BROADCASTER_HPP_

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

#include "learning_tf2_cpp/visibility_control.h"

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

namespace learning_tf2_cpp
{

class TurtleTfBroadcaster : public rclcpp::Node
{
public:
    LEARNING_TF2_CPP_PUBLIC
    explicit TurtleTfBroadcaster(const rclcpp::NodeOptions & options);

private:
    void handle_turtle_pose(const std::shared_ptr<turtlesim::msg::Pose> msg);

private:
    rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr turtle_pose_sub_;
    std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
    std::string turtle_name_;
};

} // namespace learning_tf2_cpp
#endif // LEARNING_TF2_CPP__TURTLE_TF_BROADCASTER_HPP_

（3）编写 tf 广播器：turtle_tf_broadcaster.cpp

#include "learning_tf2_cpp/turtle_tf_broadcaster.hpp"

#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(learning_tf2_cpp::TurtleTfBroadcaster)

namespace learning_tf2_cpp
{

TurtleTfBroadcaster::TurtleTfBroadcaster(const rclcpp::NodeOptions & options)
                                        : Node("turtle_tf_broadcaster", options) {
    auto param_desc = rcl_interfaces::msg::ParameterDescriptor{};
    param_desc.description = "This parameter is used to pass in the name of the turtle";
    turtle_name_ = this->declare_parameter<std::string>("turtle_name", "turtle1", param_desc);

    tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);

    std::string topic_name = "/" + turtle_name_ + "/pose";
    turtle_pose_sub_ = this->create_subscription<turtlesim::msg::Pose>(
        topic_name, 10, std::bind(&TurtleTfBroadcaster::handle_turtle_pose, this, std::placeholders::_1));

}
// 小乌龟 TF 广播器的核心是通过订阅小乌龟的 Pose 信息，将 Pose 信息经过加工得到小乌龟相对 world 坐标系的 TF 坐标变换，
// 坐标变换主要分为两部分，一是平移变换，二是旋转变换，然后通过 TF 广播器将 TF 信息广播出去。
// 关于 Pose 信息，欧拉角以及四元数的理解，请参考：https://blog.csdn.net/cy1641395022/article/details/131236155
// 关于计算过程的理解，请参考：https://blog.csdn.net/cy1641395022/article/details/131236155
void TurtleTfBroadcaster::handle_turtle_pose(const std::shared_ptr<turtlesim::msg::Pose> msg) {
    geometry_msgs::msg::TransformStamped tf_stamped;

    tf_stamped.header.stamp = this->now();
    tf_stamped.header.frame_id = "world";
    tf_stamped.child_frame_id = this->turtle_name_;

    tf_stamped.transform.translation.x = msg->x;
    tf_stamped.transform.translation.y = msg->y;
    tf_stamped.transform.translation.z = 0.0;

    tf2::Quaternion tf_q;
    tf_q.setRPY(0, 0, msg->theta);
    
    tf_stamped.transform.rotation.x = tf_q.x();
    tf_stamped.transform.rotation.y = tf_q.y();
    tf_stamped.transform.rotation.z = tf_q.z();
    tf_stamped.transform.rotation.w = tf_q.w();
    
    this->tf_broadcaster_->sendTransform(tf_stamped);
}
} // namespace learning_tf2_cpp

（4）编写 tf 接收器头文件：turtle_tf_listener.hpp

#ifndef LEARNING_TF2_CPP__TURTLE_TF_LISTENER_HPP_
#define LEARNING_TF2_CPP__TURTLE_TF_LISTENER_HPP_

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

#include "learning_tf2_cpp/visibility_control.h"

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

namespace learning_tf2_cpp
{

class TurtleTfListener : public rclcpp::Node
{
public:
    LEARNING_TF2_CPP_PUBLIC
    explicit TurtleTfListener(const rclcpp::NodeOptions & options);

private:
    void on_timer();

private:
    bool turtle_spawning_service_ready_ = false;
    bool turtle_spawned_ = false;

    std::string target_frame_;
    std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
    std::shared_ptr<tf2_ros::TransformListener> tf_listener_;

    rclcpp::Client<turtlesim::srv::Spawn>::SharedPtr spawn_turtle_client_;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr turtle_cmd_pub_;
    rclcpp::TimerBase::SharedPtr timer_;
};

} // namespace learning_tf2_cpp
#endif // LEARNING_TF2_CPP__TURTLE_TF_LISTENER_HPP_

（5）编写 tf 接收器：turtle_tf_listener.cpp

#include "learning_tf2_cpp/turtle_tf_listener.hpp"

#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(learning_tf2_cpp::TurtleTfListener)

namespace learning_tf2_cpp
{

TurtleTfListener::TurtleTfListener(const rclcpp::NodeOptions & options)
                                        : Node("turtle_tf_listener", options) {
    auto param_desc = rcl_interfaces::msg::ParameterDescriptor{};
    param_desc.description = "This parameter is used to pass in the name of the target turtle";
    target_frame_ = this->declare_parameter<std::string>("target_frame", "turtle1", param_desc);

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

    spawn_turtle_client_ = this->create_client<turtlesim::srv::Spawn>("spawn");
    turtle_cmd_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("turtle2/cmd_vel", 10);

    timer_ = this->create_wall_timer(std::chrono::seconds(1), std::bind(&TurtleTfListener::on_timer, this));


    RCLCPP_INFO(this->get_logger(), "TurtleTfListener has been started");
}
// TF 监听器的逻辑是先生成一个小乌龟2，然后定时订阅在小乌龟2坐标系下，目标小乌龟的 TF 信息，
// 拿到 TF 信息后，计算得到小乌龟2的 Twist 的控制命令，然后以 1Hz 的频率发布到小乌龟2的 cmd_vel 话题上。
// Twist 的理解，参考：https://blog.csdn.net/cy1641395022/article/details/131236155
// 计算过程的理解，参考：https://blog.csdn.net/cy1641395022/article/details/131236155
void TurtleTfListener::on_timer() {
    if (false == turtle_spawning_service_ready_) {
        if (false == spawn_turtle_client_->service_is_ready()) {
            RCLCPP_INFO(this->get_logger(), "Waiting for the spawn service to be ready");
            return;
        }
        turtle_spawning_service_ready_ = true;
    }

    if (false == turtle_spawned_) {
        auto request = std::make_shared<turtlesim::srv::Spawn::Request>();
        request->x = 2.0;
        request->y = 2.0;
        request->theta = 0.0;
        request->name = "turtle2";

        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_spawned_ = true;
                  RCLCPP_INFO(this->get_logger(), "Spawn turtle2 successfully");
            } else {
                RCLCPP_ERROR(this->get_logger(), "Service callback result mismatch");
            }
        };
        auto result = spawn_turtle_client_->async_send_request(request, response_received_callback);
        return;
    }

    geometry_msgs::msg::TransformStamped tf_stamped;
    try {
        tf_stamped = tf_buffer_->lookupTransform("turtle2", target_frame_, tf2::TimePointZero);
    } catch (tf2::TransformException & ex) {
        RCLCPP_ERROR(this->get_logger(), "Could not lookup transform: %s", ex.what());
        return;
    }

    geometry_msgs::msg::Twist cmd_vel;
    cmd_vel.linear.x = 0.5 * std::sqrt(pow(tf_stamped.transform.translation.x, 2) + pow(tf_stamped.transform.translation.y, 2));
    cmd_vel.angular.z = 1.0 * std::atan2(tf_stamped.transform.translation.y, tf_stamped.transform.translation.x);

    RCLCPP_INFO(this->get_logger(), "send cmd_vel to turtle2: linear.x: %f, angular.z: %f", cmd_vel.linear.x, cmd_vel.angular.z);
    turtle_cmd_pub_->publish(cmd_vel);
}
}

（6）编写 visibility_control.h：可见性头文件是为了让程序兼容 windows 和 linux，直接套模板改的，这里不再列出，参考：visibility_control
（7）编写 turtle_tf2_demo_launch.py

import os
from ament_index_python.packages import get_package_share_directory
import launch
from launch_ros.actions import ComposableNodeContainer
from launch_ros.descriptions import ComposableNode
from launch_ros.actions import Node

def generate_launch_description():
    # 启动turtlesim节点，这个是基础环境
    turtlesim_node = Node(
        package='turtlesim',
        executable='turtlesim_node',
        name='turtlesim_node'
    )
    # 启动turtle1的tf广播节点，广播turtle1的tf
    turtle1_tf_broadcaster = ComposableNodeContainer(
            name='turtle1_tf_broadcaster',
            namespace='',
            package='rclcpp_components',
            executable='component_container',
            composable_node_descriptions=[
                ComposableNode(
                    package='learning_tf2_cpp',
                    plugin='learning_tf2_cpp::TurtleTfBroadcaster',
                    name='turtle1_tf_bc',
                    parameters=[{'turtle_name': 'turtle1'}])
            ],
            output='screen',
    )
    # 启动turtle2的tf广播节点，广播turtle2的tf
    turtle2_tf_broadcaster = ComposableNodeContainer(
            name='turtle2_tf_broadcaster',
            namespace='',
            package='rclcpp_components',
            executable='component_container',
            composable_node_descriptions=[
                ComposableNode(
                    package='learning_tf2_cpp',
                    plugin='learning_tf2_cpp::TurtleTfBroadcaster',
                    name='turtle2_tf_bc',
                    parameters=[{'turtle_name': 'turtle2'}])
            ],
            output='screen',
    )
    # 启动rviz2，加载turtlesim的 world 和两个小乌龟坐标系的可视化配置
    turtlesim_world_rviz_config = os.path.join(
        get_package_share_directory('learning_tf2_cpp'),
        'config',
        'turtle_tf.rviz'
    )
    turtlesim_world_rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        arguments=['-d', turtlesim_world_rviz_config]
    )
    # 启动tf监听节点，监听在 turtle2 坐标系下的 turtle1 的 tf，并转换为 turtle2 的控制指令
    turtle2_listen_turtle1 = ComposableNodeContainer(
            name='turtle2_listen_turtle1',
            namespace='',
            package='rclcpp_components',
            executable='component_container',
            composable_node_descriptions=[
                ComposableNode(
                    package='learning_tf2_cpp',
                    plugin='learning_tf2_cpp::TurtleTfListener',
                    name='turtle_tf_ls',
                    parameters=[{'target_frame': 'turtle1'}])
            ],
            output='screen',
    )
    return launch.LaunchDescription([turtlesim_node, turtle1_tf_broadcaster, turtle2_tf_broadcaster, turtlesim_world_rviz_node, turtle2_listen_turtle1])

（8）编写 CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(learning_tf2_cpp)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_components REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(turtlesim REQUIRED)

include_directories(include)

add_library(tf_broadcaster SHARED src/turtle_tf_broadcaster.cpp)
ament_target_dependencies(tf_broadcaster rclcpp rclcpp_components tf2 tf2_ros turtlesim)
rclcpp_components_register_nodes(tf_broadcaster "learning_tf2_cpp::TurtleTfBroadcaster")

add_library(tf_listener SHARED src/turtle_tf_listener.cpp)
ament_target_dependencies(tf_listener rclcpp rclcpp_components tf2 tf2_ros turtlesim)
rclcpp_components_register_nodes(tf_listener "learning_tf2_cpp::TurtleTfListener")

install(TARGETS 
  tf_broadcaster
  tf_listener
  LIBRARY DESTINATION lib
)

install(DIRECTORY
  launch
  config
  DESTINATION share/${PROJECT_NAME}
)

ament_package()

（9）编译并运行

cd ~/colcon_ws/src
colcon build --packages-select learning_tf2_cpp
source install/local_setup.bash
ros2 launch learning_tf2_cpp turtle_tf2_demo_launch.py
# 再开一个窗口，光标留在这个窗口，使用方向键控制 turtle1 主动运动
ros2 run turtlesim turtle_teleop_key

2.2 learning_tf2_py

（1）learning_tf2_py 是 python 版本的 learning_tf2_cpp，这里不再详细讲述了，源码在：learning_tf2_py

2.3 三个 TF 的调试命令

# 参考 2.1 节，启动 learning_tf2_cpp 样例
# 可以得到当前树状坐标系的拓朴图
ros2 run tf2_tools view_frames

# 打印在 turtle2 坐标系下，turtle1 的 TF 值
ros2 run tf2_ros tf2_echo turtle2 turtle1

# 监控并统计 TF 信息收发的情况
ros2 run tf2_ros tf2_monitor turtle2 turtle1

3 总结

本文样例托管在本人的github上：learning_tf2_cpp，learning_tf2_py，欢迎复现提问。