ROS2学习记录——TF坐标变换工具（二）

        上篇文章中宏观上介绍下TF的核心功能作用，并简单演示了如何在命令行中发布坐标变换关系。本文将继续学习TF坐标变换工具，主要涉及如何用C++节点发布静态坐标系和动态坐标系，以及如何用C++节点查询TF关系。

        往期链接：

        ROS2学习记录——工作空间与功能包创建_ros2 创建功能包-CSDN博客

        ROS2学习记录——运行第一个节点_从零开始的ros2学习记录-CSDN博客

        ROS2学习记录——以海龟模拟器初探话题通信（一）-CSDN博客

        ROS2学习记录——以海龟模拟器初探话题通信（二）-CSDN博客

        ROS2学习记录——服务与参数通信-CSDN博客

        ROS2学习记录——在C++节点中使用参数-CSDN博客

        ROS2学习记录——TF坐标变换工具（一）_ros2如何搭建tf-CSDN博客

---

 1 C++发布静态T

        新建一个demo_tf的功能包，输入如下命令：

ros2 pkg create demo_tf --build-type ament_cmake --dependencies rclcpp tf2 tf2_ros geometry_msgs tf2_geometry_msgs --license Apache-2.0

        其中，tf2和tf2_ros为用tf的基础依赖，geometry_msgs为消息接口依赖，tf2_geometry_msgs用于提供消息类型的转换函数。

        在功能包的src文件夹下创建static_tf_broadcast.cpp文件，代码如下：

#include <memory>
#include "geometry_msgs/msg/transform_stamped.hpp"  // 提供消息接口
#include "rclcpp/rclcpp.hpp"
#include "tf2/LinearMath/Quaternion.h"              // 提供 tf2::Quaternion 类
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"  // 提供消息类型转换函数
#include "tf2_ros/static_transform_broadcaster.h"   // 提供静态坐标系广播类class StaticTFBroadcaster : public rclcpp::Node {
public:StaticTFBroadcaster() : Node("tf_broadcaster_node") {// 创建静态广播器并发布broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);this->publish_tf();}void publish_tf() {geometry_msgs::msg::TransformStamped transform;transform.header.stamp = this->get_clock()->now();transform.header.frame_id = "map";transform.child_frame_id = "target_point";transform.transform.translation.x = 5.0;transform.transform.translation.y = 3.0;transform.transform.translation.z = 0.0;tf2::Quaternion quat;quat.setRPY(0, 0, 60 * M_PI / 180);  // 角度制欧拉角转四元数transform.transform.rotation = tf2::toMsg(quat);  // 转成消息接口类型broadcaster_->sendTransform(transform);}private:std::shared_ptr<tf2_ros::StaticTransformBroadcaster> broadcaster_;
};int main(int argc, char** argv) {rclcpp::init(argc, argv);auto node = std::make_shared<StaticTFBroadcaster>();rclcpp::spin(node);rclcpp::shutdown();return 0;
}

        首先引入相关头文件，接着定义 StaticTFBroadcaster 节点类，在类中声明了一个静态坐标系广播器类的共享指针，然后在构造函数中进行初始化，初始化完成后在构造函数中调用 publish_tf 方法进行静态 TF 的发布。

        在 publish_tf 方法中，首先创建一个 TransformStamped 消息接口对象 transform，接着使用 this->get_clock()->now() 获取当前时间，并对 transform 中的时间戳进行赋值。然后依次对坐标系名称、平移部分赋值。对于旋转部分则先定义一个 tf2::Quaternion 类的对象 quat，然后使用 quat.setRPY 将欧拉角转换成四元数并放入 quat 中，接着调用 tf2::toMsg(quat) 将 quat 转换成 rotation 的对应类型并赋值，最后调用 broadcaster_ 的 sendTransform 方法将坐标变换发布出去。

        静态变换只需要发布一次，因为 ROS 2 会为订阅者保留数据，当出现新的订阅者时，可以直接获取保留的数据。

        接着在cmakelists.txt中注册节点并安装，代码如下：

add_executable(static_tf_broadcaster src/static_tf_broadcaster.cpp)
ament_target_dependencies(static_tf_broadcaster rclcpp tf2 tf2_ros geometry_msgs tf2_geometry_msgs)install(TARGETSstatic_tf_broadcasterDESTINATION lib/${PROJECT_NAME}
)ament_package()

        重新构建功能包并运行节点，可以在终端中查看map和target_point之间的关系，一个终端中运行节点，一个终端中运行下图命令。

2 C++发布动态TF

        相比于静态tf，动态tf最大的区别就是需要不断向外发布坐标变换的消息，在功能包的src文件夹下创建dynamic_tf_broadcast.cpp文件，代码如下：

#include <memory>
#include "geometry_msgs/msg/transform_stamped.hpp"  // 提供消息接口
#include "rclcpp/rclcpp.hpp"
#include "tf2/LinearMath/Quaternion.h"              // 提供 tf2::Quaternion 类
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"  // 提供消息类型转换函数
#include "tf2_ros/transform_broadcaster.h"          // 提供坐标系广播类
#include <chrono>                                   // 引入时间相关头文件// 使用时间单位的字面量，可以在代码中使用 s 和 ms 表示时间
using namespace std::chrono_literals;class DynamicTFBroadcaster : public rclcpp::Node {
public:DynamicTFBroadcaster() : Node("dynamic_tf_broadcaster"){tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);timer_ = create_wall_timer(10ms, std::bind(&DynamicTFBroadcaster::publishTransform, this));}void publishTransform(){geometry_msgs::msg::TransformStamped transform;transform.header.stamp = this->get_clock()->now();transform.header.frame_id = "map";transform.child_frame_id = "base_link";transform.transform.translation.x = 2.0;transform.transform.translation.y = 3.0;transform.transform.translation.z = 0.0;tf2::Quaternion quat;quat.setRPY(0, 0, 30 * M_PI / 180);  // 角度制欧拉角转四元数transform.transform.rotation = tf2::toMsg(quat);  // 转换消息接口类型tf_broadcaster_->sendTransform(transform);}private:std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;rclcpp::TimerBase::SharedPtr timer_;
};int main(int argc, char **argv)
{rclcpp::init(argc, argv);auto node = std::make_shared<DynamicTFBroadcaster>();rclcpp::spin(node);rclcpp::shutdown();return 0;
}

        为了使用坐标广播器类，包含了头文件 tf2_ros/transform_broadcaster.h。接着定义了 DynamicTFBroadcaster 类，在类中声明了广播器的共享指针和定时器对象，然后在构造函数中进行初始化，其中定时器初始化周期为10ms，这是因为动态坐标系变换需要持续的发布，否则订阅端将无法收到最新的变换消息。

        在publishTransform方法中，首先构造了消息接口对象transform，然后分别对数据进行赋值，最后调用 sendTransform 发布数据。

        在CMakeLists.txt 对 dynamic_tf_broadcaster 节点进行注册，重新构建后运行动态发布节点以及静态发布节点。接着就可以使用命令行查看 base_link 到 target_point 之间的关系了：

3 C++查询TF关系

        在功能包的src文件夹下创建tf_listener.cpp文件，代码如下：

#include <memory>
#include "geometry_msgs/msg/transform_stamped.hpp"  // 提供消息接口
#include "rclcpp/rclcpp.hpp"
#include "tf2/LinearMath/Quaternion.h"              // 提供 tf2::Quaternion 类
#include "tf2/utils.h"                              // 提供 tf2::getEulerYPR 函数
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"  // 提供消息类型转换函数
#include "tf2_ros/buffer.h"                         // 提供 TF 缓冲类 Buffer
#include "tf2_ros/transform_listener.h"             // 提供坐标监听器类
#include <chrono>                                   // 引入时间相关头文件
using namespace std::chrono_literals;class TFListener : public rclcpp::Node {
public:TFListener() : Node("tf_listener") {buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());listener_ = std::make_shared<tf2_ros::TransformListener>(*buffer_, this);timer_ = this->create_wall_timer(5s, std::bind(&TFListener::getTransform, this));}void getTransform() {try {// 等待变换可用const auto transform = buffer_->lookupTransform("base_link", "target_point", this->get_clock()->now() - now(),rclcpp::Duration::from_seconds(1.0f));// 读取并输出变换信息const auto &translation = transform.transform.translation;const auto &rotation = transform.transform.rotation;double yaw, pitch, roll;tf2::getEulerYPR(rotation, yaw, pitch, roll);  // 四元数转欧拉角RCLCPP_INFO(get_logger(), "平移分量: (%.f, %.f, %.f)", translation.x, translation.y, translation.z);RCLCPP_INFO(get_logger(), "旋转分量: (%.f, %.f, %.f)", roll, pitch, yaw);} catch (tf2::TransformException &ex) {RCLCPP_WARN(get_logger(), "异常: %s", ex.what());  // 处理异常}}private:std::shared_ptr<tf2_ros::Buffer> buffer_;std::shared_ptr<tf2_ros::TransformListener> listener_;rclcpp::TimerBase::SharedPtr timer_;
};int main(int argc, char **argv) {rclcpp::init(argc, argv);auto node = std::make_shared<TFListener>();rclcpp::spin(node);rclcpp::shutdown();return 0;
}

        头文件 transform_listener.h，用于订阅 TF 坐标变换信息；tf2_ros/buffer.h，该类负责缓存接收到的TF数据；另外引入了tf2/utils.h，用于提供将四元数转换为欧拉角的工具函数。

        在构造函数中，首先通过 make_shared 创建了 Buffer 对象，并传入当前节点的时钟 clock 作为参数。随后，使用该 Buffer 对象初始化 TransformListener 实例。需要注意的是，TransformListener 的第一个参数是 Buffer 的引用，因此通过 *buffer_ 获取其指向的对象；第二个参数为当前节点 this。最后，创建了一个定时器，每隔 5 秒调用一次 getTransform 方法。

        在 getTransform 方法中，由于查询可能失败，因此使用 try-catch 块来捕获异常。函数中调用 buffer_->lookupTransform 来查询从 base_link 到 target_point 的坐标变换，最后两个参数分别表示起始时间（当前时刻）和超时时间（1 秒）。查询成功后，程序获取变换的平移与旋转部分，并调用 getEulerYPR 将四元数转换为欧拉角进行输出。

        保存代码后，在 CMakeLists.txt 文件中注册 tf_listener 节点，重新编译并运行，执行命令及其结果如图所示：

4 参考资料

《ROS2机器人开发：从入门到实践20》(桑欣)

---

        最近课程上的事很多，终于有空闲时间自己学习记录了，下次将继续学习ros2中的Rviz工具。

        不断学习ing！欢迎大家点赞、收藏、评论，一起交流学习！！