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 broadcaster_;

public:
    StaticTFBroadcaster() : Node("static_tf_broadcaster")
    {
        // 初始化静态 TF 广播器
        broadcaster_ = std::make_shared(this);
        // 发布静态 TF 变换
        publish_static_tf();
    }

    void publish_static_tf()
    {
        // 创建 TransformStamped 消息
        geometry_msgs::msg::TransformStamped transform;

        // 设置 header
        transform.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 2
    rclcpp::init(argc, argv);
    // 创建节点并保持运行
    auto node = std::make_shared();
    rclcpp::spin(node);
    // 关闭 ROS 2
    rclcpp::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"  // 使用 TransformBroadcaster

class DynamicTFBroadcaster : public rclcpp::Node
{
public:
    DynamicTFBroadcaster() : Node("dynamic_tf_broadcaster")
    {
        // 初始化动态 TF 广播器
        broadcaster_ = std::make_shared(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;

        // 设置 header
        transform.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 broadcaster_;
    rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char **argv)
{
    // 初始化 ROS 2
    rclcpp::init(argc, argv);
    // 创建节点并保持运行
    auto node = std::make_shared();
    rclcpp::spin(node);
    // 关闭 ROS 2
    rclcpp::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 保持节点运行。