TF2：座標轉換系統

2026-07-18
  • ros2
  • tf2
  • coordinate-frames
  • robot-model

問題定義

一台機器人上有雷射感測器、相機、機械手臂末端、輪子——每一個都有自己的座標系，而且座標系之間的相對關係可能還會隨時間變動（手臂在動、輪子在轉）。如果每個模組都要自己手動計算「感測器座標系相對於機身座標系的轉換矩陣」，任何一個地方改了尺寸或裝配位置，全部模組都要跟著改。TF2 把這個問題集中處理：所有座標系之間的關係被廣播到一個共用的樹狀結構裡，任何模組只要知道座標系的名字，就能查詢兩者之間任意時刻的轉換關係，不用自己維護計算邏輯。

核心概念說明

TF 樹：一個不斷更新的座標系族譜

TF2 把系統裡所有座標系（frame）組織成一棵樹，每個座標系除了 base_link、map 這種根節點之外，都恰好有一個父座標系。第 4.1 節寫的 URDF 裡每個 <joint>，其實就是在宣告一段父子座標系之間的轉換關係——robot_state_publisher 會讀取 URDF 結構與目前的關節角度，持續把這些轉換關係廣播到 TF2。

用 ros2 run tf2_tools view_frames 可以把目前系統裡的 TF 樹畫成一張圖，這是排查「兩個座標系之間到底有沒有連通」最直接的方式：

bash
ros2 run tf2_tools view_frames

執行後會在當前目錄產生一份 frames.pdf，畫出所有座標系的父子關係與每個座標系被更新的頻率。

Static 與動態 broadcaster：關係會不會隨時間變

tf2_ros 提供兩種廣播方式，選哪一種取決於這段座標關係在物理上會不會隨時間改變：

  • StaticTransformBroadcaster：適合真正固定不變的關係，例如相機外殼鎖死在機身上的位置。只需要廣播一次，之後不用重複發送，底層用 transient local QoS，晚加入的訂閱者依然能拿到這筆資料。
  • TransformBroadcaster：適合會隨時間變化的關係，例如手臂關節角度、機器人在地圖上的移動位置，需要在每次數值改變時（通常搭配 timer 或每次收到新感測器資料時）重新廣播一次帶著新時間戳記的轉換。

查詢轉換：lookupTransform

有了廣播端，另一邊需要用到座標轉換的節點，透過 tf2_ros.Buffer 查詢任意兩個座標系之間的關係：

python
transform = tf_buffer.lookup_transform(
    "base_link",       # target frame
    "camera_link",      # source frame
    rclpy.time.Time(),  # 時間點，Time() 代表取得最新可用的資料
)

這行程式碼實際做的事情，是沿著 TF 樹，把 camera_link 到 base_link 之間所有中間座標系的轉換依序疊乘，回傳一個合併後的單一轉換結果——呼叫端完全不需要知道中間經過了幾層座標系。

實作範例：廣播並查詢一個移動中的感測器座標系

假設有一個雲台相機，會繞著機身的一個軸線持續旋轉，示範動態 broadcaster 與另一個節點查詢的完整流程。

檔案位置：~/ros2_ws/src/my_package/my_package/camera_tf_broadcaster.py

python
import math
import rclpy
from rclpy.node import Node
from tf2_ros import TransformBroadcaster
from geometry_msgs.msg import TransformStamped


class CameraTfBroadcaster(Node):
    def __init__(self):
        super().__init__("camera_tf_broadcaster")
        self.broadcaster = TransformBroadcaster(self)
        self.angle = 0.0
        self.create_timer(0.05, self.broadcast_transform)

    def broadcast_transform(self):
        t = TransformStamped()
        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = "base_link"
        t.child_frame_id = "camera_link"
        t.transform.translation.x = 0.2
        t.transform.translation.y = 0.0
        t.transform.translation.z = 0.3
        t.transform.rotation.z = math.sin(self.angle / 2)
        t.transform.rotation.w = math.cos(self.angle / 2)
        self.broadcaster.sendTransform(t)
        self.angle += 0.02


def main():
    rclpy.init()
    node = CameraTfBroadcaster()
    try:
        rclpy.spin(node)
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    main()

檔案位置：~/ros2_ws/src/my_package/my_package/camera_tf_listener.py

python
import rclpy
from rclpy.node import Node
from tf2_ros import TransformListener, Buffer
from tf2_ros import LookupException, ConnectivityException, ExtrapolationException


class CameraTfListener(Node):
    def __init__(self):
        super().__init__("camera_tf_listener")
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)
        self.create_timer(1.0, self.query_transform)

    def query_transform(self):
        try:
            t = self.tf_buffer.lookup_transform(
                "base_link", "camera_link", rclpy.time.Time()
            )
            self.get_logger().info(
                f"camera_link 相對 base_link: "
                f"x={t.transform.translation.x:.3f}, "
                f"rotation.z={t.transform.rotation.z:.3f}"
            )
        except (LookupException, ConnectivityException, ExtrapolationException) as e:
            self.get_logger().warn(f"查詢失敗: {e}")


def main():
    rclpy.init()
    node = CameraTfListener()
    try:
        rclpy.spin(node)
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    main()

預期輸出

先啟動 camera_tf_broadcaster，間隔幾秒後再啟動 camera_tf_listener：

text
[INFO] [camera_tf_listener]: camera_link 相對 base_link: x=0.200, rotation.z=0.841
[INFO] [camera_tf_listener]: camera_link 相對 base_link: x=0.200, rotation.z=0.909

常見錯誤與除錯技巧

錯誤一：lookupTransform 拋出 LookupException

text
"camera_link" passed to lookupTransform argument target_frame does not exist.

原因：查詢的座標系名稱在 TF 樹裡根本不存在——可能是名稱打錯字，或是負責廣播這個座標系的節點還沒啟動、或啟動了但還沒送出第一筆轉換。

排除方式：用 ros2 run tf2_ros tf2_echo base_link camera_link 手動查詢，如果連這個指令都拿不到結果，問題出在廣播端而不是你自己寫的查詢程式碼：

bash
ros2 run tf2_ros tf2_echo base_link camera_link
text
Waiting for transform base_link ->  camera_link: Invalid frame ID "camera_link" passed to canTransform argument target_frame - frame does not exist

錯誤二：lookupTransform 拋出 ExtrapolationException，即使兩個座標系都存在

text
Lookup would require extrapolation into the future

原因：這是本文開頭範例故意留下的常見時序問題——camera_tf_listener 如果在 camera_tf_broadcaster 啟動的同一瞬間就開始查詢，TF buffer 裡可能還沒有累積到足夠的歷史資料，查詢時間點落在 buffer 已知範圍之外。

排除方式：查詢端在節點剛啟動的頭幾次呼叫本來就該預期查詢可能失敗，用 try/except 包住（如範例程式碼所示）並在失敗時等待重試，而不是假設 TF 資料一定馬上就緒。如果錯誤持續發生、不是只有啟動瞬間才出現，則要檢查廣播端的 timer 頻率是不是太低，或系統時間是否有跨節點不同步的問題。

小結

TF2 把所有座標系組織成一棵樹，靠廣播與查詢兩端分工：不變的關係用 StaticTransformBroadcaster 廣播一次，會變動的關係用 TransformBroadcaster 持續更新；查詢端透過 lookupTransform 取得任意兩個座標系之間的轉換，不需要自己手算中間經過幾層。tf2_echo 與 view_frames 是排查 TF 問題最常用的兩個工具。到這裡，模型（URDF/xacro）與座標系統（TF2）都有了，下一節介紹 RViz2，把這些東西實際視覺化出來，也是後面章節除錯時每天都會用到的工具。

延伸閱讀

常見問題

faq_01.log
TF 樹裡可以有兩個座標系互為父子（形成環）嗎？
不行，TF 樹在結構上要求每個座標系只能有一個父座標系，整體必須是一棵樹，不能出現環。如果程式邏輯上真的需要類似『雙向』的關係，通常是設計上該重新檢視哪個座標系該是父、哪個該是子，而不是嘗試讓 TF 樹出現環。
faq_02.log
static_transform_publisher 跟一般的 TransformBroadcaster 可以互相替代嗎？
技術上 static transform 也能用一般的 TransformBroadcaster 每個週期重發，但這樣做沒有必要且浪費資源——tf2_ros 提供的 StaticTransformBroadcaster 底層用的是 transient local 的 QoS，只發布一次、晚加入的訂閱者依然拿得到，效能與語意都比較合適，靜態不變的關係應該一律用 static 版本。
faq_03.log
lookupTransform 查詢的時間點一定要是 rclpy.time.Time() 這種『現在』嗎？
不用，可以指定任意過去的時間點（只要 TF buffer 裡還保留那個時間點附近的資料，預設緩衝時間是 10 秒），這在需要對齊某個感測器訊息當下的座標關係時很有用——用該訊息 header.stamp 的時間去查詢，而不是查詢當下最新的座標，能避免因為些微時間差造成的座標誤差。