2.4tf2 변환 노드 작성

1.9장에서 로봇의 부품마다 프레임(좌표계)이 있고, 프레임들이 부모와 자식으로 이어져 트리를 이룬다고 배웠습니다. 이 트리를 누가 만들까요. 노드들이 /tf 토픽으로 "자식 프레임은 부모 프레임에서 이만큼 떨어져 있다"를 발행하고, 그것을 모은 쪽이 트리를 짭니다. 이 장은 발행하는 노드와 조회하는 노드를 직접 씁니다.

변환 메시지 읽기

변환 하나는 geometry_msgs/TransformStamped입니다. 필드는 다섯 묶음입니다.

방향을 헷갈리기 쉽습니다. header.frame_id는 부모, child_frame_id는 자식이고 값은 "부모 좌표계에서 본 자식의 자세"입니다. 예를 들어 base_link에서 앞쪽 0.1 m, 위쪽 0.2 m에 라이다가 달려 있다면 부모는 base_link, 자식은 laser, 이동은 (0.1, 0, 0.2)입니다.

회전은 쿼터니언으로 적습니다. 바닥에서 도는 로봇은 z축 회전(yaw)만 쓰므로 (0, 0, sin(yaw/2), cos(yaw/2))로 만듭니다. 회전이 없는 상태는 w = 1, 나머지 0입니다. Humble에는 오일러 각을 쿼터니언으로 바꿔 주는 tf_transformations가 기본으로 들어 있지 않아서 아래 코드는 math로 직접 계산합니다.

고정된 변환과 움직이는 변환

변환에는 두 종류가 있습니다.

고정된 변환은 /tf_static 토픽으로 한 번 보내면 나중에 켠 노드도 받습니다. 움직이는 변환은 시간이 지나면 낡은 값이 되므로 주기적으로 갱신해야 합니다.

발행하고 조회하기

아래 코드는 노드 둘로 이루어져 있습니다.

• 발행 노드는 base_link의 위치를 odom 기준으로 10 Hz로 갱신합니다. 앞으로 0.4 m/s로 가면서 초당 0.5 rad씩 회전합니다. laser는 base_link 앞 0.1 m, 위 0.2 m에 고정해 한 번만 발행합니다.
• 조회 노드는 1초마다 odom 기준으로 본 laser의 위치와 방향을 조회해 출력합니다.

lookup_transform(목표 프레임, 원본 프레임, 시각)의 순서에 주의하세요. 결과는 "목표 프레임에서 본 원본 프레임의 자세"입니다. lookup_transform('odom', 'laser', Time())은 odom 좌표계에서 laser가 어디에 어느 방향으로 있는지 알려 줍니다. Time()은 "가장 최근 값"을 뜻합니다.

조회가 실패하는 경우

TransformException을 상속한 예외가 몇 가지 있습니다.

rclpy에서는 조회 직후가 아니라 시작한 뒤 한동안 LookupException이 나는 것이 정상입니다. 예외를 잡아서 건너뛰고 다음 주기에 다시 시도하도록 짜세요. 에뮬레이터는 시각을 무시하므로 ExtrapolationException을 내지 않습니다.

센서의 점을 다른 좌표계로 옮기기

변환을 조회하는 가장 흔한 이유는 센서가 잰 점을 다른 프레임 좌표로 옮기기 위해서입니다. 라이다가 "내 앞 1 m에 물체가 있다"고 알려 주면, 그 물체가 odom(또는 map) 기준으로 어디인지 알아야 지도에 그리거나 피해 갈 수 있습니다.

점 (x, y)를 변환 t로 옮기는 식은 평면에서 다음과 같습니다. 회전각을 yaw라고 하면 새 좌표는 이동에 회전한 점을 더한 값입니다.

x' = tx + x*cos(yaw) - y*sin(yaw)
y' = ty + x*sin(yaw) + y*cos(yaw)

아래 코드에서는 상황을 일부러 헷갈리게 만들었습니다. 라이다를 base_link에서 뒤쪽으로 돌려(yaw 180도) 달았고, 로봇은 odom의 (2, 1)에서 +y 방향(yaw 90도)을 봅니다. 라이다는 자기 앞(laser 프레임의 +x) 1 m에서 물체를 봤습니다. 물체는 odom 기준으로 어디일까요?

이런 계산을 직접 하지 않아도 되도록 실제 ROS2에는 tf2_geometry_msgs 패키지가 PoseStamped, PointStamped 같은 메시지를 변환해 주는 함수를 제공합니다. 원리는 위의 식과 같으니, 라이브러리를 쓸 때도 어느 프레임에서 어느 프레임으로 가는 변환인지 방향을 먼저 따져야 합니다.

실제 환경에서 확인하기

노드 코드가 맞게 짜였는지는 터미널 도구로 확인합니다. 이 명령들은 3부에서 VM을 만든 뒤 실행하고, RViz에서 같은 트리를 보는 방법은 3.8장에서 다룹니다.

# 두 프레임 사이의 변환을 계속 출력 (목표 프레임, 원본 프레임 순서)
ros2 run tf2_ros tf2_echo odom laser

# 지금 발행되는 프레임 트리를 그림 파일로 저장
ros2 run tf2_tools view_frames

tf2_echo의 인자 순서도 lookup_transform과 같습니다. 목표 프레임이 먼저입니다. view_frames는 트리의 부모-자식 관계와 각 변환의 발행 주기를 PDF로 만들어 줍니다. 끊긴 곳, 중복 발행, 주기가 너무 느린 변환을 찾는 데 씁니다.

TIKI-MINI의 실제 프레임 트리는 다음 장 2.5장에서 URDF로 읽습니다. URDF에 적힌 고정 변환(base_link, laser 등)은 robot_state_publisher가 URDF를 읽어 발행합니다. 같은 변환을 StaticTransformBroadcaster로 또 발행하면 중복이 되므로 이미 URDF에 있는 센서 위치는 다시 발행하지 않습니다.

막혔을 때

한눈에 정리

1. 변환은 TransformStamped이고 header.frame_id가 부모, child_frame_id가 자식, 값은 부모에서 본 자식의 자세이다.
2. 움직이는 변환은 TransformBroadcaster로 주기적으로, 고정된 변환은 StaticTransformBroadcaster로 한 번 발행한다.
3. 조회는 Buffer와 TransformListener를 만들고 lookup_transform(목표, 원본, 시각)을 부른다. 결과는 목표 프레임에서 본 원본 프레임의 자세이다.
4. 조회 직후의 LookupException은 정상이므로 try/except로 받아 다음 주기에 다시 시도한다.
5. 센서의 점은 조회한 변환의 이동과 회전으로 다른 프레임 좌표로 옮긴다.

확인 문제

다음 장

2.5장에서는 TIKI-MINI의 URDF를 읽으며 이 장에서 만든 프레임 트리가 실제 로봇에서는 어떤 모양인지 확인합니다.