tf2 좌표 변환 기초 - 프레임 트리

이 장에서 배우는 것

두리가 건물 밖으로 나오면 위치를 표현하는 기준도 늘어난다. 바퀴로 계산한 이동량, 지도 위의 위치, 로봇 앞쪽으로 떨어진 지점은 서로 다른 기준을 사용한다. 좌표가 모두 미터 단위라고 해서 바로 더하거나 비교할 수는 없다. 먼저 어느 기준에서 표현한 값인지 알아야 한다.

프레임(frame)은 위치와 방향을 표현하는 좌표계다. tf2는 프레임 사이의 관계를 저장하고, 연결된 관계를 따라 필요한 변환을 구한다. 앞 장에서 여러 콜백을 실행하는 방법을 살펴보았다면, 여기서는 콜백이 주고받는 위치 정보의 기준을 맞춘다. 좌표 변환의 방향을 먼저 정리한 뒤, 두리의 프레임 트리를 만들고 같은 계산을 NumPy로 재현한다.

• map, odom, base_link가 각각 어떤 기준을 나타내는지 설명한다.
• 부모와 자식 관계로 프레임 트리를 구성하고 변환의 중복 발행을 피한다.
• 정적 브로드캐스터와 동적 브로드캐스터로 변환을 발행한다.
• 대상 프레임과 원본 프레임을 구분하여 변환을 조회하고 점에 적용한다.
• 회전 행렬과 동차 변환 행렬을 NumPy로 계산하여 tf2의 결과를 이해한다.

문제 상황

두리가 출발 지점에서 동쪽으로 1m 이동한 뒤 북쪽을 바라보고 멈췄다고 하자. 로봇 앞쪽 1m에 놓인 물건은 로봇 기준으로는 (1, 0, 0)이다. 그러나 출발 지점을 원점으로 삼은 좌표계에서는 (1, 1, 0)이다. 두 좌표는 같은 물건을 가리키지만 숫자가 다르다.

지도에서 출발 지점의 좌표가 (2, 1, 0)이라면 물건의 지도 좌표는 다시 (3, 2, 0)이 된다. 로봇 기준의 x값을 지도 기준 x값에 그대로 더하면 (4, 1, 0)이라는 잘못된 결과를 얻는다. 로봇의 앞쪽이 지도에서 어느 방향인지 반영하지 않았기 때문이다.

실제 프로그램에서는 문제가 더 잘 드러나지 않는다. 위치 메시지에는 정상적인 실수가 들어 있고 통신도 성공한다. 하지만 두리가 회전한 뒤부터 물건의 위치가 옆으로 밀리거나, 이동할수록 표시 위치가 벌어진다. 이런 현상을 줄이려면 좌표마다 프레임 이름을 붙이고, 프레임 간 관계를 하나의 트리로 관리해야 한다.

이 장의 실습은 평평한 바닥에서 움직이는 두리를 가정한다. 거리는 미터, 각도는 라디안을 사용한다. 지도와 출발 좌표계의 축 방향은 같고, 두리는 위에서 내려다봤을 때 반시계 방향으로 90도 돌아 있다. 높이는 모두 0으로 두어 회전과 평행 이동의 순서를 확인하는 데 집중한다.

프레임 트리와 세 가지 기준

ROS의 일반적인 로봇 몸체 좌표계는 오른손 좌표계이며 x축은 앞쪽, y축은 왼쪽, z축은 위쪽이다. 두리가 바라보는 방향이 바뀌면 base_link의 축도 함께 돌아간다. 반면 지도나 출발 지점에 놓은 축이 로봇을 따라 도는 것은 아니다. 같은 x축이라는 이름보다 어느 프레임의 x축인지가 더 중요하다.

두리의 위치를 설명하는 세 프레임의 역할

프레임	기준	주요 성질	두리에서의 의미
map	장기적인 지도 기준	위치추정 보정으로 좌표가 불연속적으로 바뀔 수 있다	배달 목적지와 로봇 위치를 비교하는 기준
odom	지역적인 이동 기준	연속성을 유지하지만 오차가 누적될 수 있다	출발 이후의 부드러운 이동 표현
base_link	로봇 몸체에 고정된 기준	로봇과 함께 이동하고 회전한다	두리의 앞·왼쪽·위 방향

map과 odom은 둘 다 바닥에 놓인 좌표계처럼 보이지만 목적이 다르다. 바퀴의 회전량을 누적하면 짧은 구간의 움직임을 연속적으로 표현하기 좋다. 대신 바퀴가 미끄러지면 실제 위치와의 차이가 쌓인다. 지도와 대조한 위치추정은 이 오차를 보정할 수 있지만, 보정 직후의 위치가 이전 값에서 갑자기 달라질 수 있다.

이 두 특성을 함께 사용하기 위해 보통 map을 odom의 부모로, odom을 base_link의 부모로 둔다. odom에서 본 base_link는 연속적으로 움직이고, map에서 odom으로 이어지는 관계가 지도 기준 보정을 담당한다. map의 원점이 위도·경도 원점이거나 축이 반드시 동·북 방향인 것은 아니다. 실제 지도와 시스템이 정한 약속을 따라야 한다.

트리에서 자식 프레임은 한 부모에 연결되며, 연결을 따라 돌아왔을 때 자기 자신으로 되돌아오는 순환이 없어야 한다. 두리가 가진 동일한 base_link를 map 아래에도 직접 연결하면 관계가 모호해진다. map에서 base_link로 가는 변환은 이미 두 연결을 합성하여 계산할 수 있으므로 별도로 발행할 필요가 없다.

프레임 관계마다 발행 책임도 하나로 정한다. 예를 들어 바퀴 이동량을 계산하는 노드와 다른 위치추정 노드가 동시에 odom과 base_link의 관계를 발행하면, 시간에 따라 서로 다른 값이 버퍼에 들어올 수 있다. 노드 수가 아니라 각 연결의 책임자를 기준으로 구성을 점검해야 한다.

실습에서는 지도 보정 장치가 없으므로 map에서 odom으로 이어지는 관계를 고정한다. 이것은 실습용 가정이다. 실제 주행 시스템에서 위치추정이 이 관계를 계속 보정한다면 같은 관계를 정적으로 발행해서는 안 된다. 프레임의 역할에 관한 기준은 ROS 좌표 프레임 규약에서, 축과 단위는 ROS 단위와 좌표계 규약에서 확인할 수 있다.

변환의 방향과 좌표 계산

변환 메시지에서 부모 프레임은 header.frame_id, 자식 프레임은 child_frame_id에 들어간다. 이 메시지는 부모 기준으로 본 자식의 원점과 자세를 표현한다. 좌표 계산에 적용하면 자식 기준 점을 부모 기준 점으로 바꾸는 변환이다. 트리 그림에서 부모에서 자식으로 화살표를 그렸다고 해서 좌표 변환의 적용 방향까지 같다고 생각하면 혼동하기 쉽다.

부모를 A, 자식을 B라고 쓰자. B 기준 점 p_B를 A 기준으로 바꾸는 식은 p_A = R_AB p_B + t_AB다. R_AB는 B의 축 방향을 A 기준으로 나타내는 회전 행렬이고, t_AB는 B 원점의 A 기준 좌표다. 점을 회전시켜 축 방향을 맞춘 다음, 두 원점 사이의 차이를 더한다.

3차원에서는 회전 행렬이 3×3이고 평행 이동 벡터가 길이 3이다. 두 계산을 한 번의 행렬 곱으로 묶으려면 점 뒤에 1을 붙이고 4×4 동차 변환 행렬을 사용한다. 마지막 행은 (0, 0, 0, 1)이며 오른쪽 위 열에 평행 이동을 넣는다. 방향 벡터를 변환할 때는 원점 이동을 더하지 않는다는 차이가 있지만, 이 장의 코드는 위치를 나타내는 점만 다룬다.

p_A = R_AB @ p_B + t_AB

T_AB = [ R_AB  t_AB ]
       [ 0 0 0   1  ]

p_A_h = T_AB @ p_B_h

T_map_base = T_map_odom @ T_odom_base

합성에서는 오른쪽 행렬부터 점에 작용한다. base_link 기준 점을 map으로 옮기려면 먼저 odom으로 바꾸고, 그다음 map으로 바꾼다. 행렬 이름을 목적지와 출발지 순서로 적으면 중간 이름인 odom이 맞물리는지 확인하기 쉽다. 곱셈 순서를 바꿔도 같은 결과가 나온다고 가정해서는 안 된다.

반대 방향은 역변환으로 구한다. 회전 행렬의 역행렬은 전치 행렬이지만, 전체 변환에서 평행 이동의 부호만 바꾸는 것으로는 부족하다. 원래 식을 정리하면 p_B = R_AB의 전치 @ (p_A - t_AB)가 된다. 따라서 역변환의 평행 이동은 -R_AB의 전치 @ t_AB다.

tf2 메시지는 회전을 쿼터니언(quaternion)으로 표현한다. 필드 순서는 x, y, z, w이며 네 성분의 제곱합이 1인 단위 쿼터니언을 사용한다. 회전이 없을 때는 (0, 0, 0, 1)이다. 기본 생성된 네 필드가 모두 0인 상태를 회전 없음으로 사용하면 안 된다.

평면에서 z축 주위로 각도 θ만큼 회전한다면 x와 y는 0, z는 sin(θ/2), w는 cos(θ/2)다. 두리의 90도 회전에는 θ = π/2를 넣는다. 이 장의 NumPy 예제는 같은 회전을 행렬로 만들고, ROS 예제는 쿼터니언으로 만들어 두 표현이 같은 자세를 나타내도록 구성한다.

정적 발행, 동적 발행, 변환 조회

정적 브로드캐스터(static broadcaster)는 시간에 따라 바뀌지 않는 관계를 발행한다. 로봇 몸체에 단단히 고정한 구조물의 위치가 대표적인 예다. 정적 변환은 /tf_static으로 전달되며, 발행자가 제공하는 내구성 설정을 통해 나중에 시작한 구독자도 받을 수 있다. 실습에서는 고정된 map과 odom 관계를 여기에 넣는다.

동적 브로드캐스터(dynamic broadcaster)는 시각과 함께 변환을 계속 발행한다. 두리의 위치가 바뀌면 odom 기준 base_link의 평행 이동과 회전도 바뀐다. 이 변환은 /tf로 전달된다. 로봇이 잠시 멈춰 값이 같아지더라도 움직일 수 있는 관계라면 동적 발행을 유지하는 편이 관계의 의미에 맞다.

고정 관계와 움직이는 관계를 발행하는 방법

구분	클래스	전달 경로	실습의 사용 위치
정적 변환	StaticTransformBroadcaster	/tf_static	map 기준 odom의 고정 위치
동적 변환	TransformBroadcaster	/tf	odom 기준 base_link의 이동
수신과 저장	TransformListener와 Buffer	두 경로를 구독한다	조회 노드가 트리를 구성한다

수신 측에서는 리스너(listener)가 변환 메시지를 받아 버퍼(buffer)에 넣는다. 조회 함수는 버퍼에 이미 들어온 관계를 찾아 합성한다. 조회를 호출한다고 다른 노드에 변환을 요청하는 서비스 호출이 발생하는 것은 아니다. 그러므로 리스너를 만든 직후에는 발행 노드가 정상이어도 데이터가 아직 없을 수 있다.

lookup_transform의 인수는 대상 프레임, 원본 프레임, 조회 시각 순서다. lookup_transform("map", "base_link", Time())은 base_link 기준 좌표를 map 기준으로 바꾸는 변환을 구한다. 여기서 Time()의 0 시각은 최신으로 조회 가능한 변환을 요청한다는 뜻이다. 여러 동적 연결이 있으면 체인 전체에서 조회 가능한 최신 공통 시각의 제약을 받는다.

이번 코드는 조회가 준비되지 않았을 때 예외를 잡고 다음 타이머 호출에서 다시 시도한다. 콜백 안에서 긴 시간 기다리지 않으므로 단일 스레드 실행기에서도 리스너의 수신 콜백이 실행될 기회를 얻는다. 지정한 과거 시각이나 서로 다른 시각 사이의 변환은 다음 장에서 다룬다. 여기서는 프레임 이름과 변환 방향을 정확히 맞추는 데 집중한다.

API의 세부 정의는 Jazzy의 Python Buffer API와 TransformListener API에서 확인할 수 있다. 아래 프로그램과 수치는 두리의 상황에 맞추어 구성한 예제다.

완성 코드

파일 두 개를 같은 디렉터리에 저장한다. 첫 번째는 ROS 없이 좌표 계산을 확인하는 보조 프로그램이며 NumPy만 필요하다. 두 번째는 ROS 2 Jazzy의 rclpy, geometry_msgs, tf2_ros, tf2_geometry_msgs를 사용한다. ROS가 없는 macOS나 Linux에서는 첫 번째 파일을 실행하고, 두 번째 파일은 문법 검사까지 수행할 수 있다. ROS 프로그램의 실제 실행에는 해당 패키지를 사용할 수 있는 Jazzy 환경이 필요하다.

frame_math.py

import math

import numpy as np


def planar_transform(x, y, yaw):
    c = math.cos(yaw)
    s = math.sin(yaw)
    transform = np.eye(4, dtype=float)
    transform[:3, :3] = np.array(
        [
            [c, -s, 0.0],
            [s, c, 0.0],
            [0.0, 0.0, 1.0],
        ],
        dtype=float,
    )
    transform[:3, 3] = [x, y, 0.0]
    return transform


def rigid_inverse(transform):
    rotation = transform[:3, :3]
    translation = transform[:3, 3]
    inverse = np.eye(4, dtype=float)
    inverse[:3, :3] = rotation.T
    inverse[:3, 3] = -rotation.T @ translation
    return inverse


def show_point(label, point):
    values = [
        0.0 if abs(float(value)) < 1e-12 else float(value)
        for value in point[:3]
    ]
    print(
        f"{label}: "
        f"({values[0]:.3f}, {values[1]:.3f}, {values[2]:.3f})"
    )


def main():
    map_from_odom = planar_transform(2.0, 1.0, 0.0)
    odom_from_base = planar_transform(1.0, 0.0, math.pi / 2.0)
    map_from_base = map_from_odom @ odom_from_base

    point_base = np.array([1.0, 0.0, 0.0, 1.0])
    point_odom = odom_from_base @ point_base
    point_map = map_from_base @ point_base

    base_from_map = rigid_inverse(map_from_base)
    point_back = base_from_map @ point_map

    np.testing.assert_allclose(
        point_odom, [1.0, 1.0, 0.0, 1.0], atol=1e-12
    )
    np.testing.assert_allclose(
        point_map, [3.0, 2.0, 0.0, 1.0], atol=1e-12
    )
    np.testing.assert_allclose(
        point_back, point_base, atol=1e-12
    )
    np.testing.assert_allclose(
        base_from_map @ map_from_base, np.eye(4), atol=1e-12
    )

    show_point("base_link", point_base)
    show_point("odom", point_odom)
    show_point("map", point_map)
    show_point("복원한 base_link", point_back)
    print("검사: 통과")


if __name__ == "__main__":
    main()

duri_frames.py

import argparse
import math

import rclpy
from geometry_msgs.msg import PointStamped, TransformStamped
from rclpy.node import Node
from rclpy.time import Time
from tf2_geometry_msgs import do_transform_point
from tf2_ros import (
    Buffer,
    StaticTransformBroadcaster,
    TransformBroadcaster,
    TransformException,
    TransformListener,
)


def make_transform(parent, child, stamp, x, y, yaw):
    transform = TransformStamped()
    transform.header.stamp = stamp
    transform.header.frame_id = parent
    transform.child_frame_id = child
    transform.transform.translation.x = float(x)
    transform.transform.translation.y = float(y)
    transform.transform.translation.z = 0.0
    transform.transform.rotation.x = 0.0
    transform.transform.rotation.y = 0.0
    transform.transform.rotation.z = math.sin(yaw / 2.0)
    transform.transform.rotation.w = math.cos(yaw / 2.0)
    return transform


class DuriFrames(Node):
    def __init__(self):
        super().__init__("duri_frame_broadcaster")
        self.static_broadcaster = StaticTransformBroadcaster(self)
        self.dynamic_broadcaster = TransformBroadcaster(self)
        self.step = 0

        fixed = make_transform(
            "map",
            "odom",
            self.get_clock().now().to_msg(),
            2.0,
            1.0,
            0.0,
        )
        self.static_broadcaster.sendTransform(fixed)
        self.timer = self.create_timer(0.2, self.publish_pose)

    def publish_pose(self):
        moving = make_transform(
            "odom",
            "base_link",
            self.get_clock().now().to_msg(),
            1.0 + 0.1 * self.step,
            0.0,
            math.pi / 2.0,
        )
        self.dynamic_broadcaster.sendTransform(moving)
        self.step += 1


class DuriQuery(Node):
    def __init__(self):
        super().__init__("duri_frame_query")
        self.buffer = Buffer()
        self.listener = TransformListener(self.buffer, self)
        self.done = False
        self.exit_code = 0
        self.attempts = 0
        self.timer = self.create_timer(0.1, self.try_query)

    def try_query(self):
        self.attempts += 1
        try:
            transform = self.buffer.lookup_transform(
                "map", "base_link", Time()
            )
        except TransformException:
            if self.attempts >= 50:
                print("変換を取得できませんでした")
                self.exit_code = 1
                self.done = True
                self.timer.cancel()
            return

        point = PointStamped()
        point.header.frame_id = "base_link"
        point.header.stamp = transform.header.stamp
        point.point.x = 1.0
        point.point.y = 0.0
        point.point.z = 0.0
        converted = do_transform_point(point, transform)

        print(
            f"조회 성공: {transform.header.frame_id}"
            f" <- {transform.child_frame_id}"
        )
        print(f"map 기준 점의 z: {converted.point.z:.3f} m")
        self.done = True
        self.timer.cancel()


def main():
    parser = argparse.ArgumentParser()
    parser.add_argument("mode", choices=["broadcast", "query"])
    options, ros_args = parser.parse_known_args()
    rclpy.init(args=ros_args)
    node = None
    exit_code = 0

    try:
        if options.mode == "broadcast":
            node = DuriFrames()
            rclpy.spin(node)
        else:
            node = DuriQuery()
            while rclpy.ok() and not node.done:
                rclpy.spin_once(node, timeout_sec=0.2)
            exit_code = node.exit_code
    except KeyboardInterrupt:
        pass
    finally:
        if node is not None:
            node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()

    return exit_code


if __name__ == "__main__":
    raise SystemExit(main())

줄별 해설

frame_math.py의 planar_transform은 단위 행렬에서 시작한다. 단위 행렬은 회전과 이동을 모두 하지 않는 변환이므로 마지막 행을 별도로 채울 필요가 없다. c와 s를 계산하는 줄은 z축 회전에 사용할 코사인과 사인을 준비한다. 왼쪽 위 3×3 영역을 채우는 줄에서 첫 번째 열은 회전한 x축, 두 번째 열은 회전한 y축을 나타낸다.

transform[:3, 3]에 값을 넣는 줄은 자식 원점의 부모 기준 위치를 지정한다. 행 벡터를 사용하는 다른 자료와 섞으면 평행 이동을 마지막 행에 넣기 쉽다. 이 프로그램은 점을 열 벡터로 취급하고 행렬을 왼쪽에서 곱하므로 평행 이동이 마지막 열에 있어야 한다.

rigid_inverse의 rotation과 translation은 입력 행렬에서 회전과 이동을 분리한다. rotation.T를 넣는 줄은 회전을 되돌린다. 그다음 줄은 부모 기준 평행 이동을 자식 축 방향으로 다시 표현하고 부호를 바꾼다. 이 함수는 회전과 평행 이동으로 구성된 강체 변환을 전제로 하며, 확대·축소가 섞인 일반 행렬의 역행렬 함수는 아니다.

main에서 map_from_odom이라는 이름은 “odom에서 map으로 바꾸는 행렬”을 뜻한다. 두 행렬을 곱하는 줄의 순서를 바꾸면 이동 벡터도 다른 회전의 영향을 받는다. 이어지는 네 검사는 중간 좌표, 최종 좌표, 원래 점으로의 복원, 역변환과 원래 변환의 곱을 각각 확인한다. 부동소수점 계산이므로 정확한 등호 대신 허용 오차를 사용한다.

show_point는 표시할 때만 아주 작은 값을 0으로 바꾼다. 90도의 코사인을 컴퓨터로 계산하면 0에 가까운 작은 값이 남을 수 있고, 계산 경로에 따라 음의 0처럼 표시될 수도 있다. 출력 정리는 행렬이나 검증값 자체를 수정하지 않는다.

duri_frames.py의 make_transform은 변환의 양 끝 이름, 시각, 위치, 각도를 받아 메시지를 만든다. translation의 세 필드는 미터 단위다. rotation의 마지막 두 필드에는 절반 각도의 사인과 코사인을 넣는다. 메시지를 만들 때마다 모든 회전 성분을 채워 초기값에 의존하지 않도록 했다.

DuriFrames 생성자에서 두 브로드캐스터를 인스턴스 속성으로 보관한다. fixed를 발행하는 줄은 map 기준 odom 원점을 (2, 1, 0)에 놓는다. 발행 노드는 계속 살아 있으므로 뒤늦게 실행한 조회 노드도 정적 변환을 받을 수 있다. 정적 메시지에도 생성 시각을 넣지만, tf2는 이를 시간에 따라 변하지 않는 관계로 취급한다.

publish_pose는 타이머가 호출할 때마다 base_link의 x 위치를 0.1m 증가시킨다. 첫 호출의 위치는 (1, 0, 0)이고 방향은 항상 90도다. 북쪽을 바라본 채 지도 x축 방향으로 이동하는 시험 궤적이므로 바퀴형 로봇의 전진 운동을 모사한 코드는 아니다. 트리의 값이 갱신되는지 확인하기 위한 입력이며, 실제 두리에서는 이 자리에 측정하거나 추정한 자세를 넣는다.

이 시험 입력은 콜백 횟수에 따라 위치가 증가한다. 0.2초 주기에서 의도한 변화율은 초당 0.5m지만 실행 지연까지 반영한 운동 적분은 아니다. 변환에 넣는 시각은 그 호출에서 노드 시계로 얻는다. 실제 데이터에서는 자세가 유효한 측정 시각과 메시지 시각의 관계도 맞춰야 한다.

DuriQuery의 Buffer와 TransformListener는 생성 후 계속 유지한다. try_query는 아직 관계를 찾지 못하면 바로 반환한다. 50회 실패하면 실패 문구를 출력하고 종료 코드 1로 끝난다. 성공하면 base_link 앞쪽 1m의 점을 만들고 조회된 변환을 적용한다. do_transform_point를 직접 호출할 때는 점의 프레임과 변환의 원본 프레임이 맞는지 호출자가 확인해야 한다.

성공 출력은 프레임 방향과 변환된 점의 z값만 표시한다. x값은 발행 시작 후 몇 번째 변환을 받았는지에 따라 달라지므로 고정된 예상 출력에 넣지 않았다. 모든 변환이 수평 이동과 z축 회전뿐이어서 이 점의 z값은 항상 0이다. main은 일반 인수와 ROS 인수를 나누고, 조회 모드에서는 수신 콜백과 조회 타이머를 번갈아 처리하다가 결과를 얻으면 정리한다.

실행 결과

ROS가 없는 환경에서는 가상 환경을 만들고 NumPy 예제를 실행한다. 다음 설치 명령의 안내 출력은 Python과 pip 버전에 따라 달라진다.

python3 -m venv .venv
. .venv/bin/activate
python3 -m pip install numpy

아래 실행의 출력은 다음과 같다.

python3 frame_math.py
base_link: (1.000, 0.000, 0.000)
odom: (1.000, 1.000, 0.000)
map: (3.000, 2.000, 0.000)
복원한 base_link: (1.000, 0.000, 0.000)
검사: 통과

두 파일의 문법은 다음 명령으로 검사한다. 성공하면 출력이 없다. py_compile은 import된 ROS 패키지를 실행하지 않으므로 ROS가 없는 환경에서도 파일의 문법을 검사할 수 있다. 다만 이 검사는 패키지 설치 상태나 실제 통신 동작을 검증하지 않는다. 아래 결과는 코드에서 도출한 예상 출력이며 ROS 환경에서 수행한 실행 기록은 아니다.

python3 -W error -m py_compile frame_math.py duri_frames.py

ROS 실행에는 별도의 터미널 두 개를 사용한다. 각 터미널에서 Jazzy 설치 환경을 먼저 불러와야 한다. Linux의 일반적인 시스템 설치에서는 다음 경로를 사용한다. macOS나 소스 빌드에서는 자신의 설치 결과에 있는 setup 파일 경로로 바꾼다. 두 터미널은 같은 ROS 도메인을 사용하며, 이 실습에서는 다른 노드가 동일한 프레임 관계를 발행하지 않도록 한다.

source /opt/ros/jazzy/setup.zsh

bash를 사용한다면 setup.zsh 대신 setup.bash를 사용한다. NumPy 실습용 가상 환경은 ROS 실행 터미널에서 활성화할 필요가 없다. 첫 번째 터미널에서 다음 명령을 실행하면 프로그램 자체는 문구를 출력하지 않고 계속 변환을 발행한다.

python3 duri_frames.py broadcast

첫 번째 터미널을 유지한 채 두 번째 터미널에서 조회한다. 두 관계를 정상적으로 수신하면 다음 두 줄을 출력하고 종료한다.

python3 duri_frames.py query
조회 성공: map <- base_link
map 기준 점의 z: 0.000 m

조회 노드를 다시 실행해도 같은 두 줄을 얻는다. 움직이는 x 좌표까지 보고 싶다면 converted.point.x를 출력하도록 바꿀 수 있다. 수신한 동적 변환이 k번째 값이고 첫 값을 k=0으로 셀 때, 변환된 점은 (3 + 0.1k, 2, 0)이다. 시작 순서와 수신 시점에 따라 k가 달라진다. 실습을 마치면 발행 터미널에서 Ctrl+C로 종료한다.

실무에서 자주 틀리는 것

대상과 원본 프레임을 뒤집는다

base_link 기준 점을 map으로 바꾸려는데 다음처럼 호출하면 반대 방향의 변환을 얻는다. 변환 조회 자체는 성공할 수 있으므로 예외가 발생하는지만 확인해서는 발견하기 어렵다.

# 잘못된 방향
transform = self.buffer.lookup_transform(
    "base_link", "map", Time()
)

결과를 표현할 프레임을 첫 번째 인수에 둔다. 변수 이름도 map_from_base처럼 읽는 방향이 드러나게 정하면 점검하기 쉽다.

# base_link 좌표를 map 좌표로 바꾼다.
transform = self.buffer.lookup_transform(
    "map", "base_link", Time()
)

각도를 그대로 쿼터니언 필드에 넣는다

rotation.z는 z축 회전각을 저장하는 칸이 아니다. 다음 코드는 원하는 90도 회전을 표현하지 않으며 단위 쿼터니언 조건도 만족하지 않는다.

# 잘못된 회전 표현
transform.transform.rotation.z = math.pi / 2.0
transform.transform.rotation.w = 1.0

평면 회전에서는 절반 각도로 네 성분을 구성한다. 도 단위 숫자를 그대로 삼각함수에 넣지 않도록 각도 단위도 함께 확인한다.

yaw = math.radians(90.0)
transform.transform.rotation.x = 0.0
transform.transform.rotation.y = 0.0
transform.transform.rotation.z = math.sin(yaw / 2.0)
transform.transform.rotation.w = math.cos(yaw / 2.0)

변하는 위치를 정적으로 발행한다

현재 값이 잠시 고정되어 있다는 이유로 움직이는 로봇의 관계를 정적 브로드캐스터에 넣으면 관계의 시간적 의미가 달라진다. 정적 변환은 이동 경로의 시각별 기록을 대신하지 않는다.

# moving은 odom 기준 base_link의 현재 자세다.
# 움직일 수 있는 관계를 정적으로 발행하는 잘못된 예다.
self.static_broadcaster.sendTransform(moving)

현재 자세를 새로 얻을 때 동적 브로드캐스터로 발행한다. 같은 관계를 발행하던 정적 발행자도 제거해야 한다. 코드 한 줄만 바꾸고 다른 정적 발행 노드를 남겨 두면 책임 중복이 계속된다.

moving.header.stamp = self.get_clock().now().to_msg()
self.dynamic_broadcaster.sendTransform(moving)

점에서 이동값만 빼서 역변환한다

지도 좌표에서 평행 이동만 빼면 지도 축으로 표현된 벡터가 남는다. 로봇이 회전한 상황에서는 이것이 base_link 기준 좌표가 아니다.

# point_map과 transform은 NumPy 배열이다.
# 잘못된 역변환
point_base_xyz = point_map[:3] - transform[:3, 3]

이동을 뺀 결과를 회전의 역방향으로 돌려야 한다. 완성 코드의 rigid_inverse도 같은 식을 행렬 형태로 작성한 것이다.

rotation = transform[:3, :3]
translation = transform[:3, 3]
point_base_xyz = rotation.T @ (point_map[:3] - translation)

한눈에 보기

프레임을 연결하고 좌표를 변환할 때 확인할 기준

항목	기억할 내용	두리 예제
트리 구조	자식은 한 부모에 연결하고 순환을 만들지 않는다	map → odom → base_link
관계의 책임	한 연결의 발행 책임자를 하나로 정한다	odom과 base_link를 중복 발행하지 않는다
메시지 의미	부모 기준으로 자식 원점과 자세를 표현한다	header.frame_id가 부모다
조회 순서	대상, 원본, 시각 순서다	map, base_link, Time()
점 변환	회전 후 평행 이동한다	R @ p + t
합성 순서	오른쪽 변환부터 적용한다	T_map_odom @ T_odom_base
쿼터니언	x, y, z, w를 사용한다	회전 없음은 (0, 0, 0, 1)이다
조회 준비	수신 콜백이 버퍼를 채울 시간을 준다	실패하면 다음 타이머에서 재시도한다

tf2는 프레임 사이의 관계를 관리하며 로봇의 위치 자체를 측정하지는 않는다. 입력한 위치가 잘못되면 그 위치를 바탕으로 일관된 변환을 계산할 뿐이다. 두리의 표시 위치가 어긋날 때는 측정값의 정확도, 프레임 연결, 조회 방향을 구분해서 살펴보아야 한다.

연습 문제

1. NumPy 예제에서 odom 기준 base_link의 위치는 그대로 두고 회전각만 0으로 바꾼다. base_link 앞쪽 1m 점의 odom 좌표와 map 좌표를 계산한다. 기존 검사에서 바꿔야 할 기대값도 적는다.
2. 원래 예제의 map 기준 점 (3, 2, 0)을 base_link로 되돌린다. 이동값만 뺐을 때 얻는 값과 회전까지 되돌렸을 때 얻는 값을 각각 구한다.
3. 두리가 지도 기반 위치추정을 시작하면서 map과 odom의 관계를 새 노드가 계속 발행한다. 실습용 DuriFrames에서 제거해야 할 부분과 그대로 둘 수 있는 부분을 설명한다.
4. 발행 노드보다 조회 노드를 먼저 실행하면 최초 조회가 실패할 수 있는 이유를 설명한다. 완성 코드가 이 상황을 어떻게 처리하며, 재시도 횟수가 50회라는 사실이 정확히 5초의 종료 시간을 보장하는지도 설명한다.

정답과 해설

1. 회전이 없으므로 로봇 앞쪽은 odom의 x축과 같은 방향이다. base_link 원점 (1, 0, 0)에 점 (1, 0, 0)을 더하면 odom 좌표는 (2, 0, 0)이다. map 기준 odom의 이동량 (2, 1, 0)을 더하면 map 좌표는 (4, 1, 0)이다. point_odom 검사의 기대값을 [2.0, 0.0, 0.0, 1.0]으로, point_map 검사의 기대값을 [4.0, 1.0, 0.0, 1.0]으로 바꾼다. 복원과 단위 행렬 검사는 그대로 성립한다.

2. map 기준 base_link 원점은 (3, 1, 0)이다. 지도 점에서 이 원점을 빼면 (0, 1, 0)이지만, 아직 지도 축을 기준으로 표현된 값이다. 이를 z축 주위로 -90도 회전하면 (1, 0, 0)이 된다. 이 결과가 로봇 앞쪽 1m라는 원래 의미와 일치한다.

3. map과 odom의 관계를 만드는 fixed와 이를 보내는 sendTransform 호출을 제거한다. 다른 정적 관계를 발행하지 않는다면 정적 브로드캐스터 생성도 제거할 수 있다. odom과 base_link의 동적 발행은 그 관계의 책임자가 계속 이 노드라면 유지할 수 있다. 실제 이동량 추정 노드가 그 관계를 맡는다면 실습용 동적 발행도 함께 중단해야 한다. 어느 연결이 정적인지는 이름이 아니라 관계가 시간에 따라 변하는지로 결정한다.

4. 조회 노드의 버퍼는 시작할 때 비어 있고, 발행 데이터가 도착해도 수신 콜백이 실행되어야 채워진다. 완성 코드는 조회 예외를 잡아 반환하고 다음 타이머 호출에서 다시 시도한다. 50회까지 성공하지 못하면 실패로 종료한다. 주기가 0.1초여도 운영체제와 실행기의 지연이 있으므로 정확히 5초의 종료를 보장하지는 않는다. 이 예제의 제한은 시도 횟수이며 엄밀한 경과 시간 제한은 아니다.