[Physical AI W2D1] 6/6 — 좌표 변환 실습: Python 변환 + TF Publisher + RViz

[Physical AI W2D1 · 6/6]

4·5편의 동차 변환 행렬을 손으로 굳힌다. NumPy로 2D·3D 변환과 역변환, 좌표계 체인을 직접 계산해 결과를 눈으로 확인하고, TF Publisher 노드를 만들어 base_link→laser·camera_link, odom→base_link를 발행한 뒤 tf2_echo와 RViz로 좌표축을 검증한다.

이 글에서 직접 해보는 것
1. NumPy로 2D 동차 변환·역변환, 3D 변환, 좌표계 체인 계산
2. 계산 결과를 4·5편의 손계산과 대조(앞 1m → odom (2,2), laser 1m → base 1.25·0.15)
3. TF Publisher 노드(coordinate_tf_publisher.py) — RPY→Quaternion + static/dynamic TF
4. tf2_echo·RViz로 odom→base_link→{laser,camera_link} 검증

(Week2 Day1의 마지막 편. 이론(4·5편)을 코드로 굳히고, 1~3편의 RViz로 다시 확인합니다.)

---

1. 환경 & 작업공간

source /opt/ros/humble/setup.bash
ros2 topic list                 # ROS 2 동작 확인

mkdir -p ~/transform_ws/src && cd ~/transform_ws
mkdir -p ~/transform_ws/matrix_examples && cd ~/transform_ws/matrix_examples

먼저 순수 Python(NumPy) 으로 변환을 계산해 직관을 잡고, 그다음 ROS 2 TF로 발행합니다.

---

2. 2D 동차 변환 — 직접 계산

nano ~/transform_ws/matrix_examples/transform_2d_demo.py

import math
import numpy as np


def make_transform_2d(x, y, yaw_deg):
    yaw = math.radians(yaw_deg)
    cos_yaw, sin_yaw = math.cos(yaw), math.sin(yaw)
    return np.array([
        [cos_yaw, -sin_yaw, x],
        [sin_yaw,  cos_yaw, y],
        [0.0,      0.0,     1.0],
    ])


def main():
    T_odom_base = make_transform_2d(x=2.0, y=1.0, yaw_deg=90.0)
    print("T_odom_base\n", T_odom_base)

    p_base = np.array([[1.0], [0.0], [1.0]])     # 로봇 기준 앞 1m
    p_odom = T_odom_base @ p_base                # @ = 행렬 곱
    print("p_odom = T_odom_base @ p_base\n", p_odom)


if __name__ == "__main__":
    main()

python3 transform_2d_demo.py

T_odom_base
 [[ 0. -1.  2.]
  [ 1.  0.  1.]
  [ 0.  0.  1.]]
p_odom = T_odom_base @ p_base
 [[2.]
  [2.]
  [1.]]

→ 4편 손계산과 동일: 로봇 기준 앞 1m 점이 odom 기준 (2, 2). 로봇이 90도 돌았으니 "앞쪽"이 odom의 y 방향이 됐습니다.

---

3. 2D 역변환 — 지도 목표를 로봇 기준으로

nano ~/transform_ws/matrix_examples/inverse_transform_2d_demo.py

import math
import numpy as np


def make_transform_2d(x, y, yaw_deg):
    yaw = math.radians(yaw_deg)
    cos_yaw, sin_yaw = math.cos(yaw), math.sin(yaw)
    return np.array([
        [cos_yaw, -sin_yaw, x],
        [sin_yaw,  cos_yaw, y],
        [0.0,      0.0,     1.0],
    ])


def main():
    T_odom_base = make_transform_2d(x=3.0, y=2.0, yaw_deg=90.0)
    T_base_odom = np.linalg.inv(T_odom_base)     # 역행렬
    print("T_base_odom\n", T_base_odom)

    p_odom = np.array([[5.0], [2.0], [1.0]])     # 지도(odom) 기준 목표점
    p_base = T_base_odom @ p_odom
    print("p_base = T_base_odom @ p_odom\n", p_base)


if __name__ == "__main__":
    main()

python3 inverse_transform_2d_demo.py
# p_base = [[ 0.], [-2.], [ 1.]]

→ 로봇 위치 (3,2)·yaw 90°에서 본 지도 목표 (5,2)는 로봇 기준 (0, -2) = 로봇 오른쪽 2m. 지도 좌표를 로봇 기준으로 바꿔야 "정면인지 왼쪽인지 오른쪽인지" 알 수 있습니다(역변환의 실전 용도 — 5편).

---

4. 3D 동차 변환

nano ~/transform_ws/matrix_examples/transform_3d_demo.py

import numpy as np


def make_transform_3d(rotation_matrix, translation_vector):
    T = np.eye(4)
    T[0:3, 0:3] = rotation_matrix      # 회전 R (좌상단 3x3)
    T[0:3, 3:4] = translation_vector   # 이동 t (우측 3x1)
    return T


def main():
    R_base_laser = np.eye(3)                          # 회전 없음
    t_base_laser = np.array([[0.25], [0.00], [0.15]]) # 앞 0.25, 위 0.15
    T_base_laser = make_transform_3d(R_base_laser, t_base_laser)
    print("T_base_laser\n", T_base_laser)

    p_laser = np.array([[1.0], [0.0], [0.0], [1.0]])  # laser 기준 앞 1m
    p_base = T_base_laser @ p_laser
    print("p_base = T_base_laser @ p_laser\n", p_base)


if __name__ == "__main__":
    main()

python3 transform_3d_demo.py
# p_base = [[1.25], [0.], [0.15], [1.]]

→ 5편 예시와 일치: LiDAR 기준 앞 1m가 본체 기준 x=1.25, z=0.15. (3편 Publisher의 static TF 값과도 동일!)

---

5. 좌표계 체인 — laser → base_link → odom

nano ~/transform_ws/matrix_examples/transform_chain_demo.py

import math
import numpy as np


def make_transform_2d_as_3d(x, y, yaw_deg):
    yaw = math.radians(yaw_deg)
    c, s = math.cos(yaw), math.sin(yaw)
    return np.array([
        [c, -s, 0.0, x],
        [s,  c, 0.0, y],
        [0.0, 0.0, 1.0, 0.0],
        [0.0, 0.0, 0.0, 1.0],
    ])


def make_transform_3d(R, t):
    T = np.eye(4)
    T[0:3, 0:3] = R
    T[0:3, 3:4] = t
    return T


def main():
    T_odom_base = make_transform_2d_as_3d(x=2.0, y=1.0, yaw_deg=90.0)
    T_base_laser = make_transform_3d(np.eye(3), np.array([[0.25], [0.0], [0.15]]))
    p_laser = np.array([[1.0], [0.0], [0.0], [1.0]])

    # 방법 1 — 단계별
    p_base = T_base_laser @ p_laser
    p_odom = T_odom_base @ p_base

    # 방법 2 — 변환을 먼저 합성(T_odom_laser)
    T_odom_laser = T_odom_base @ T_base_laser
    p_odom_direct = T_odom_laser @ p_laser

    print("p_odom\n", p_odom)
    print("p_odom_direct\n", p_odom_direct)   # 둘이 같다


if __name__ == "__main__":
    main()

python3 transform_chain_demo.py

핵심 확인:

p_odom = T_odom_base @ T_base_laser @ p_laser     (laser → base_link → odom)

단계별 계산과 변환 합성(T_odom_laser) 후 한 번에가 같은 결과를 줍니다 — 4편의 "행렬 곱 순서(체인)"가 코드로 증명됩니다. (순서를 바꾸면 좌표계 연결이 안 맞아 결과가 틀립니다.)

---

6. TF Publisher 노드 — 계산을 ROS 2 TF로

이제 위 좌표계 관계를 ROS 2 TF로 발행합니다(static base_link→laser·camera_link, dynamic odom→base_link). RPY를 Quaternion으로 바꾸는 함수도 직접 구현합니다(5편).

cd ~/transform_ws/src
ros2 pkg create transform_matrix_demo --build-type ament_python \
  --dependencies rclpy geometry_msgs tf2_ros
cd ~/transform_ws/src/transform_matrix_demo/transform_matrix_demo
nano coordinate_tf_publisher.py

import math

import rclpy
from rclpy.node import Node

from geometry_msgs.msg import TransformStamped
from tf2_ros import TransformBroadcaster, StaticTransformBroadcaster


class CoordinateTFPublisher(Node):
    def __init__(self):
        super().__init__('coordinate_tf_publisher')
        self.dynamic_broadcaster = TransformBroadcaster(self)
        self.static_broadcaster = StaticTransformBroadcaster(self)
        self.t = 0.0
        self.publish_static_transforms()
        self.timer = self.create_timer(0.1, self.publish_dynamic_transform)
        self.get_logger().info('Coordinate TF publisher started')

    def publish_static_transforms(self):
        now = self.get_clock().now().to_msg()

        base_to_laser = TransformStamped()
        base_to_laser.header.stamp = now
        base_to_laser.header.frame_id = 'base_link'
        base_to_laser.child_frame_id = 'laser'
        base_to_laser.transform.translation.x = 0.25
        base_to_laser.transform.translation.z = 0.15
        base_to_laser.transform.rotation.w = 1.0          # 회전 없음

        base_to_camera = TransformStamped()
        base_to_camera.header.stamp = now
        base_to_camera.header.frame_id = 'base_link'
        base_to_camera.child_frame_id = 'camera_link'
        base_to_camera.transform.translation.x = 0.30
        base_to_camera.transform.translation.z = 0.40
        qx, qy, qz, qw = self.rpy_to_quaternion(0.0, math.radians(-20.0), 0.0)  # 아래로 20도
        base_to_camera.transform.rotation.x = qx
        base_to_camera.transform.rotation.y = qy
        base_to_camera.transform.rotation.z = qz
        base_to_camera.transform.rotation.w = qw

        self.static_broadcaster.sendTransform([base_to_laser, base_to_camera])

    def publish_dynamic_transform(self):
        now = self.get_clock().now().to_msg()
        x = 2.0 * math.cos(self.t)
        y = 2.0 * math.sin(self.t)
        yaw = self.t + math.pi / 2.0

        odom_to_base = TransformStamped()
        odom_to_base.header.stamp = now
        odom_to_base.header.frame_id = 'odom'
        odom_to_base.child_frame_id = 'base_link'
        odom_to_base.transform.translation.x = x
        odom_to_base.transform.translation.y = y
        qx, qy, qz, qw = self.rpy_to_quaternion(0.0, 0.0, yaw)
        odom_to_base.transform.rotation.x = qx
        odom_to_base.transform.rotation.y = qy
        odom_to_base.transform.rotation.z = qz
        odom_to_base.transform.rotation.w = qw

        self.dynamic_broadcaster.sendTransform(odom_to_base)
        self.t += 0.03

    def rpy_to_quaternion(self, roll, pitch, yaw):
        cy, sy = math.cos(yaw * 0.5), math.sin(yaw * 0.5)
        cp, sp = math.cos(pitch * 0.5), math.sin(pitch * 0.5)
        cr, sr = math.cos(roll * 0.5), math.sin(roll * 0.5)
        qw = cr * cp * cy + sr * sp * sy
        qx = sr * cp * cy - cr * sp * sy
        qy = cr * sp * cy + sr * cp * sy
        qz = cr * cp * sy - sr * sp * cy
        return qx, qy, qz, qw


def main(args=None):
    rclpy.init(args=args)
    node = CoordinateTFPublisher()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

• publish_static_transforms() → base_link→laser(앞 0.25·위 0.15, 회전 없음) + base_link→camera_link(앞 0.30·위 0.40, 아래로 20도 pitch).
• publish_dynamic_transform() → odom→base_link(원형으로 이동 + yaw 회전).
• rpy_to_quaternion() → Roll·Pitch·Yaw를 Quaternion으로(5편 공식의 일반형).

등록·빌드·실행:

entry_points={
    'console_scripts': [
        'coordinate_tf_publisher = transform_matrix_demo.coordinate_tf_publisher:main',
    ],
},

cd ~/transform_ws && source /opt/ros/humble/setup.bash
colcon build --packages-select transform_matrix_demo
source install/setup.bash
ros2 run transform_matrix_demo coordinate_tf_publisher

---

7. tf2_echo · RViz로 검증

새 터미널에서 변환을 숫자로 확인:

source /opt/ros/humble/setup.bash && cd ~/transform_ws && source install/setup.bash

ros2 topic echo /tf --once          # odom → base_link (dynamic)
ros2 topic echo /tf_static --once   # base_link → laser, base_link → camera_link

ros2 run tf2_ros tf2_echo odom base_link       # 로봇이 돌며 값 변함
ros2 run tf2_ros tf2_echo base_link laser
# - Translation: [0.250, 0.000, 0.150]   ← 우리가 넣은 값 그대로
ros2 run tf2_ros tf2_echo base_link camera_link
# pitch -20도가 Quaternion으로 반영되어 출력

그다음 RViz2(3편의 헤드리스 스택)로 띄워 Fixed Frame = odom + TF Display를 추가하면 — odom을 기준으로 base_link가 원을 그리며 돌고, 그 위에 laser·camera_link 축이 붙어 함께 움직이는 게 보입니다. 4·5편에서 계산한 좌표축이 화면에 그대로 나타나는 순간입니다(설정은 .rviz로 저장해 재사용).

---

6편 & Day2 마무리

• NumPy로 2D·3D 동차 변환·역변환·체인을 계산 → 4·5편 손계산과 정확히 일치.
• T_odom_base @ T_base_laser @ p_laser = 단계별과 합성이 같은 결과(체인·곱셈 순서).
• TF Publisher 노드로 계산을 ROS 2 TF(static/dynamic)로 발행, rpy_to_quaternion 직접 구현.
• tf2_echo로 숫자, RViz로 그림 — 변환 행렬 = TF 축임을 양쪽으로 확인.

Week2 Day1 전체를 돌아보면

1·2편에서 RViz가 ROS 2 데이터를 좌표계 기준으로 그린다는 개념을, 3편에서 GCP 헤드리스 RViz로 직접, 4·5편에서 그 좌표축의 수학인 동차 변환 행렬·TF·Quaternion·기구학을, 6편에서 코드로 굳혔습니다. 이 좌표 변환은 앞으로 SLAM·내비게이션·로봇팔·강화학습까지 모든 로봇 동작의 토대가 됩니다.

📚 Week2 Day1 전체 목차 (총 6편)

• 1/6 RViz로 로봇을 눈으로 보다 — 시각화 흐름·Fixed Frame·Display
• 2/6 RViz 개념 ② — TF·좌표계·URDF·센서 Display·디버깅
• 3/6 RViz2 실습 — GCP VM 헤드리스 GUI + Publisher + 시각화
• 4/6 좌표계와 동차 변환 행렬 ① — 좌표계·2D 변환·동차좌표
• 5/6 좌표계와 동차 변환 행렬 ② — 3D·TF·Quaternion·기구학
• 6/6 좌표 변환 실습 — Python 변환 + TF Publisher + RViz — 이번 글