文章

ROS1中的TF变换

发表于
作者
20 分钟阅读
ROS1中的TF变换

ROS中的TF变换

（注意：本站所有博客均是记录本人学习过程中的理解，可能存在错误和不完善之处，请仔细甄别）

在任何复杂的机器人系统中，我们都必须处理一个核心问题：坐标关系。想象一下，一个机械臂的末端夹爪相对于机械臂的基座在什么位置？一个激光雷达扫描到的障碍物，相对于机器人底盘的中心又在哪里？如果不能精确、实时地回答这些问题，机器人就无法与物理世界进行有意义的交互。

ROS（机器人操作系统）提供了一套强大的系统来解决这个问题，它就是 TF。本文将首先深入讲解ROS中位置和姿态的表达方式，然后阐述TF系统是如何管理和变换这些坐标关系的，最后通过一个经典的“小乌龟跟随”案例，用Python代码完整地实现一个简单的TF应用。

ROS中的位置与姿态表示法

在谈论TF变换之前，我们必须先统一“语言”：ROS是如何描述一个物体在空间中的位置和姿态（Pose）的？

  • 位置 (Position)：这比较简单，通常用一个三维笛卡尔坐标 (x, y, z) 来表示。在ROS中，对应的数据类型是 geometry_msgs/Point。
  • 姿态 (Orientation)：姿态描述的是物体的朝向。虽然我们直观上习惯使用欧拉角（如：绕X轴旋转roll度，绕Y轴旋转pitch度，绕Z轴旋转yaw度），但欧拉角存在“万向节死锁”问题。因此，在三维空间中，ROS更推荐使用四元数 (Quaternion) 来表示姿态。四元数由四个值 (x, y, z, w) 组成，能够无歧义、更高效地表示三维旋转。ROS中对应的数据类型是 geometry_msgs/Quaternion。

将位置和姿态组合在一起，就得到了一个完整的位姿 (Pose)，它精确地描述了某个物体在特定坐标系下的空间状态。

TF系统：坐标系之树

TF并不仅仅是进行一次性的坐标计算，它是一个完整的坐标变换系统。我们可以把它理解为一棵“坐标系之树”。

  • 坐标系 (Frame)：每个独立的坐标参考系都是一个Frame。例如，world (世界坐标系)、robot_base (机器人基座坐标系)、camera_link (相机坐标系) 等。
  • 变换 (Transform)：一个变换定义了两个坐标系之间的平移和旋转关系。例如，TF可以告诉你 camera_link 相对于 robot_base 的位姿。
  • 树状结构：TF将系统中所有的坐标系组织成一棵树。每个坐标系（除了根坐标系，通常是world或map）都有一个父坐标系。这种结构的好处在于，ROS可以自动计算树上任意两个坐标系之间的变换关系，即使它们没有直接连接。
  • 时间：TF还考虑了时间维度。它会缓存带有时间戳的坐标变换数据，让你能够查询过去某个特定时刻的坐标关系，这对于处理传感器数据延迟等问题至关重要。

在编程中，我们主要通过两个核心组件与TF系统交互：

  • TransformBroadcaster (广播器)：用于向TF系统发布某个坐标系相对于其父坐标系的变换关系。
  • TransformListener (监听器)：用于从TF系统获取任意两个坐标系之间的变换关系。

利用TF变换编写代码

下面，我们通过一个经典的 turtlesim 案例来实践TF。目标是：创建两只小乌龟，我们用键盘控制第一只（turtle1），第二只（turtle2）能够自动追踪第一只。

广播器的任务是告诉TF系统 turtle1 在哪里。我们通过订阅 turtlesim 发布的 /turtle1/pose 话题来获取它的实时位置，然后将这个位姿作为 world 坐标系到 turtle1 坐标系的变换发布出去。

创建 turtle_tf_broadcaster.py 文件：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37

#!/usr/bin/env python3
import rospy
import tf
import turtlesim.msg

def handle_turtle_pose(msg, turtlename):
    # 1. 创建一个TransformBroadcaster对象
    br = tf.TransformBroadcaster()

    # 2. 从turtlesim的Pose消息中提取位置和姿态
    #    turtlesim是2D的，所以z坐标为0
    translation = (msg.x, msg.y, 0)
    
    #    turtlesim的朝向theta是绕Z轴的旋转，即yaw
    #    使用tf.transformations库将欧拉角转换为四元数
    rotation = tf.transformations.quaternion_from_euler(0, 0, msg.theta)

    # 3. 发布变换
    #    参数: (平移, 旋转, 时间戳, 子坐标系, 父坐标系)
    br.sendTransform(translation,
                     rotation,
                     rospy.Time.now(),
                     turtlename,
                     "world")

if __name__ == '__main__':
    rospy.init_node('turtle_tf_broadcaster')
    
    # 从参数服务器获取要广播的乌龟名称
    turtlename = rospy.get_param('~turtle')
    
    # 订阅指定乌龟的pose话题，并绑定回调函数
    rospy.Subscriber('/%s/pose' % turtlename,
                     turtlesim.msg.Pose,
                     handle_turtle_pose,
                     turtlename)
    rospy.spin()

代码解释：

  • 导入：我们导入了tf库，我们可以从中获取TransformBroadcaster类和transformations工具模块。同时，由于是小乌龟的位姿信息，我们导入turtlesim.msg，以便订阅者能够正确解析来自turtlesim的话题。其中，我们订阅的是turtlesim.msg.Pose，该消息的格式如下：

    1
2
3
4
5

    float32 x
float32 y
float32 theta
float32 linear_velocity
float32 angular_velocity

    其含义同英文释义，从Pose中可以解包出所需的消息。

  • 回调函数：我们的回调函数除了接收msg为参数外，还接收了一个turtlename的字符串参数，用来辨别是哪一只乌龟。首先使用

    tf.TransformBroadcaster()实例化一个广播器对象，接着就是要为发布变换准备需要的信息。发布变换的函数为：br.sendTransform(translation, rotation,rospy.Time.now(),turtlename,"world")，对应参数为(平移, 旋转, 时间戳, 子坐标系, 父坐标系)。我们一个一个来看：

    • 平移：格式为一个三元组(x,y,z)，为在子坐标系下的三维位置。由于此处小海龟在平面内，所以z=0。
    • 旋转：格式为一个四元数，为其形态绕x,y,z三轴转过的角度，平面内只有绕z轴的角度，直接从msg.theta提取即可。tf.transformations.quaternion_from_euler()函数可以方便的把欧拉角转化为四元数。
    • 时间戳：用在调节传感器延迟。这里直接用rospy.Time.now()获取即可
    • 子坐标系：一个字符串，其实就是当前正在被描述物体的参考系标识。
    • 父坐标系：一个字符串，即当前被描述物体的参考系，这里取”world”，一般是作为根参考系存在。

    值得注意的是，既然整句话是在说turtlename相对于”world”的位姿，为什么把turtlename叫做子坐标系呢？因为我们所有的位置都是一个相对概念，当我们在另一个TF声名发布的时候，可能就需要以这只小乌龟为参考系，那么turtlename就变成一个坐标系了。我们编写的时候，只要知道是发布第四个参数相对于第五个参数的声明就好了。

  • 主函数：一个普通的订阅流程，但是使用了turtlename = rospy.get_param('~turtle')从参数服务器里面获取名称。参数服务器相当于一个全局的字典，rospy.get_param()方法用于在该全局字典里面查询键。~表示当前节点的名称，节点的名称和参数服务器里的键值对都可以在launch文件中定义，这样，我们就可以用同一个python脚本启动两只乌龟了，只要保证两次启动节点时规定的节点名称不同，并且在参数服务器上给两个名称分配了不同的turtlename即可。

接下来要写监听器，它的任务是驱动 turtle2 去追踪 turtle1。它需要做三件事：

  1. 生成第二只乌龟 turtle2。
  2. 不断地向TF系统查询 turtle1 相对于 turtle2 的位置。
  3. 根据查询到的相对位置，计算出控制 turtle2 运动的速度指令。

创建 turtle_tf_listener.py 文件：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48

#!/usr/bin/env python3
import rospy
import tf
import math
import geometry_msgs.msg
import turtlesim.srv

if __name__ == '__main__':
    rospy.init_node('turtle_tf_listener')

    # 1. 创建一个TransformListener对象
    listener = tf.TransformListener()

    # 2. 调用turtlesim的spawn服务，创建turtle2
    rospy.wait_for_service('spawn')
    spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
    spawner(4, 2, 0, 'turtle2') # 在(4, 2)位置生成turtle2

    # 3. 创建一个Publisher，用于发布turtle2的速度指令
    turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist, queue_size=1)

    rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
        try:
            # 4. 查找turtle1相对于turtle2的变换
            #    lookupTransform(目标坐标系, 源坐标系, 时间)
            #    这里我们想知道turtle1在turtle2坐标系下的位置
            (trans, rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            # 如果变换不存在或时间戳问题，捕获异常并继续
            continue

        # 5. 根据相对位置计算速度指令
        #    trans[1]是y方向的差距, trans[0]是x方向的差距
        #    角速度(angular.z)的目标是让turtle2朝向turtle1
        angular = 4 * math.atan2(trans[1], trans[0])
        
        #    线速度(linear.x)的大小与两者距离成正比
        linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
        
        cmd = geometry_msgs.msg.Twist()
        cmd.linear.x = linear
        cmd.angular.z = angular
        
        # 6. 发布速度指令
        turtle_vel.publish(cmd)

        rate.sleep()

  • 导入：我们导入了完成此任务所需的多个模块。

    • tf库：用于获取核心的TransformListener类，它是接收和查询TF变换的接口。
    • math：一个标准的Python数学库，我们将用它来进行atan2 (计算角度) 和sqrt (计算距离) 的运算。
    • geometry_msgs.msg：ROS标准的几何消息包。我们需要用它来创建Twist类型的消息，这是一种包含线速度和角速度的标准消息格式，用于控制turtlesim的运动。
    • turtlesim.srv：turtlesim仿真器提供的服务定义。我们需要调用turtlesim的spawn服务来动态地创建第二只乌龟，因此需要导入其服务类型。

  • 主函数：这个脚本没有回调函数，其核心逻辑全部在主函数的一个循环中。我们可以将其分为两个部分：初始化准备和主循环。

    • 初始化与准备：

      1. 和广播器一样，首先初始化节点，然后最关键的一步是使用 listener = tf.TransformListener() 实例化一个监听器对象。这个对象一旦被创建，就会自动开始订阅TF话题，接收系统中所有的坐标变换广播，并在内部维护一个带时间缓存的坐标系树。
      2. 接下来，我们调用了ROS的服务(Service)。服务是一种请求/响应模式的通信。我们通过rospy.ServiceProxy(‘spawn’, turtlesim.srv.Spawn)创建了一个名为spawner的服务客户端，然后调用spawner(4, 2, 0, ‘turtle2’)，向turtlesim节点发送一个请求，让它在坐标(4, 2)处创建一只名为turtle2的新乌龟。
      3. 最后，创建一个发布者turtle_vel，它将向/turtle2/cmd_vel话题发布geometry_msgs.msg.Twist类型的消息，从而控制turtle2的运动。

    • 主循环： 这是一个while循环，只要ROS核心没有关闭，它就会以10Hz的频率不断执行。

      1. 循环中最核心的是listener.lookupTransform()函数，但它被包裹在一个try…except块中。这是因为在节点刚启动时，TF监听器可能还没有接收到足够的变换信息，此时查询会失败并抛出异常。try…except可以捕获这些异常，防止程序崩溃，保证了节点的健壮性。

      2. listener.lookupTransform(‘/turtle2’, ‘/turtle1’, rospy.Time(0))是本程序的精髓，它的参数如下：

        • 目标坐标系(target_frame): ‘turtle2’。可以理解为“你站在谁的视角”。
        • 源坐标系(source_frame): ‘turtle1’。可以理解为“你要观察的目标是谁”。
        • 时间戳(time): rospy.Time(0)。这是一个常用技巧，表示查询“最新可用的”变换数据。
        • 返回值: (trans, rot)，即平移和旋转。

        因此，这整句话的含义是：“请告诉我，在当前最新的时刻，站在turtle2的坐标系下看，turtle1相对于我的位置(trans)和姿态(rot)是什么？”

      3. 得到相对位置trans（一个(x, y, z)元组）后，我们就可以进行控制计算了。math.atan2(trans[1], trans[0])利用相对的y和x坐标计算出turtle1相对于turtle2的角度，我们将其作为turtle2的角速度。math.sqrt(trans[0] ** 2 + trans[1] ** 2)计算出两者之间的直线距离，我们将其作为turtle2的线速度。

      4. 最后，创建一个Twist消息对象，将计算出的线速度和角速度填入，并通过我们之前创建的turtle_vel发布者发布出去。turtle2接收到这个速度指令后，就会朝turtle1移动了。

为了方便，我们编写一个launch文件来启动所有节点。

创建 start_tf_demo.launch 文件：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

<launch>
    <!-- 启动turtlesim仿真器 -->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>

    <!-- 为turtle1启动一个广播器 -->
    <node pkg="<your_package_name>" type="turtle_tf_broadcaster.py" name="turtle1_tf_broadcaster">
        <param name="turtle" value="turtle1" />
    </node>
    
    <!-- 为turtle2也启动一个广播器，这样TF树才完整 -->
    <node pkg="<your_package_name>" type="turtle_tf_broadcaster.py" name="turtle2_tf_broadcaster">
        <param name="turtle" value="turtle2" />
    </node>

    <!-- 启动监听器，驱动turtle2 -->
    <node pkg="<your_package_name>" type="turtle_tf_listener.py" name="listener" />
    
    <!-- 启动键盘控制节点来移动turtle1 -->
    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
</launch>

注意：请将上面代码中的 <your_package_name> 替换为您自己功能包的名称。

现在，通过 roslaunch 启动：

1

roslaunch <your_package_name> start_tf_demo.launch

你将会看到 turtlesim 窗口和两只小乌龟。点击运行键盘控制节点的终端，使用方向键移动 turtle1，你会发现 turtle2 已经开始实时跟随它了！

效果展示

my_tf

ROS, 学习记录
ROS ROS1
本文由作者按照 CC BY 4.0 进行授权

热门标签

ROS1 ROS