坐标系是我们非常熟悉的一个概念，也是机器人学中的重要基础，在一个完整的机器人系统中，会存在很多坐标系。
在机械臂形态的机器人中，机器人安装的位置叫做基坐标系Base Frame，机器人安装位置在外部环境下的参考系叫做世界坐标系World Frame，机器人末端夹爪的位置叫做工具坐标系，外部被操作物体的位置叫做工件坐标系，在机械臂抓取外部物体的过程中，这些坐标系之间的关系也在跟随变化。
在移动机器人系统中，坐标系一样至关重要，比如一个移动机器人的中心点是基坐标系Base Link，雷达所在的位置叫做雷达坐标系laser link，机器人要移动，里程计会累积位置，这个位置的参考系叫做里程计坐标系odom，里程计又会有累积误差和漂移，绝对位置的参考系叫做地图坐标系map。
一层一层坐标系之间关系复杂，有一些是相对固定的，也有一些是不断变化的，看似简单的坐标系也在空间范围内变得复杂，良好的坐标系管理系统就显得格外重要。
坐标系变换关系的基本理论在各种机器人教材中都有详细说明，可以分解为平移和旋转两个部分，通过一个四乘四的矩阵进行描述，在空间中画出坐标系，那两者之间的变换关系，其实就是向量的数学描述。
以下是借助海龟，通过TF操作坐标系的一个实例，展示了基于坐标系的一种机器人跟随算法：
需要优先在命令行窗口内运行以下语句以安装TF及其相关用到的工具包：
sudo apt install ros-humble-turtle-tf2-py ros-humble-tf2-tools
sudo pip3 install transforms3d再打开两个命令行窗口分别运行：
ros2 launch turtle_tf2_py turtle_tf2_demo.launch.pyros2 run turtlesim turtle_teleop_key此时打开的画布上有两只海龟，通过控制其中一只海龟，另一只会自动调整自己的位置，与第一只海龟重合。
可以通过这个小工具来做查看有哪些坐标系：
ros2 run tf2_tools view_frames 该指令会默认在当前终端路径下生成一个frames.pdf文件，打开之后，就可以看到系统中各个坐标系之间结构上的关系了。（但这个图本身并不好看明白）
每个圆表示一个坐标系
此外，还可以在终端内运行以下代码，通过tf2_echo这个工具以查看某两个坐标系之间的具体关系。
ros2 run tf2_ros tf2_echo turtle2 turtle1运行成功后，终端中就会循环打印坐标系的变换数值了，由平移和旋转两个部分组成，还有旋转矩阵，形如：
At time 1775897788.687342787
- Translation: [0.000, 0.000, 0.000]
- Rotation: in Quaternion (xyzw) [0.000, 0.000, -0.007, 1.000]
- Rotation: in RPY (radian) [0.000, 0.000, -0.015]
- Rotation: in RPY (degree) [0.000, 0.000, -0.837]
- Matrix:
1.000 0.015 0.000 0.000
-0.015 1.000 -0.000 0.000
-0.000 0.000 1.000 0.000
0.000 0.000 0.000 1.000
Translation为平移变换，rotation为旋转变换。三种旋转变换分别为四元素坐标（Quaternion ）、欧拉角弧度制（RPY (radian)）、欧拉角角度制（RPY (degree)）。海龟移动时，这些数据会跟随变化。
最后，还可以用可视化软件来做显示：
ros2 run rviz2 rviz2 -d $(ros2 pkg prefix --share turtle_tf2_py)/rviz/turtle_rviz.rviz这条命令就是用run调用rviz2包的rviz2命令，-d指定参数，让 RViz2 在启动时加载一个预设的配置文件，而不是使用默认布局，该参数即为此配置文件的路径。ros2 pkg prefix为ROS 2 的命令，用于查询一个功能包的安装路径前缀，通过“–share”告诉命令，我们想要的是该功能包下的share 目录。turtle_tf2_py则是一个 ROS 2 官方示例功能包，用于演示 TF2 (坐标变换) 功能。它的 share 目录下通常会存放配置文件。$( … )是 Linux 终端中的命令替换符。它会先执行括号内的命令，然后将该命令的输出结果“填充”到当前位置。最后，在得到的 share 目录的路径后面，再接上 urtle_tf2_py 包内 rviz 子文件夹下的 turtle_rviz.rviz 配置文件路径。
小海龟运动时，Rviz中的坐标轴就会开始运动。
静态TF变换：相对位置不发生变化的情况，如激光雷达和机器人底盘之间的位置关系，安装好之后基本不会变化。
一个示例的代码如下：
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() # 创建一个坐标变换的消息对象，这是ROS2内的标准定义类
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）。四元素不直观，但是ros2的标准方式
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()
self.tf_broadcaster.sendTransform(static_transformStamped) 将坐标系相对于另一个固定坐标系的空间关系（位置和朝向）发布到 ROS 系统中。
数据对齐与融合：当机器人上有激光雷达、摄像头、IMU 等多个传感器时，它们各自测量得到的数据默认都在自身坐标系下。通过广播这些传感器到机器人中心（如 base_link）的静态变换，其他节点就能把所有传感器的数据转换到同一个坐标系下进行融合处理。
实现“查询式”坐标转换：其他任何 ROS 节点（如导航模块、定位模块）都可以通过 tf2 接口（如 lookupTransform）随时查询这两个坐标系之间的关系。例如，知道激光雷达点云中的某个点，在机器人坐标系下是什么位置。
可视化与调试：RViz 等可视化工具会监听 TF 广播。广播后，你就能在 RViz 中看到坐标轴的图形显示，直观检查各部件的位置是否安装正确，或观察机器人的运动轨迹。
静态与动态变换：此处static_transformStamped 通常表示静态变换（如传感器在车身上的固定安装位置）。这种关系不变，只需广播一次，后续会被缓存并永久使用。如果是动态变换（如随关节转动的机械臂末端），则需要以高频（如 100Hz）持续广播，以反映实时运动。
可以定时监听两个坐标系的情况：
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) # 创建一个固定周期的定时器，处理坐标信息，频率为1hz，self.on_timer为回调函数，以1hz频率调用它
def on_timer(self):
try:
now = rclpy.time.Time() # 获取ROS系统的当前时间
trans = self.tf_buffer.lookup_transform( # 监听当前时刻源坐标系到目标坐标系的坐标变换，now指定了当前时间。返回的是一个坐标变换数值，和上一个文件中的tf_broadcaster 结构相同
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接口
自行手动复现上面海龟跟随的例子：
launch文件如下：
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'} # 输入参数。两个节点都在广播海龟相对world坐标系的变化，与下面那个node几乎一样，但通过参数修改实现的资源重定向
]
),
DeclareLaunchArgument(
'target_frame', default_value='turtle1',
description='Target frame name.'
),
Node(
package='learning_tf',
executable='turtle_tf_broadcaster', # 与上一个node的可执行文件相同
name='broadcaster2', # 。与上一个node功能包与命令相同，但名字不同，防止重名
parameters=[
{'turtlename': 'turtle2'}
]
),
Node(
package='learning_tf',
executable='turtle_following', # 监听两个海龟位置关系的可执行文件
name='listener',
parameters=[
{'target_frame': LaunchConfiguration('target_frame')}
]
),
])此处DeclareLaunchArgument 的作用是声明一个可由用户在启动时自定义的 launch 参数。在这个例子中，它声明了名为 target_frame 的参数，默认值为 'turtle1'，描述为“目标坐标系名称”。通过这个声明，用户可以在运行 launch 文件时通过命令行传递不同的值（例如 target_frame:=turtle2），该值随后会通过 LaunchConfiguration('target_frame') 传递给下游节点（这里是 turtle_following 节点的 target_frame 参数），从而灵活改变节点的行为，而无需修改 launch 文件本身。简言之，它让 launch 文件变得更加通用和可配置。
其中turtle_tf_broadcaster可执行文件的代码如下：
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') # 创建一个海龟名称的参数，通过launch设置的参数取代海龟的名字
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): # 创建一个处理海龟位置消息的回调函数，将位置消息转变成坐标变换，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接口
turtle_following的可执行文件代码如下：
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接口