ros2 从零开始29 添加帧变换C++

前言

背景

在之前的教程中，我们通过写一个tf2广播和tf2监听。 本教程教会你如何在变换树中添加额外的固定帧和动态帧。 事实上，在 tf2 中添加帧与创建 tf2 广播器非常相似，但这个示例将展示 tf2 的一些额外功能。

对于许多涉及变换相关的任务，在局部框架内思考更容易。 例如，在激光扫描仪中心的框架中推理激光扫描测量是最容易的。 TF2允许你为系统中的每个传感器、链路或关节定义本地帧。 在从一个帧转换到另一个帧时，tf2 会处理所有引入的隐藏中间帧变换。
本章学习如何添加额外帧。

TF2树

tf2 构建了一个树状结构的坐标系（帧），因此不允许在坐标系结构中出现闭环。这意味着一个坐标系只能有一个父坐标系，但可以有多个子坐标系。

• 在 ROS 的 tf2 系统中，坐标系之间的关系必须是一个有向无环图（DAG），即树状结构。
• world 通常是根坐标系，所有其他坐标系都是它的直接或间接子坐标系。
• 这种设计保证了坐标系变换的唯一性和可计算性。

‌典型结构示例‌
一个典型的移动机器人TF2树可能如下所示：

world (或 map)
└── odom (里程计坐标系)
    └── base_link (机器人底座/底盘)
        ├── laser (激光雷达)
        ├── camera (摄像头)
        └── imu (惯性测量单元)

目前，我们的 tf2 树包含三个坐标系：world、turtle1 和 turtle2。这两个乌龟坐标系是 world 坐标系的子坐标系。如果我们想向 tf2 添加一个新的坐标系，那么现有的三个坐标系中的一个需要成为父坐标系，而新的坐标系将成为它的子坐标系。
如何查看TF2树，请看之前文章《ros2 从零开始25 介绍tf2》

实践

1.创建一个包

本次不另外创建包，我们使用《编写静态广播C++》。我们创建一个新的帧carrot1，是turtle1的子节点，作为第二只乌龟的目标。

2.编写广播C++

在learning_tf2_cpp/src目录下，创建源文件fixed_frame_tf2_broadcaster.cpp , 其内容如下：

#include <chrono>
#include <functional>
#include <memory>

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_broadcaster.h"

using namespace std::chrono_literals;

class FixedFrameBroadcaster : public rclcpp::Node
{
public:
  FixedFrameBroadcaster()
  : Node("fixed_frame_tf2_broadcaster")
  {
    tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
    timer_ = this->create_wall_timer(
      100ms, std::bind(&FixedFrameBroadcaster::broadcast_timer_callback, this));
  }

private:
  void broadcast_timer_callback()
  {
    geometry_msgs::msg::TransformStamped t;

    t.header.stamp = this->get_clock()->now();
    t.header.frame_id = "turtle1";
    t.child_frame_id = "carrot1";
    t.transform.translation.x = 0.0;
    t.transform.translation.y = 2.0;
    t.transform.translation.z = 0.0;
    t.transform.rotation.x = 0.0;
    t.transform.rotation.y = 0.0;
    t.transform.rotation.z = 0.0;
    t.transform.rotation.w = 1.0;

    tf_broadcaster_->sendTransform(t);
  }

rclcpp::TimerBase::SharedPtr timer_;
  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<FixedFrameBroadcaster>());
  rclcpp::shutdown();
  return 0;
}

代码和tf2广非常相似，唯一的区别是这里的变换不会随时间变化。

2.1 代码解析

现在，关键代码如下，我们定义一个新的变换，从父turtle1变换到子carrot1的变换。 carrot1在turtle1坐标第中，纵轴y偏移2米。

    geometry_msgs::msg::TransformStamped t;

    t.header.stamp = this->get_clock()->now();
    t.header.frame_id = "turtle1";
    t.child_frame_id = "carrot1";
    t.transform.translation.x = 0.0;
    t.transform.translation.y = 2.0;
    t.transform.translation.z = 0.0;
    t.transform.rotation.x = 0.0;
    t.transform.rotation.y = 0.0;
    t.transform.rotation.z = 0.0;
    t.transform.rotation.w = 1.0;

2.3 更新CMakeLists.txt

在CMakeLists.txt添加配置可执行程序fixed_frame_tf2_broadcaster, 如下：

add_executable(fixed_frame_tf2_broadcaster src/fixed_frame_tf2_broadcaster.cpp)
ament_target_dependencies(
    fixed_frame_tf2_broadcaster
    geometry_msgs
    rclcpp
    tf2_ros
)

最后，添加安装配置install(TARGETS…)

install(TARGETS
    fixed_frame_tf2_broadcaster
    DESTINATION lib/${PROJECT_NAME})

3 启动文件

现在为此演示创建一个启动文件。在src/learning_tf2_cpp 目录下创建一个 launch 文件夹。使用文本编辑器，在 launch 文件夹中创建一个名为 turtle_tf2_fixed_frame_demo_launch 的新文件，扩展名为 .py、.xml 或 .yaml，随便选一个。并添加以下内容：

3.1.1 XML

<?xml version="1.0" encoding="UTF-8"?>
<launch>
  <include file="$(find-pkg-share learning_tf2_cpp)/launch/turtle_tf2_demo_launch.xml" />
  <node pkg="learning_tf2_cpp" exec="fixed_frame_tf2_broadcaster" name="fixed_broadcaster" />
</launch>

3.1.2 YAML

%YAML 1.2
---
launch:
  - include:
      file: "$(find-pkg-share learning_tf2_cpp)/launch/turtle_tf2_demo_launch.yaml"
  - node:
      pkg: "learning_tf2_cpp"
      exec: "fixed_frame_tf2_broadcaster"
      name: "fixed_broadcaster"

3.1.3 PYTHON

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.substitutions import PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():
    return LaunchDescription([
        IncludeLaunchDescription(
            PathJoinSubstitution([
                FindPackageShare('learning_tf2_cpp'), 'launch', 'turtle_tf2_demo.launch.py'])
        ),
        Node(
            package='learning_tf2_cpp',
            executable='fixed_frame_tf2_broadcaster',
            name='fixed_broadcaster',
        ),
    ])

注意到，turtle_tf2_fixed_frame_demo_launch.*文件里面使用turtle_tf2_demo_launch.*，例如xml里面

<include file="$(find-pkg-share learning_tf2_cpp)/launch/turtle_tf2_demo_launch.xml" />

在这种情况下，会先启动turtle_tf2_demo_launch.*定义的4个node节点， 再启动本节点fixed_broadcaster
一定要保证这里的文件turtle_tf2_demo_launch.*是上一节创建的。

4 编译和运行

进入工作区，用colcon build ，等待编译完成。

root@bc2bf85b2e4a:/# cd ~/ros2_ws
root@bc2bf85b2e4a:~/ros2_ws# colcon build --packages-select learning_tf2_cpp
Starting >>> learning_tf2_cpp
Finished <<< learning_tf2_cpp [5.52s]                     

Summary: 1 package finished [5.87s]

编译完成后，我们需要安装后才能运行，使用

source install/setup.sh

打开一个终端，输入如下命令运行(这里取turtle_tf2_demo_launch.xml,如果你用yaml或python，需要自己替换)

root@bc2bf85b2e4a:~/ros2_ws# source install/setup.sh 
root@bc2bf85b2e4a:~/ros2_ws# ros2 launch learning_tf2_cpp turtle_tf2_fixed_frame_demo_launch.xml

查看TF2树（命令ros2 run tf2_tools view_frames）

现在移动小乌龟turtle1，turtle2仍然紧紧跟随turtle1，跟上一章没有区别。这是因为目前我们还没有使用新的帧数据carrot1。现在我们把参数传过去，令小乌龟使用帧数据carrot1。
重新运行turtle_tf2_fixed_frame_demo_launch，传入参数target_frame:=carrot1

root@bc2bf85b2e4a:~/ros2_ws# ros2 launch learning_tf2_cpp turtle_tf2_fixed_frame_demo_launch.xml target_frame:=carrot1
[INFO] [launch]: All log files can be found below /root/.ros/log/2026-06-23-09-21-08-776721-bc2bf85b2e4a-25547
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [turtlesim_node-1]: process started with pid [25551]
[INFO] [turtle_tf2_broadcaster-2]: process started with pid [25553]
[INFO] [turtle_tf2_broadcaster-3]: process started with pid [25555]
[INFO] [turtle_tf2_listener-4]: process started with pid [25557]
[INFO] [fixed_frame_tf2_broadcaster-5]: process started with pid [25559]
[turtlesim_node-1] QStandardPaths: XDG_RUNTIME_DIR not set, defaulting to '/tmp/runtime-root'
[turtlesim_node-1] [INFO] [1782206468.950885987] [sim]: Starting turtlesim with node name /sim
[turtlesim_node-1] [INFO] [1782206468.956032375] [sim]: Spawning turtle [turtle1] at x=[5.544445], y=[5.544445], theta=[0.000000]
[turtlesim_node-1] [INFO] [1782206469.914678148] [sim]: Spawning turtle [turtle2] at x=[4.000000], y=[2.000000], theta=[0.000000]
[turtle_tf2_listener-4] [INFO] [1782206470.896408863] [listener]: Successfully spawned

启动时，2只乌龟就有间隔了。

跟随运动

总结

我们发布静态的位置数据carrot1， 这个位置相对于turtle1是静止的。
扩展练习：发布一个随时间变化的位置数据carrot1
把代码fixed_frame_tf2_broadcaster.cpp中的函数broadcast_timer_callback修改成随时间变化的值。如下：

  void broadcast_timer_callback()
  {
    const double PI = 3.141592653589793238463;
    rclcpp::Time now = this->get_clock()->now();
    double x = now.seconds() * PI;

    geometry_msgs::msg::TransformStamped t;
    
    t.header.stamp = now;
    t.header.frame_id = "turtle1";
    t.child_frame_id = "carrot1";
    t.transform.translation.x = 10 * sin(x);
    t.transform.translation.y = 10 * cos(x);
    t.transform.translation.z = 0.0;
    t.transform.rotation.x = 0.0;
    t.transform.rotation.y = 0.0;
    t.transform.rotation.z = 0.0;
    t.transform.rotation.w = 1.0;

    tf_broadcaster_->sendTransform(t);
  }

重新编译后运行，可以看看效果。