ros2TF坐标系静态广播器实现案例

💡 本文的源码精心编撰而成，其灵感来源于赵虚左老师出色的教学内容，包括他的视频课程和详细文档。他的课程内容丰富，讲解清晰明了，注重实践操作，能将复杂的技术知识讲解得通俗易懂。 本文呈现的源码紧密结合了赵老师课程中的核心概念和技巧，并通过实际案例，方便读者亲自动手实践，加深对知识的理解，实现从理论到实践的转化。 所以，对于想要在编程领域深入学习、提高自身技能的读者来说，本文的源码是一份非常有价值的资源。它能帮助您跟随赵虚左老师的指导，感受编程的乐趣，在实践中不断进步。

👉 可以去 b 站搜索<猛狮训练营>

先创建c++功能包

ros2 pkg create cpp03_tf_broadcaster --build-type ament_cmake --dependencies rclcpp tf2 tf2_ros geometry_msgs turtlesim

实现静态广播器的代码 

#include <geometry_msgs/msg/transform_stamped.hpp>  // 用于处理坐标变换消息
#include <rclcpp/rclcpp.hpp>  // ROS 2 的 C++客户端库
#include <tf2/LinearMath/Quaternion.h>  // 用于处理四元数
#include <tf2_ros/static_transform_broadcaster.h>  // 用于发布静态坐标变换

using std::placeholders::_1;  // 用于函数绑定

// 自定义节点类，继承自 rclcpp::Node
class TFStaticBroadcaster : public rclcpp::Node
{
public:
    // 构造函数，接收终端传入的参数数组
    explicit TFStaticBroadcaster(char *argv[]) : Node("tf_statuc_broadcaster_node_cpp")
    {
        // 创建一个共享指针指向 tf2_ros 的静态坐标变换广播器，并与当前节点关联
        broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);
        // 调用私有函数发布静态坐标变换
        pub_static_tf(argv);
        // 在日志中输出信息，表示节点创建成功
        RCLCPP_INFO(this->get_logger(), "TFStaticBroadcaster 创建！");
    }

private:
    // 存储静态坐标变换广播器的共享指针
    std::shared_ptr<tf2_ros::StaticTransformBroadcaster> broadcaster_;

    // 私有函数，用于组织和发布静态坐标变换消息
    void pub_static_tf(char *argv[])
    {
        geometry_msgs::msg::TransformStamped transform;  // 创建一个坐标变换消息对象

        // 获取当前节点的时钟，并获取当前时间
        rclcpp::Time now = this->get_clock()->now();
        // 设置消息的时间戳为当前时间
        transform.header.stamp = this->now();
        // 设置消息的父坐标系 ID
        transform.header.frame_id = argv[7];
        // 设置消息的子坐标系 ID
        transform.child_frame_id = argv[8];

        // 设置子坐标系相对于父坐标系的平移分量
        transform.transform.translation.x = atof(argv[1]);
        transform.transform.translation.y = atof(argv[2]);
        transform.transform.translation.z = atof(argv[3]);

        tf2::Quaternion qtn;  // 创建一个四元数对象
        // 根据传入的参数设置四元数的旋转分量（以弧度为单位）
        qtn.setRPY(atof(argv[4]), atof(argv[5]), atof(argv[6]));

        // 设置四元数的各个分量到坐标变换消息的旋转部分
        transform.transform.rotation.x = qtn.x();
        transform.transform.rotation.y = qtn.y();
        transform.transform.rotation.z = qtn.z();
        transform.transform.rotation.w = qtn.w();

        // 通过广播器发送坐标变换消息
        broadcaster_->sendTransform(transform);
    }
};

// 主函数
int main(int argc, char *argv[])
{
    // 获取日志记录器
    auto logger = rclcpp::get_logger("logger");
    // 2. 判断终端传入的参数是否合法；如果参数数量不是 9 个，输出提示信息并返回
    if (argc!= 9)
    {
        RCLCPP_INFO(
            logger, "运行程序时请按照：x y z roll pitch yaw frame_id child child_frame_id 的格式传入参数");
        return 1;
    }

    // 3. 初始化 ROS 2 客户端
    rclcpp::init(argc, argv);
    // 4. 调用 spin 函数，并传入自定义节点对象的共享指针
    rclcpp::spin(std::make_shared<TFStaticBroadcaster>(argv));
    // 6. 释放资源
    rclcpp::shutdown();
    return 0;
}

CMakeLists.txt 配置如下：

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(turtlesim REQUIRED)
add_executable(demo01_static_tf_broadcaster src/demo01_static_tf_broadcaster.cpp)
ament_target_dependencies(
  demo01_static_tf_broadcaster
  "rclcpp"
  "tf2"
  "tf2_ros"
  "geometry_msgs"
  "turtlesim"
)
install(TARGETS demo01_static_tf_broadcaster
  DESTINATION lib/${PROJECT_NAME})

 

编译静态广播器代码后，在当前工作空间下，启动两个终端输入以下命令可以在rviz2中查看雷达和摄像头的坐标关系，更改命令中0.4这些数字可以改变雷达摄像头的位置

ros2 run cpp03_tf_broadcaster demo01_static_tf_broadcaster 0.4 0.0 0.2 0 0 1.87 base_link laser

ros2 run cpp03_tf_broadcaster demo01_static_tf_broadcaster 0.5 0.1 0.6 0 0 1.1 base_link camera