ROS2之TF介绍
坐标系是我们非常熟悉的一个概念，也是机器人学中的重要基础，在一个完整的机器人系统中，会存在很多坐标系，这些坐标系之间的位置关系该如何管理？
ROS给我们提供了一个坐标系的管理神器——TF。
一、机器人中的坐标系
你有没有玩过迷宫游戏？地图上会有一个“你在这儿”的小红点。如果没有这个红点，你就不知道自己在哪。
坐标系就像那个“你在这儿”的标记，它告诉你：
- 原点在哪儿：坐标为(
0,0,0）； - 前方是哪边（
x轴），右边是哪边（y轴），上方是哪边（z轴）；
有了坐标系，我们就可以给任何物体标记一个具体的位置，比如：
“小球在我前方 2 米，右边 1 米的位置”
这就可以写成坐标：(x=2,y=1, z=0)。
在机器人操作系统中，坐标系是描述空间位置和方向的基础语言，那么在机器人身上到底有多少个坐标系？
1.1 机械臂
在机械臂形态的机器人中，主要涉及四大坐标系；
- 机器人安装的位置叫做基坐标系
Base Frame； - 机器人安装位置在外部环境下的参考系叫做世界坐标系
World Frame； - 机器人末端夹爪的位置叫做工具坐标系；
- 外部被操作物体的位置叫做工件坐标系。
在机械臂抓取外部物体的过程中，这些坐标系之间的关系也在跟随变化，如图所示；
1.1.1 世界坐标系( World Frame)
World Frame是世界坐标系，这是我们人类设计的外部参考系。通常选择：
- 机器人工作台的某个固定角落作为原点(
0,0,0)； - 沿着工作台长边方向为
X轴正方向； - 沿着工作台宽边方向为
Y轴正方向； - 垂直于工作台向上为
Z轴正方向。
这是我们设计和规划机器人任务时使用的全局坐标系，所有其他坐标系最终都要映射到这个坐标系中来统一描述。
1.1.2 基坐标系(Base Frame)
Base Frame是机械臂的基准坐标系。可以这样理解：
- 机械臂安装到工作台上，我们就把机械臂底座的中心点定义为
Base Frame的原点； - 这个坐标系是相对于
World Frame固定的，一旦安装完成就不再变化； - 所有机械臂关节的运动都是相对于这个
Base Frame来描述的。
例如，一个六轴机械臂的第一个关节（底座旋转关节）的旋转就是绕着Base Frame的Z轴进行的。
1.1.3 工具坐标系(Tool Frame)
Tool Frame是机械臂末端执行器的坐标系。对于不同的任务，工具坐标系有不同的定义：
- 对于夹爪：通常把原点定义在两个夹爪的中间点；
- 对于焊枪：把原点定义在焊枪的尖端；
- 对于吸盘：把原点定义在吸盘的吸附面中心。
这个坐标系是随着机械臂末端一起运动的，它会根据末端的姿态和位置不断变化。工具坐标系的主要作用是：
- 精确控制工具的操作位置；
- 进行工具中心点(
TCP)的校准； - 实现工具与工件的精确对齐。
1.1.4 工件坐标系( Work Frame)
Work Frame是被操作物体所在的坐标系。例如：
- 要抓取的零件在传送带上的位置；
- 要焊接的工件在夹具中的位置；
- 要装配的部件在工作台上的位置。
这个坐标系的特点是：
- 它可能相对于
World Frame是固定的（如固定在夹具中的工件）； - 也可能相对于
World Frame是运动的（如传送带上的零件）； - 还可能相对于
World Frame是未知的（需要视觉系统来识别）。
在实际应用中，我们通常先通过视觉系统或其他传感器识别出Work Frame相对于World Frame的位姿，然后将Tool Frame变换到这个Work Frame中，从而精确地操作工件。
1.2 移动机器人
在移动机器人系统中，坐标系一样至关重要，比如：
- 一个移动机器人的中心点是基坐标系
Base Link； - 雷达所在的位置叫做雷达坐标系
Laser Link； - 机器人要移动，里程计会累积位置，这个位置的参考系叫做里程计坐标系
odom； - 里程计又会有累积误差和漂移，绝对位置的参考系叫做地图坐标系
map；
如图所示：
1.2.1 地图坐标系(map)
map是地图坐标系，假设我们给房间画一张地图，左下角是 (0,0)，正右是x轴，正上是y轴。这是我们人类眼中的世界坐标系。
1.2.2 里程计坐标系(odom)
odom是机器人自己的一套参考系统。可以这样理解：
- 机器人一启动，就把它站着的那个点当成了(
0,0)，然后它往前走1米，odom坐标就变成了 (1,0)。
但是，这个位置可能不是房间地图上的 (1,0)，因为机器人只靠轮子估算自己走了多远，而轮子会打滑、误差累积。
所以，odom是一种估算出来的位置，但会随着时间逐渐偏离真实位置。
你可以想象odom是机器人的小脑袋，它说：“我大概走了 3 米向前”，但不一定百分百准确。
1.2.3 机器人自身坐标系(base_link)
base_link是移动机器人自己身体的坐标系，一般我们把这个坐标系的原点放在机器人身体中间，比如底盘中心，或两个轮子之间。
这个点不是真的设在哪里，而是我们人为地定义一个“我是谁”的起点。
从此以后：
- 摄像头在
base_link前面0.3米，高度0.5米； - 激光雷达在
base_link前面0.2米； - 机械臂根部就在
base_link的正中间；
所有设备的位置都可以用base_link作为基准来描述。
1.3 坐标系变换
既然我们有那么做坐标系，那么一个点的位置，到底是谁说了算？比如：
- 摄像头说这个红球在
x=1米、y=0米的位置，但机器人能直接走过去吗？
不能。因为这个坐标是摄像头视角的。机器人自己并不认识这个坐标。那怎么办？
答案就是：我们要把这个坐标，从摄像头坐标系转换成机器人自己坐标系下的坐标，这样它才能理解红球在哪儿。
这种从一个坐标系转到另一个坐标系的过程，就叫做坐标变换。
关于坐标变换关系的基本理论，在每一本机器人学的教材中都会有讲解，可以分解为平移和旋转两个部分，通过一个四乘四的矩阵进行描述，在空间中画出坐标系，那两者之间的变换关系，其实就是向量的数学描述。
ROS中TF功能的底层原理，就是对这些数学变换进行了封装，详细的理论知识可以参考机器人学的教材，也可以看我之前写的文章《第六节、双目视觉之相机标定》。
在本篇博客我们主要讲解TF坐标管理系统的使用方法。
1.3.1 什么是TF
TF是ROS中的一个系统，全称叫transform（变换），它有两个重要功能：
- 记录各个坐标系之间的关系；
- 帮你把一个坐标从
A转换到B；
比如：
map → odom → base_link → camera_link
每个箭头之间代表坐标变换。比如：
- 从
map到odom：表示机器人在地图中的真实位置； - 从
odom到base_link：表示机器人认为自己在哪 - 从
base_link到camera_link：表示摄像头安装在哪；
这个结构像什么？像一棵树，每个坐标系都是一个节点，每条边是一种连接关系，所以我们叫它TF树。
1.3.2 案例
如果不用TF，意味着要自己手动计算所有坐标变换！
来看个例子：
- 摄像头看到红球在它正前方
1米处 (1.0,0.0)； - 摄像头安装在机器人前方
0.3米、高0.5米 - 机器人自己认为它在
odom中是 (2.0,1.0)，朝向是东；
如果想知道：这个红球在map中的真实位置是多少？如果没有TF，我们需要：
- 自己写数学公式：先从
camera_link转成base_link，再从base_link转成odom，再从odom转到map； - 考虑机器人是否旋转过；
- 考虑每个坐标的时间戳是否一致；
- 一堆复杂的矩阵变换、旋转计算……
有了TF呢，我们就可以使用TF帮我把camera_link中的这个点转换到map中，它会自动完成所有事情，并且考虑时间、方向、位置等。
二、TF案例
使用TF我们首先需要安装一些依赖库：
pi@NanoPC-T6:~/dev_ws$ sudo apt update
pi@NanoPC-T6:~/dev_ws$ sudo apt install ros-humble-tf2-tools
pi@NanoPC-T6:~/dev_ws$ ls $(ros2 pkg prefix tf2_tools)/lib/tf2_tools/
view_frames
pi@NanoPC-T6:~/dev_ws$ ls $(ros2 pkg prefix tf2_tools)/lib/tf2_ros/
buffer_server static_transform_publisher tf2_echo tf2_monitor
pi@NanoPC-T6:~/dev_ws$ sudo pip3 install transforms3d
其中：
ros-humble-tf2-tools：安装TF2相关的工具集，提供了一系列调试和可视化TF变换的工具；包含的主要工具有：view_frames：生成TF帧关系的PDF图；tf2_monitor：监控TF帧之间的变换关系；tf2_echo：查看两个特定帧之间的变换数据；
transforms3d：一个Python库，用于处理3D空间中的几何变换，提供的主要功能：- 旋转矩阵、欧拉角、四元数、轴角之间的转换；
3D变换矩阵的操作；- 坐标系变换计算。
接着创建my_learning_tf的Python版本的功能包；
pi@NanoPC-T6:~/dev_ws$ cd src
pi@NanoPC-T6:~/dev_ws/src$ ros2 pkg create --build-type ament_python my_learning_tf
2.1 TF命令行操作(海龟案例)
ROS中的TF该如何使用呢？我们先通过两只海龟的示例，了解下基于坐标系的一种机器人跟随算法。
我们首先需要安装ROS 2 Humble版本的Turtle TF2示例程序；
pi@NanoPC-T6:~/dev_ws$ sudo apt install ros-humble-turtle-tf2-py
这个包包含一个经典的"海龟"示例，会创建两个海龟：
-
一只可以通过键盘控制移动的海龟；
-
另一只会自动跟随第一只的海龟；
-
主要用于学习
TF2（变换库）的基本概念，包括：- 坐标变换；
- 帧（
frames）的发布和订阅； - 位姿变换计算。
2.1.1 运行launch
启动turtle-tf2示例：
pi@NanoPC-T6:~/dev_ws$ ros2 launch turtle_tf2_py turtle_tf2_demo.launch.py
[INFO] [launch]: All log files can be found below /home/pi/.ros/log/2025-12-21-00-35-22-173055-NanoPC-T6-7502
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [turtlesim_node-1]: process started with pid [7503]
[INFO] [turtle_tf2_broadcaster-2]: process started with pid [7505]
[INFO] [turtle_tf2_broadcaster-3]: process started with pid [7507]
[INFO] [turtle_tf2_listener-4]: process started with pid [7509]
[turtlesim_node-1] Warning: Ignoring XDG_SESSION_TYPE=wayland on Gnome. Use QT_QPA_PLATFORM=wayland to run on Wayland anyway.
[turtlesim_node-1] [INFO] [1766277322.890167955] [sim]: Starting turtlesim with node name /sim
[turtlesim_node-1] [INFO] [1766277322.899916544] [sim]: Spawning turtle [turtle1] at x=[5.544445], y=[5.544445], theta=[0.000000]
[turtlesim_node-1] [INFO] [1766277323.918337095] [sim]: Spawning turtle [turtle2] at x=[4.000000], y=[2.000000], theta=[0.000000]
[turtle_tf2_listener-4] [INFO] [1766277324.967066696] [listener]: Successfully spawned turtle2xxxxxxxxxx [INFO] [launch]: All log files can be found below /home/pi/.ros/log/2025-12-21-00-35-22-173055-NanoPC-T6-7502[INFO] [launch]: Default logging verbosity is set to INFO[INFO] [turtlesim_node-1]: process started with pid [7503][INFO] [turtle_tf2_broadcaster-2]: process started with pid [7505][INFO] [turtle_tf2_broadcaster-3]: process started with pid [7507][INFO] [turtle_tf2_listener-4]: process started with pid [7509][turtlesim_node-1] Warning: Ignoring XDG_SESSION_TYPE=wayland on Gnome. Use QT_QPA_PLATFORM=wayland to run on Wayland anyway.[turtlesim_node-1] [INFO] [1766277322.890167955] [sim]: Starting turtlesim with node name /sim[turtlesim_node-1] [INFO] [1766277322.899916544] [sim]: Spawning turtle [turtle1] at x=[5.544445], y=[5.544445], theta=[0.000000][turtlesim_node-1] [INFO] [1766277323.918337095] [sim]: Spawning turtle [turtle2] at x=[4.000000], y=[2.000000], theta=[0.000000][turtle_tf2_listener-4] [INFO] [1766277324.967066696] [listener]: Successfully spawned turtle2pi@NanoPC-T6:~/dev_ws$ ros2 launch turtle_tf2_py turtle_tf2_demo.launch.py
从输出消息中可知：运行该演示会启动4个节点：
1个turtlesim_node节点：节点名字为/sim；- 2个
turtle_tf2_broadcaster节点：节点名字分别为/broadcaster1、/broadcaster2，这两个广播者节点会发布这两只海龟的坐标系； 1个turtle_tf2_listener：节点名字为/listener，这个侦听者节点会计算两只海龟坐标系之间的差异，并让一只海龟（turtle2）跟随另一只海龟（turtle1）移动；
运行完成后，我们可以看到turtlesim_node节点生成2只海龟（turtle1和turtle2），然后turtle2会沿着一条弧线移动靠近turtle1；
然后我们可以控制其中的一只海龟，另外一只海龟会自动跟随运动；
pi@NanoPC-T6:~/dev_ws$ ros2 run turtlesim turtle_teleop_key
然后就可以利用键盘来控制turtle1的旋转和移动，而turtle2也会跟随turtle1进行旋转和移动。
2.1.2 查看TF树
软件包tf2_tools中有一个view_frames工具（可执行文件），可以用来创建通过ROS2系统的tf2正在广播的坐标变换树的图形（diagram）。在一个新的终端中运行以下命令：
pi@NanoPC-T6:~/dev_ws$ ros2 run tf2_tools view_frames
[INFO] [1766277680.566351638] [view_frames]: Listening to tf data for 5.0 seconds...
[INFO] [1766277685.582050455] [view_frames]: Generating graph in frames.pdf file...
[INFO] [1766277685.593336251] [view_frames]: Result:tf2_msgs.srv.FrameGraph_Response(frame_yaml="turtle2: \n parent: 'world'\n broadcaster: 'default_authority'\n rate: 62.697\n most_recent_transform: 1766277685.575447\n oldest_transform: 1766277680.535321\n buffer_length: 5.040\nturtle1: \n parent: 'world'\n broadcaster: 'default_authority'\n rate: 62.701\n most_recent_transform: 1766277685.575661\n oldest_transform: 1766277680.535844\n buffer_length: 5.040\n")
pi@NanoPC-T6:~/dev_ws$ ls
build install turtlesim.yaml
frames_2025-12-21_00.41.25.gv log
frames_2025-12-21_00.41.25.pdf src
view_frames会订阅5s内的数据，tf2侦听者节点正在侦听通过ROS2系统广播的坐标系，并绘制这些坐标系相互连接形成的坐标变换树。
我们打开frames_2025-12-21_00.41.25.pdf文件，就可以看到如下图所示的坐标变换树图形：
从图中可以看到当前系统一共有三个坐标系：
world：世界坐标系，它的原点(0,0)在海龟运行的地图的左下角，向右为x轴，向前为y轴；turtle1：海龟1自身坐标系；turtle2：海龟2自身坐标系；
其中world坐标系是turtle1坐标系和turtle2坐标系的父级坐标系。view_frames还报告了一些有关何时接收到最早和最近的坐标系变换以及为调试目的将tf2坐标系发布到tf2的频率等信息。
2.1.3 查询坐标变换信息
只看到坐标系的结构还不行，如果我们想要知道某两个坐标系之间的具体关系，可以通过tf2_echo这个工具查看。运行该工具的命令语法如下：
ros2 run tf2_tools tf2_ros <source_frame> <target_frame>
例如，来看一下从turtle2到turtle1的坐标变换，请运行以下命令：
i@NanoPC-T6:~/dev_ws$ ros2 run tf2_ros tf2_echo turtle2 turtle1
[INFO] [1766280764.210970045] [tf2_echo]: Waiting for transform turtle2 -> turtle1: Invalid frame ID "turtle2" passed to canTransform argument target_frame - frame does not exist
At time 1766280765.158573876
- Translation: [0.000, 0.000, 0.000]
- Rotation: in Quaternion (xyzw) [0.000, 0.000, 0.291, 0.957]
- Rotation: in RPY (radian) [0.000, -0.000, 0.590]
- Rotation: in RPY (degree) [0.000, -0.000, 33.831]
- Matrix:
0.831 -0.557 0.000 0.000
0.557 0.831 0.000 0.000
0.000 0.000 1.000 0.000
0.000 0.000 0.000 1.000
At time 1766280766.150517038
- Translation: [0.000, 0.000, 0.000]
- Rotation: in Quaternion (xyzw) [0.000, 0.000, 0.291, 0.957]
- Rotation: in RPY (radian) [0.000, -0.000, 0.590]
- Rotation: in RPY (degree) [0.000, -0.000, 33.831]
- Matrix:
0.831 -0.557 0.000 0.000
0.557 0.831 0.000 0.000
0.000 0.000 1.000 0.000
0.000 0.000 0.000 1.000
At time 1766280767.158621948
......
可以看到当前两个坐标系的变换由两部分组成，平移变换和旋转变换，由于此时两个海龟已经重叠了，所以平移变换等于0，而旋转变换有个较小弧度的旋转（围绕z轴的）。
在旋转变换里面三种描述方式，分别是：
- 四元数；
- 欧拉角；
- 轴角之间的转换；
如果我们同时在运行turtle_teleop_key节点的终端中旋转和移动turtle1，这样就会在这个终端中输出如下所示的消息：
At time 1766280771.159109321
- Translation: [1.648, -0.010, 0.000]
- Rotation: in Quaternion (xyzw) [0.000, 0.000, 0.210, 0.978]
- Rotation: in RPY (radian) [0.000, -0.000, 0.423]
- Rotation: in RPY (degree) [0.000, -0.000, 24.227]
- Matrix:
0.912 -0.410 0.000 1.648
0.410 0.912 0.000 -0.010
0.000 0.000 1.000 0.000
0.000 0.000 0.000 1.000
At time 1766280772.151677830
- Translation: [0.830, 0.003, 0.000]
- Rotation: in Quaternion (xyzw) [0.000, 0.000, 0.850, 0.526]
- Rotation: in RPY (radian) [0.000, -0.000, 2.033]
- Rotation: in RPY (degree) [0.000, -0.000, 116.475]
- Matrix:
-0.446 -0.895 0.000 0.830
0.895 -0.446 0.000 0.003
0.000 0.000 1.000 0.000
0.000 0.000 0.000 1.000
从上述输出消息中可以看出，当通过键盘驱动海龟turtle1四处移动时，可以看到随着两只海龟相对移动而发生的坐标变换的变化情况。
2.1.4 坐标系可视化
rviz2是一种可视化工具，可用于检查tf2坐标系，现在可以使用rviz2来看看这两只海龟的坐标系；
i@NanoPC-T6:~/dev_ws$ ros2 run rviz2 rviz2 -d $(ros2 pkg prefix --share turtle_tf2_py)/rviz/turtle_rviz.rviz
再让海龟动起来，Rviz中的坐标轴就会开始运动，这样是不是更加直观了呢！
海龟跟随的案例有点意思，这背后的原理是怎样的呢？大家不要着急，我们先来了解下TF的使用方法，便于大家慢慢理解。
2.2 静态TF广播
我们说TF的主要作用是对坐标系进行管理，那就管理一个试试呗？
坐标变换中最为简单的应该是相对位置不发生变化的情况，比如你家的房子在哪个位置，只要房子不拆，这个坐标应该就不会变化。
在机器人系统中也很常见，比如激光雷达和机器人底盘之间的位置关系，安装好之后基本不会变化。
在TF中，这种情况也称之为静态TF变换，我们来看看在程序中该如何实现？
2.2.1 代码实现
打开my_learning_tf功能包，在my_learning_tf目录下创建static_tf_broadcaster.py；
"""
ROS2 TF示例-广播静态的坐标变换
@author: zy
@since : 2025/12/21
"""
import rclpy # ROS2 Python接口库
from rclpy.node import Node # ROS2 节点类
from geometry_msgs.msg import TransformStamped # 坐标变换消息
import tf_transformations # TF坐标变换库
from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster # TF静态坐标系广播器类
class StaticTFBroadcaster(Node):
def __init__(self, name):
super().__init__(name) # ROS2节点父类初始化
self.tf_broadcaster = StaticTransformBroadcaster(self) # 创建一个TF广播器对象
static_transformStamped = TransformStamped() # 创建一个坐标变换的消息对象
static_transformStamped.header.stamp = self.get_clock().now().to_msg() # 设置坐标变换消息的时间戳
static_transformStamped.header.frame_id = 'world' # 设置一个坐标变换的源坐标系
static_transformStamped.child_frame_id = 'house' # 设置一个坐标变换的目标坐标系
static_transformStamped.transform.translation.x = 10.0 # 设置坐标变换中的X、Y、Z向的平移
static_transformStamped.transform.translation.y = 5.0
static_transformStamped.transform.translation.z = 0.0
quat = tf_transformations.quaternion_from_euler(0.0, 0.0, 0.0) # 将欧拉角转换为四元数（roll, pitch, yaw）
static_transformStamped.transform.rotation.x = quat[0] # 设置坐标变换中的X、Y、Z向的旋转（四元数）
static_transformStamped.transform.rotation.y = quat[1]
static_transformStamped.transform.rotation.z = quat[2]
static_transformStamped.transform.rotation.w = quat[3]
self.tf_broadcaster.sendTransform(static_transformStamped) # 广播静态坐标变换，广播后两个坐标系的位置关系保持不变
def main(args=None):
rclpy.init(args=args) # ROS2 Python接口初始化
node = StaticTFBroadcaster("static_tf_broadcaster") # 创建ROS2节点对象并进行初始化
rclpy.spin(node) # 循环等待ROS2退出
node.destroy_node() # 销毁节点对象
rclpy.shutdown()
这段代码创建了一个静态坐标系变换，将world坐标系和house坐标系之间的关系固定下来并广播到ROS2系统中，两个坐标系关系：
world (父坐标系)
↓ 平移(10, 5, 0)，无旋转
house (子坐标系)
完成代码的编写后需要设置功能包的编译选项，让系统知道Python程序的入口，打开功能包的setup.py文件，加入如下入口点的配置：
entry_points={
'console_scripts': [
'static_tf_broadcaster = my_learning_tf.static_tf_broadcaster:main',
],
},
2.2.2 编译运行
编译程序：
pi@NanoPC-T6:~/dev_ws$ colcon build --paths src/my_learning_tf
pi@NanoPC-T6:~/dev_ws$ source install/setup.sh
启动终端，运行如下命令：
pi@NanoPC-T6:~/dev_ws$ ros2 run my_learning_tf static_tf_broadcaster
2.2.2.1 查看TF树
接着我们使用view_frames 查看TF树；
pi@NanoPC-T6:~/dev_ws$ ros2 run tf2_tools view_frames
可以看到当前系统中存在两个坐标系，一个是world，一个是house，两者之间的相对位置不会发生改变，通过一个静态的TF对象进行维护。
2.2.2.2 查看坐标系变换信息
来看一下从world到house的坐标变换，请运行以下命令：
i@NanoPC-T6:~/dev_ws$ ros2 run tf2_ros tf2_echo world house
[INFO] [1766283504.651139360] [tf2_echo]: Waiting for transform world -> house: Invalid frame ID "world" passed to canTransform argument target_frame - frame does not exist
At time 0.0
- Translation: [10.000, 5.000, 0.000]
- Rotation: in Quaternion (xyzw) [0.000, 0.000, 0.000, 1.000]
- Rotation: in RPY (radian) [0.000, -0.000, 0.000]
- Rotation: in RPY (degree) [0.000, -0.000, 0.000]
- Matrix:
1.000 0.000 0.000 10.000
0.000 1.000 0.000 5.000
0.000 0.000 1.000 0.000
0.000 0.000 0.000 1.000
从world到house的变换矩阵\(P\)：
\[\begin{pmatrix} 1 & 0 & 0 & 10 \\ 0 & 1 & 0 & 5 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{pmatrix}
\]
旋转部分\(R\)：
\[\begin{pmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{pmatrix}
\]
平移部分\(T\)：
\[\begin{pmatrix} 10 \\ 5 \\ 0 \end{pmatrix}
\]
因此可以得到坐标变换公式：
\[P_h = RP_w + T
\]
也就是在在house坐标系中的点(0,0,0)，在world坐标系中就是(10,5,0)。
这里我们稍微提一下使用欧拉角来表示旋转部分；
roll（横滚角）：绕X轴旋转0；pitch（俯仰角）：绕Y轴旋转0；yaw（偏航角）：绕Z轴旋转0；
相信做过四轴飞行器开发的小伙伴对欧拉角应该再熟悉不过了。
2.2.3 流程分析
总结一下，想要实现一个静态TF广播，代码的实现流程是这样做；
开始
↓
rclpy.init() # 初始化ROS2
↓
创建StaticTFBroadcaster节点
↓
节点初始化:
- 创建StaticTransformBroadcaster
- 设置world到house的坐标变换
- 位置: (10, 5, 0)
- 姿态: 无旋转
- 广播该静态变换
↓
rclpy.spin() # 进入事件循环
↓
节点持续运行，静态变换保持在系统中
↓
按Ctrl+C触发退出
↓
node.destroy_node() # 清理节点
↓
rclpy.shutdown() # 关闭ROS2
2.3 TF监听
我们再来学习下如何查询两个坐标系之间的位置关系。
2.3.1 代码实现
在my_learning_tf目录下创建tf_listener.py；
"""
ROS2 TF示例-监听某两个坐标系之间的变换
@author: zy
@since : 2025/12/21
"""
import rclpy # ROS2 Python接口库
from rclpy.node import Node # ROS2 节点类
import tf_transformations # TF坐标变换库
from tf2_ros import TransformException # TF左边变换的异常类
from tf2_ros.buffer import Buffer # 存储坐标变换信息的缓冲类
from tf2_ros.transform_listener import TransformListener # 监听坐标变换的监听器类
class TFListener(Node):
def __init__(self, name):
super().__init__(name) # ROS2节点父类初始化
self.declare_parameter('source_frame', 'world') # 创建一个源坐标系名的参数
self.source_frame = self.get_parameter( # 优先使用外部设置的参数值，否则用默认值
'source_frame').get_parameter_value().string_value
self.declare_parameter('target_frame', 'house') # 创建一个目标坐标系名的参数
self.target_frame = self.get_parameter( # 优先使用外部设置的参数值，否则用默认值
'target_frame').get_parameter_value().string_value
self.tf_buffer = Buffer() # 创建保存坐标变换信息的缓冲区
self.tf_listener = TransformListener(self.tf_buffer, self) # 创建坐标变换的监听器
self.timer = self.create_timer(1.0, self.on_timer) # 创建一个固定周期的定时器，处理坐标信息
def on_timer(self):
try:
now = rclpy.time.Time() # 获取ROS系统的当前时间
trans = self.tf_buffer.lookup_transform( # 监听当前时刻源坐标系到目标坐标系的坐标变换
self.target_frame,
self.source_frame,
now)
except TransformException as ex: # 如果坐标变换获取失败，进入异常报告
self.get_logger().info(
f'Could not transform {self.target_frame} to {self.source_frame}: {ex}')
return
pos = trans.transform.translation # 获取位置信息
quat = trans.transform.rotation # 获取姿态信息（四元数）
euler = tf_transformations.euler_from_quaternion([quat.x, quat.y, quat.z, quat.w])
self.get_logger().info('Get %s --> %s transform: [%f, %f, %f] [%f, %f, %f]'
% (self.source_frame, self.target_frame, pos.x, pos.y, pos.z, euler[0], euler[1], euler[2]))
def main(args=None):
rclpy.init(args=args) # ROS2 Python接口初始化
node = TFListener("tf_listener") # 创建ROS2节点对象并进行初始化
rclpy.spin(node) # 循环等待ROS2退出
node.destroy_node() # 销毁节点对象
rclpy.shutdown() # 关闭ROS2 Python接口
这段代码监听两个坐标系之间的变换关系，定期获取变换信息（位置和姿态）并打印出来。
完成代码的编写后需要设置功能包的编译选项，让系统知道Python程序的入口，打开功能包的setup.py文件，加入如下入口点的配置：
entry_points={
'console_scripts': [
'static_tf_broadcaster = my_learning_tf.static_tf_broadcaster:main',
'tf_listener = my_learning_tf.tf_listener:main',
],
},
2.3.2 编译运行
编译程序：
pi@NanoPC-T6:~/dev_ws$ colcon build --paths src/my_learning_tf
pi@NanoPC-T6:~/dev_ws$ source install/setup.sh
启动终端，运行如下节点，就可以在终端中看到周期显示的坐标关系了；
pi@NanoPC-T6:~/dev_ws$ ros2 run my_learning_tf tf_listener
[INFO] [1766285087.274053360] [tf_listener]: Get world --> house transform: [-10.000000, -5.000000, 0.000000] [0.000000, -0.000000, 0.000000]
[INFO] [1766285088.240051202] [tf_listener]: Get world --> house transform: [-10.000000, -5.000000, 0.000000] [0.000000, -0.000000, 0.000000]
[INFO] [1766285089.240043078] [tf_listener]: Get world --> house transform: [-10.000000, -5.000000, 0.000000] [0.000000, -0.000000, 0.000000]
[INFO] [1766285090.240062078] [tf_listener]: Get world --> house transform: [-10.000000, -5.000000, 0.000000] [0.000000, -0.000000, 0.000000]
2.3.3 流程分析
总结一下，想要实现一个TF监听，代码的实现流程是这样做；
开始执行
↓
rclpy.init() # 初始化ROS2环境
↓
TFListener节点初始化：
1. 声明参数(source_frame, target_frame)
2. 创建TF缓冲区(Buffer)
3. 创建TF监听器(TransformListener)
4. 创建1秒定时器
↓
TransformListener自动：
1. 订阅/tf话题（动态变换）
2. 订阅/tf_static话题（静态变换）
3. 接收到的变换存入Buffer
↓
进入rclpy.spin()事件循环：
┌─────────────────────────────────────┐
│ 定时器每秒触发 → 执行on_timer() │
│ 1. 获取当前时间 │
│ 2. 查询坐标变换 │
│ 3. 提取位置和姿态 │
│ 4. 四元数转欧拉角 │
│ 5. 打印结果 │
└─────────────────────────────────────┘
↓
Ctrl+C触发退出
↓
销毁节点，关闭ROS2
2.3.4 数据流向
数据流线图如下：
外部TF广播节点
↓ 发布
/tf 或 /tf_static 话题
↓
TransformListener (自动订阅)
↓ 填充
Buffer (存储历史变换)
↓ 查询
on_timer()中调用lookup_transform()
↓ 返回
TransformStamped消息
↓ 提取
位置(translation) + 姿态(rotation)
↓ 转换
四元数 → 欧拉角
↓ 输出
打印到终端
2.4 海龟跟随功能实现
在前面我们介绍了海龟跟随案例，那它是如何实现的呢？
在两只海龟的仿真器中，我们定义三个坐标系，比如：
- 仿真器的全局参考系叫做
world； turtle1和turtle2坐标系在两只海龟的中心点；
这样，turtle1和world坐标系的相对位置，就可以表示海龟1的位置，海龟2也同理。
那如何实现海龟2向海龟1运动，我们在两者中间做一个连线，再加一个箭头，怎么样，是不是有想起高中时学习的向量计算？
我们说坐标变换的描述方法就是向量，所以在这个跟随例程中，用TF就可以很好的解决；
- 向量的长度表示距离；
- 方向表示角度；
有了距离和角度，我们随便设置一个时间，不就可以计算得到速度了么，然后就是速度话题的封装和发布，海龟2也就可以动起来了。
所以这个例程的核心就是通过坐标系实现向量的计算，两只海龟还会不断运动，这个向量也得按照某一个周期计算，这就得用上TF的动态广播与监听了。
接下来我们自己通过代码来实现海龟跟随。
2.4.1 坐标系动态广播
我们首先需要订阅海龟的位置消息(定义的话题是/${turtlename}/pose)，然后将海龟的位置和姿态实时广播为TF坐标变换，建立world坐标系到${turtlename}坐标系的动态关系。
海龟1和海龟2在world坐标系下的坐标变换，在turtle_tf_broadcaster节点中实现，除了海龟坐标系的名字不同之外，针对两个海龟的功能是一样的。
在my_learning_tf目录下创建turtle_tf_broadcaster.py；
"""
ROS2 TF示例-广播动态的坐标变换
@author: zy
@since : 2025/12/21
"""
import rclpy # ROS2 Python接口库
from rclpy.node import Node # ROS2 节点类
from geometry_msgs.msg import TransformStamped # 坐标变换消息
import tf_transformations # TF坐标变换库
from tf2_ros import TransformBroadcaster # TF坐标变换广播器
from turtlesim.msg import Pose # turtlesim海龟位置消息
class TurtleTFBroadcaster(Node):
def __init__(self, name):
super().__init__(name) # ROS2节点父类初始化
self.declare_parameter('turtlename', 'turtle') # 创建一个海龟名称的参数
self.turtlename = self.get_parameter( # 优先使用外部设置的参数值，否则用默认值
'turtlename').get_parameter_value().string_value
self.tf_broadcaster = TransformBroadcaster(self) # 创建一个TF坐标变换的广播对象并初始化
self.subscription = self.create_subscription( # 创建一个订阅者，订阅海龟的位置消息
Pose,
f'/{self.turtlename}/pose', # 使用参数中获取到的海龟名称
self.turtle_pose_callback, 1)
def turtle_pose_callback(self, msg): # 创建一个处理海龟位置消息的回调函数，将位置消息转变成坐标变换
transform = TransformStamped() # 创建一个坐标变换的消息对象
transform.header.stamp = self.get_clock().now().to_msg() # 设置坐标变换消息的时间戳
transform.header.frame_id = 'world' # 设置一个坐标变换的源坐标系
transform.child_frame_id = self.turtlename # 设置一个坐标变换的目标坐标系
transform.transform.translation.x = msg.x # 设置坐标变换中的X、Y、Z向的平移
transform.transform.translation.y = msg.y
transform.transform.translation.z = 0.0
q = tf_transformations.quaternion_from_euler(0, 0, msg.theta) # 将欧拉角转换为四元数（roll, pitch, yaw）
transform.transform.rotation.x = q[0] # 设置坐标变换中的X、Y、Z向的旋转（四元数）
transform.transform.rotation.y = q[1]
transform.transform.rotation.z = q[2]
transform.transform.rotation.w = q[3]
# Send the transformation
self.tf_broadcaster.sendTransform(transform) # 广播坐标变换，海龟位置变化后，将及时更新坐标变换信息
def main(args=None):
rclpy.init(args=args) # ROS2 Python接口初始化
node = TurtleTFBroadcaster("turtle_tf_broadcaster") # 创建ROS2节点对象并进行初始化
rclpy.spin(node) # 循环等待ROS2退出
node.destroy_node() # 销毁节点对象
rclpy.shutdown() # 关闭ROS2 Python接口
完成代码的编写后需要设置功能包的编译选项，让系统知道Python程序的入口，打开功能包的setup.py文件，加入如下入口点的配置：
entry_points={
'console_scripts': [
'static_tf_broadcaster = my_learning_tf.static_tf_broadcaster:main',
'tf_listener = my_learning_tf.tf_listener:main',
'turtle_tf_broadcaster = my_learning_tf.turtle_tf_broadcaster:main',
],
},
2.4.2 海龟跟随
坐标系都正常广播了，接下来我们就可以订阅两只海龟的位置关系，并且变换成速度指令进行控制啦。
在my_learning_tf目录下创建turtle_following.py；
"""
TF示例-通过坐标变化实现海龟跟随功能
@author: zy
@since : 2025/12/21
"""
import math
import rclpy # ROS2 Python接口库
from rclpy.node import Node # ROS2 节点类
import tf_transformations # TF坐标变换库
from tf2_ros import TransformException # TF左边变换的异常类
from tf2_ros.buffer import Buffer # 存储坐标变换信息的缓冲类
from tf2_ros.transform_listener import TransformListener # 监听坐标变换的监听器类
from geometry_msgs.msg import Twist # ROS2 速度控制消息
from turtlesim.srv import Spawn # 海龟生成的服务接口
class TurtleFollowing(Node):
def __init__(self, name):
super().__init__(name) # ROS2节点父类初始化
self.declare_parameter('source_frame', 'turtle1') # 创建一个源坐标系名的参数
self.source_frame = self.get_parameter( # 优先使用外部设置的参数值，否则用默认值
'source_frame').get_parameter_value().string_value
self.tf_buffer = Buffer() # 创建保存坐标变换信息的缓冲区
self.tf_listener = TransformListener(self.tf_buffer, self) # 创建坐标变换的监听器
self.spawner = self.create_client(Spawn, 'spawn') # 创建一个请求产生海龟的客户端
self.turtle_spawning_service_ready = False # 是否已经请求海龟生成服务的标志位
self.turtle_spawned = False # 海龟是否产生成功的标志位
self.publisher = self.create_publisher(Twist, 'turtle2/cmd_vel', 1) # 创建跟随运动海龟的速度话题
self.timer = self.create_timer(1.0, self.on_timer) # 创建一个固定周期的定时器，控制跟随海龟的运动
def on_timer(self):
from_frame_rel = self.source_frame # 源坐标系
to_frame_rel = 'turtle2' # 目标坐标系
if self.turtle_spawning_service_ready: # 如果已经请求海龟生成服务
if self.turtle_spawned: # 如果跟随海龟已经生成
try:
now = rclpy.time.Time() # 获取ROS系统的当前时间
trans = self.tf_buffer.lookup_transform( # 监听当前时刻源坐标系到目标坐标系的坐标变换
to_frame_rel,
from_frame_rel,
now)
except TransformException as ex: # 如果坐标变换获取失败，进入异常报告
self.get_logger().info(
f'Could not transform {to_frame_rel} to {from_frame_rel}: {ex}')
return
msg = Twist() # 创建速度控制消息
scale_rotation_rate = 1.0 # 根据海龟角度，计算角速度
msg.angular.z = scale_rotation_rate * math.atan2(
trans.transform.translation.y,
trans.transform.translation.x)
scale_forward_speed = 0.5 # 根据海龟距离，计算线速度
msg.linear.x = scale_forward_speed * math.sqrt(
trans.transform.translation.x ** 2 +
trans.transform.translation.y ** 2)
self.publisher.publish(msg) # 发布速度指令，海龟跟随运动
else: # 如果跟随海龟没有生成
if self.result.done(): # 查看海龟是否生成
self.get_logger().info(
f'Successfully spawned {self.result.result().name}')
self.turtle_spawned = True
else: # 依然没有生成跟随海龟
self.get_logger().info('Spawn is not finished')
else: # 如果没有请求海龟生成服务
if self.spawner.service_is_ready(): # 如果海龟生成服务器已经准备就绪
request = Spawn.Request() # 创建一个请求的数据
request.name = 'turtle2' # 设置请求数据的内容，包括海龟名、xy位置、姿态
request.x = float(4)
request.y = float(2)
request.theta = float(0)
self.result = self.spawner.call_async(request) # 发送服务请求
self.turtle_spawning_service_ready = True # 设置标志位，表示已经发送请求
else:
self.get_logger().info('Service is not ready') # 海龟生成服务器还没准备就绪的提示
def main(args=None):
rclpy.init(args=args) # ROS2 Python接口初始化
node = TurtleFollowing("turtle_following") # 创建ROS2节点对象并进行初始化
rclpy.spin(node) # 循环等待ROS2退出
node.destroy_node() # 销毁节点对象
rclpy.shutdown() # 关闭ROS2 Python接口
完成代码的编写后需要设置功能包的编译选项，让系统知道Python程序的入口，打开功能包的setup.py文件，加入如下入口点的配置：
entry_points={
'console_scripts': [
'static_tf_broadcaster = my_learning_tf.static_tf_broadcaster:main',
'tf_listener = my_learning_tf.tf_listener:main',
'turtle_tf_broadcaster = my_learning_tf.turtle_tf_broadcaster:main',
'turtle_following = my_learning_tf.turtle_following:main',
],
},
代码流程如下：
开始
↓
初始化节点
↓
检查spawn服务 → 等待直到可用
↓
请求生成turtle2 → 等待生成完成
↓
每秒执行on_timer():
1. 查询turtle1在turtle2坐标系中的位置(x,y)
2. 计算转向角度：θ = atan2(y,x)
3. 计算前进距离：d = √(x²+y²)
4. 设置角速度：ω = k₁ × θ
5. 设置线速度：v = k₂ × d
6. 发布速度指令
↓
turtle2根据指令移动
↓
位置变化 → TF更新 → 新的控制计算
（闭环反馈控制）
2.4.3 Launch文件
接着创建子文件夹launch，并新增turtle_following_demo.launch.py文件，这个文件里面启动四个节点，分别是：
- 海龟仿真器：
- 海龟节点名称：
/sim; - 发布海龟位置和姿态的话题：
/turtle1/pos，其中turtle1为海龟的名字； - 控制海龟运动速度的话题：
/turtle1/cmd_vel，其中turtle1为海龟的名字； - 会创建第一个海龟，海龟的名字为
turtle1；
- 海龟节点名称：
- 海龟
1的坐标系广播；- 广播节点名称：
/broadcaster1； - 设置参数
turtlename为turtle1；
- 广播节点名称：
- 海龟
2的坐标系广播；- 广播节点名称：
/broadcaster2； - 设置参数
turtlename为turtle2；
- 广播节点名称：
- 海龟跟随控制；
- 节点名称：
/listener； - 设置参数
target_frame为turtle1； - 会通过请求服务
/spawn生成第二个海龟，海龟名字为turtle2；
- 节点名称：
其中，两个坐标系的广播复用了turtle_tf_broadcaster节点，通过传入的参数名修改维护的坐标系名称。
在launch目录下创建turtle_following_demo.py；
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package='turtlesim',
executable='turtlesim_node',
name='sim'
),
Node(
package='learning_tf',
executable='turtle_tf_broadcaster',
name='broadcaster1',
parameters=[
{'turtlename': 'turtle1'}
]
),
DeclareLaunchArgument(
'target_frame', default_value='turtle1',
description='Target frame name.'
),
Node(
package='learning_tf',
executable='turtle_tf_broadcaster',
name='broadcaster2',
parameters=[
{'turtlename': 'turtle2'}
]
),
Node(
package='learning_tf',
executable='turtle_following',
name='listener',
parameters=[
{'target_frame': LaunchConfiguration('target_frame')}
]
),
])
修改setup.py文件，添加配置文件：
import os
from glob import glob
...
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
(os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*.launch.py'))),
(os.path.join('share', package_name, 'config'), glob(os.path.join('config', '*.*'))),
(os.path.join('share', package_name, 'rviz'), glob(os.path.join('rviz', '*.*'))),
],
...
2.4.4 编译运行
编译程序：
pi@NanoPC-T6:~/dev_ws$ colcon build --paths src/my_learning_tf
pi@NanoPC-T6:~/dev_ws$ source install/setup.sh
启动终端，使用ros2中的launch命令来启动launch文件：
pi@NanoPC-T6:~/dev_ws$ ros2 launch my_learning_tf turtle_following_demo.launch.py
[INFO] [launch]: All log files can be found below /home/pi/.ros/log/2025-12-21-03-51-59-893481-NanoPC-T6-10891
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [turtlesim_node-1]: process started with pid [10892]
[INFO] [turtle_tf_broadcaster-2]: process started with pid [10894]
[INFO] [turtle_tf_broadcaster-3]: process started with pid [10896]
[INFO] [turtle_following-4]: process started with pid [10898]
[turtlesim_node-1] Warning: Ignoring XDG_SESSION_TYPE=wayland on Gnome. Use QT_QPA_PLATFORM=wayland to run on Wayland anyway.
[turtlesim_node-1] [INFO] [1766289120.233310301] [sim]: Starting turtlesim with node name /sim
[turtlesim_node-1] [INFO] [1766289120.240942934] [sim]: Spawning turtle [turtle1] at x=[5.544445], y=[5.544445], theta=[0.000000]
[turtlesim_node-1] [INFO] [1766289121.534892282] [sim]: Spawning turtle [turtle2] at x=[4.000000], y=[2.000000], theta=[0.000000]
[turtle_following-4] [INFO] [1766289122.560611052] [listener]: Successfully spawned turtle2
从输出消息中可知：运行该演示会启动4个节点：
1个turtlesim_node节点：节点名称为/sim；- 2个
turtle_tf2_broadcaster节点：节点名称分别为/broadcaster1、/broadcaster2，这两个广播者节点会发布这两只海龟的坐标系； 1个turtle_following：节点名称为/listener，这个侦听者节点会计算两只海龟坐标系之间的差异，并让一只海龟（turtle2）跟随另一只海龟（turtle1）移动；
运行完成后，我们可以看到turtlesim_node节点生成2只海龟（turtle1和turtle2），然后turtle2会沿着一条弧线移动靠近turtle1；
查看节点名称：
pi@NanoPC-T6:~/dev_ws$ ros2 node list
/broadcaster1
/broadcaster2
/listener
/sim
查看话题名称：
pi@NanoPC-T6:~/dev_ws$ ros2 topic list
/parameter_events
/rosout
/tf
/tf_static
/turtle1/cmd_vel
/turtle1/color_sensor
/turtle1/pose
/turtle2/cmd_vel
/turtle2/color_sensor
/turtle2/pose
参考文章
[1] 古月居ROS2入门教程学习笔记
[3] 第六节、双目视觉之相机标定
[4] 机器人操作系统ROS2
浙公网安备 33010602011771号