25/1/22 算法笔记＜ROS2＞ TF变换

TF（Transform） 是 ROS（Robot Operating System）中的一个核心功能，用于管理和发布坐标系之间的变换关系。TF 的主要作用是描述机器人系统中各个部分（如传感器、执行器、底盘等）之间的位置和姿态关系，从而实现数据的统一和模块化。

静态 TF（Static Transform） 是 ROS（Robot Operating System）中用于描述两个坐标系之间固定不变的变换关系的一种机制。静态 TF 适用于那些在机器人系统中不会随时间变化的坐标系关系，例如传感器相对于机器人底盘的位置和姿态。

以下是使用 C++ 编写的发布静态 TF 变换的完整代码。代码实现了一个 ROS 2 节点，用于发布静态 TF 变换，描述 camera_link 相对于 base_link 的位置和姿态。

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_ros/static_transform_broadcaster.h"class StaticTFBroadcaster : public rclcpp::Node
{
private:std::shared_ptr<tf2_ros::StaticTransformBroadcaster> broadcaster_;public:StaticTFBroadcaster() : Node("static_tf_broadcaster"){// 初始化静态 TF 广播器broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);// 发布静态 TF 变换publish_static_tf();}void publish_static_tf(){// 创建 TransformStamped 消息geometry_msgs::msg::TransformStamped transform;// 设置 headertransform.header.stamp = this->get_clock()->now();transform.header.frame_id = "base_link";  // 父坐标系transform.child_frame_id = "camera_link"; // 子坐标系// 设置平移transform.transform.translation.x = 0.5;transform.transform.translation.y = 0.3;transform.transform.translation.z = 0.6;// 设置旋转（欧拉角转换为四元数）tf2::Quaternion q;q.setRPY(M_PI, 0.0, 0.0); // roll=180°, pitch=0°, yaw=0°transform.transform.rotation.x = q.x();transform.transform.rotation.y = q.y();transform.transform.rotation.z = q.z();transform.transform.rotation.w = q.w();// 发布静态 TF 变换broadcaster_->sendTransform(transform);RCLCPP_INFO(this->get_logger(), "发布静态 TF: camera_link 相对于 base_link");}
};int main(int argc, char **argv)
{// 初始化 ROS 2rclcpp::init(argc, argv);// 创建节点并保持运行auto node = std::make_shared<StaticTFBroadcaster>();rclcpp::spin(node);// 关闭 ROS 2rclcpp::shutdown();return 0;
}

1. 静态 TF 的实现步骤

1. 创建节点

• 创建一个 ROS 2 节点，用于发布静态 TF 变换。

2. 初始化静态 TF 广播器

• 使用 tf2_ros::StaticTransformBroadcaster 初始化静态 TF 广播器。

3. 设置 TF 变换

• 创建 geometry_msgs::msg::TransformStamped 消息，设置 header、平移和旋转。

4. 发布静态 TF 变换

• 调用 sendTransform 发布静态 TF 变换。

• 静态 TF 只需要发布一次，不需要定期更新。

5. 保持节点运行

• 使用 rclcpp::spin 保持节点运行。

动态 TF（Dynamic Transform） 是 ROS 2 中用于描述两个坐标系之间随时间变化的变换关系的一种机制。与静态 TF 不同，动态 TF 适用于那些在机器人系统中会随时间变化的坐标系关系，例如移动的机器人底盘、机械臂的末端执行器等。

动态 TF 的实现

在 ROS 2 中，动态 TF 通过 tf2_ros::TransformBroadcaster 实现。与静态 TF 不同，动态 TF 需要定期发布变换关系。

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_ros/transform_broadcaster.h"  // 使用 TransformBroadcasterclass DynamicTFBroadcaster : public rclcpp::Node
{
public:DynamicTFBroadcaster() : Node("dynamic_tf_broadcaster"){// 初始化动态 TF 广播器broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);// 创建定时器，每 0.1 秒调用一次 publish_tf 函数timer_ = this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&DynamicTFBroadcaster::publish_tf, this));}private:void publish_tf(){// 创建 TransformStamped 消息geometry_msgs::msg::TransformStamped transform;// 设置 headertransform.header.stamp = this->get_clock()->now();transform.header.frame_id = "base_link";  // 父坐标系transform.child_frame_id = "camera_link"; // 子坐标系// 设置平移transform.transform.translation.x = 0.5;transform.transform.translation.y = 0.3;transform.transform.translation.z = 0.6;// 设置旋转（欧拉角转换为四元数）tf2::Quaternion q;q.setRPY(M_PI, 0.0, 0.0); // roll=180°, pitch=0°, yaw=0°transform.transform.rotation.x = q.x();transform.transform.rotation.y = q.y();transform.transform.rotation.z = q.z();transform.transform.rotation.w = q.w();// 发布动态 TF 变换broadcaster_->sendTransform(transform);RCLCPP_INFO(this->get_logger(), "发布动态 TF: camera_link 相对于 base_link");}std::shared_ptr<tf2_ros::TransformBroadcaster> broadcaster_;rclcpp::TimerBase::SharedPtr timer_;
};int main(int argc, char **argv)
{// 初始化 ROS 2rclcpp::init(argc, argv);// 创建节点并保持运行auto node = std::make_shared<DynamicTFBroadcaster>();rclcpp::spin(node);// 关闭 ROS 2rclcpp::shutdown();return 0;
}

2. 动态 TF 的实现步骤

1. 创建节点

• 创建一个 ROS 2 节点，用于发布动态 TF 变换。

2. 初始化动态 TF 广播器

• 使用 tf2_ros::TransformBroadcaster 初始化动态 TF 广播器。

3. 设置 TF 变换

• 创建 geometry_msgs::msg::TransformStamped 消息，设置 header、平移和旋转。

4. 创建定时器

• 使用 create_wall_timer 创建一个定时器，定期调用发布函数。

5. 发布动态 TF 变换

• 在定时器回调函数中，更新 transform.header.stamp 为当前时间。

• 调用 sendTransform 发布动态 TF 变换。

6. 保持节点运行

• 使用 rclcpp::spin 保持节点运行。