659.34 lookupTransform()을 이용한 변환 조회

1. 개요

lookupTransform()은 TF2 라이브러리에서 가장 핵심적인 함수로, 두 좌표 프레임 사이의 기하학적 변환(geometric transformation)을 조회하는 데 사용한다. 이 함수는 tf2::Buffer 객체에 저장된 변환 트리(transform tree)를 탐색하여, 지정된 소스 프레임(source frame)에서 타깃 프레임(target frame)으로의 변환 정보를 geometry_msgs::msg::TransformStamped 메시지 형태로 반환한다. 로봇 시스템에서 센서 데이터의 좌표 변환, 로봇 팔의 엔드 이펙터(end-effector) 위치 계산, 자율 주행 로봇의 전역 위치 추정 등 거의 모든 공간적 연산에 lookupTransform()이 활용된다.

2. lookupTransform()의 기본 원리

2.1 변환 트리 탐색 메커니즘

TF2는 모든 좌표 프레임 간의 관계를 방향성 비순환 트리(directed acyclic tree) 구조로 관리한다. lookupTransform()이 호출되면, 내부적으로 다음과 같은 절차를 따른다.

1. 소스 프레임에서 루트로의 경로 탐색: 소스 프레임에서 변환 트리의 루트 프레임까지의 경로를 역추적한다.
2. 타깃 프레임에서 루트로의 경로 탐색: 타깃 프레임에서 루트 프레임까지의 경로를 역추적한다.
3. 최소 공통 조상 (Lowest Common Ancestor, LCA) 식별: 두 경로가 교차하는 가장 가까운 공통 조상 프레임을 찾는다.
4. 변환 합성 (Composition): 소스 프레임에서 LCA까지의 변환과 LCA에서 타깃 프레임까지의 변환을 합성하여 최종 변환을 계산한다.

이러한 탐색 과정은 변환 트리의 깊이에 비례하는 시간 복잡도 O(d)를 가지며, d는 트리의 최대 깊이를 나타낸다.

2.2 변환 방향의 의미

lookupTransform(target_frame, source_frame, time)에서 반환되는 변환 T는 소스 프레임에서 표현된 점 \mathbf{p}_{\text{source}}를 타깃 프레임에서의 표현 \mathbf{p}_{\text{target}}으로 변환하는 관계를 나타낸다.

\mathbf{p}_{\text{target}} = T \cdot \mathbf{p}_{\text{source}}

여기서 T는 동차 변환 행렬(homogeneous transformation matrix)로 표현할 수 있으며, 회전 성분 R과 병진 성분 \mathbf{t}로 구성된다.

T = \begin{bmatrix} R & \mathbf{t} \\ \mathbf{0}^T & 1 \end{bmatrix}

주의할 점은, lookupTransform()의 첫 번째 인자가 타깃 프레임이고 두 번째 인자가 소스 프레임이라는 것이다. 이는 “타깃 프레임 기준에서 소스 프레임이 어디에 있는가“를 질의하는 것과 동일하다.

3. rclcpp에서의 lookupTransform() 사용법

3.1 기본 사용 패턴

rclcpp(C++ 클라이언트 라이브러리)에서 lookupTransform()을 사용하려면, 먼저 tf2_ros::Buffer와 tf2_ros::TransformListener를 초기화해야 한다.

#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <rclcpp/rclcpp.hpp>

class FrameLookupNode : public rclcpp::Node
{
public:
    FrameLookupNode()
    : Node("frame_lookup_node")
    {
        tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
        tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);

        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(100),
            std::bind(&FrameLookupNode::lookupCallback, this));
    }

private:
    void lookupCallback()
    {
        try {
            geometry_msgs::msg::TransformStamped t =
                tf_buffer_->lookupTransform(
                    "map",           // 타깃 프레임
                    "base_link",     // 소스 프레임
                    tf2::TimePointZero);  // 최신 가용 시간

            RCLCPP_INFO(this->get_logger(),
                "Translation: [%.3f, %.3f, %.3f]",
                t.transform.translation.x,
                t.transform.translation.y,
                t.transform.translation.z);

            RCLCPP_INFO(this->get_logger(),
                "Rotation: [%.3f, %.3f, %.3f, %.3f]",
                t.transform.rotation.x,
                t.transform.rotation.y,
                t.transform.rotation.z,
                t.transform.rotation.w);

        } catch (const tf2::TransformException &ex) {
            RCLCPP_WARN(this->get_logger(),
                "Transform lookup failed: %s", ex.what());
        }
    }

    std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
    std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
    rclcpp::TimerBase::SharedPtr timer_;
};

3.2 특정 시간에서의 변환 조회

특정 시점의 변환이 필요한 경우, tf2::TimePoint 또는 rclcpp::Time을 사용하여 시간을 지정한다.

// 현재 ROS 시간에서의 변환 조회
rclcpp::Time now = this->get_clock()->now();
auto t = tf_buffer_->lookupTransform(
    "map", "base_link", now);

// 특정 시점의 변환 조회
rclcpp::Time query_time(1234567890, 0, RCL_ROS_TIME);
auto t_past = tf_buffer_->lookupTransform(
    "map", "base_link", query_time);

3.3 타임아웃을 지정한 변환 조회

변환이 아직 수신되지 않은 경우를 대비하여, 타임아웃(timeout)을 지정하여 변환이 가용해질 때까지 대기할 수 있다.

// 최대 1초까지 변환 대기
auto t = tf_buffer_->lookupTransform(
    "map", "base_link",
    tf2::TimePointZero,
    tf2::durationFromSec(1.0));

타임아웃 기간 내에 변환이 가용하지 않을 경우, tf2::LookupException 또는 tf2::ConnectivityException이 발생한다. 타임아웃을 사용할 때에는 해당 호출이 블로킹(blocking) 호출이 되므로, 실시간 제어 루프에서는 주의하여 사용해야 한다.

4. rclpy에서의 lookupTransform() 사용법

4.1 기본 사용 패턴

rclpy(Python 클라이언트 라이브러리)에서도 동일한 논리 구조를 따르되, Python의 문법에 맞추어 사용한다.

import rclpy
from rclpy.node import Node
from tf2_ros import Buffer, TransformListener, TransformException
from rclpy.duration import Duration
from rclpy.time import Time


class FrameLookupNode(Node):
    def __init__(self):
        super().__init__('frame_lookup_node')

        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)

        self.timer = self.create_timer(0.1, self.lookup_callback)

    def lookup_callback(self):
        try:
            t = self.tf_buffer.lookup_transform(
                'map',           # 타깃 프레임
                'base_link',     # 소스 프레임
                Time())          # 최신 가용 시간 (Time()은 시간 0)

            self.get_logger().info(
                f'Translation: ['
                f'{t.transform.translation.x:.3f}, '
                f'{t.transform.translation.y:.3f}, '
                f'{t.transform.translation.z:.3f}]')

            self.get_logger().info(
                f'Rotation: ['
                f'{t.transform.rotation.x:.3f}, '
                f'{t.transform.rotation.y:.3f}, '
                f'{t.transform.rotation.z:.3f}, '
                f'{t.transform.rotation.w:.3f}]')

        except TransformException as ex:
            self.get_logger().warn(
                f'Transform lookup failed: {ex}')


def main(args=None):
    rclpy.init(args=args)
    node = FrameLookupNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

4.2 타임아웃을 지정한 변환 조회 (rclpy)

from rclpy.duration import Duration

try:
    t = self.tf_buffer.lookup_transform(
        'map',
        'base_link',
        Time(),
        timeout=Duration(seconds=1.0))
except TransformException as ex:
    self.get_logger().warn(f'Transform lookup failed: {ex}')

5. lookupTransform()의 내부 동작 상세

5.1 시간 보간 (Time Interpolation)

lookupTransform()에 지정된 시간이 캐시에 저장된 두 변환 사이의 시간에 해당할 경우, TF2는 자동으로 보간(interpolation)을 수행한다.

• 병진 성분: 선형 보간(Linear Interpolation, LERP)을 적용한다.
• 회전 성분: 구면 선형 보간(Spherical Linear Interpolation, SLERP)을 적용한다.

보간의 수학적 표현은 다음과 같다. 시간 t_0와 t_1 사이의 시간 t에 대하여, 보간 비율 \alpha를 다음과 같이 정의한다.

\alpha = \frac{t - t_0}{t_1 - t_0}

병진 벡터 \mathbf{p}의 보간:

\mathbf{p}(t) = (1 - \alpha)\mathbf{p}(t_0) + \alpha \mathbf{p}(t_1)

쿼터니언 \mathbf{q}의 SLERP 보간:

\mathbf{q}(t) = \frac{\sin((1-\alpha)\theta)}{\sin\theta}\mathbf{q}(t_0) + \frac{\sin(\alpha\theta)}{\sin\theta}\mathbf{q}(t_1)

여기서 \theta = \cos^{-1}(\mathbf{q}(t_0) \cdot \mathbf{q}(t_1))이다.

5.2 변환 체인 합성

두 프레임 사이에 여러 중간 프레임이 존재할 경우, lookupTransform()은 경로상의 모든 변환을 순차적으로 합성한다. 예를 들어, map → odom → base_link → camera_link의 경로가 있을 때, lookupTransform("map", "camera_link", time)의 결과는 다음과 같이 계산된다.

T_{\text{map}}^{\text{camera}} = T_{\text{map}}^{\text{odom}} \cdot T_{\text{odom}}^{\text{base\_link}} \cdot T_{\text{base\_link}}^{\text{camera}}

이 합성 과정에서 각 변환의 시간 보간이 독립적으로 수행되므로, 서로 다른 주파수로 발행되는 변환들 사이에서도 정확한 시간 동기화가 보장된다.

6. 시간 파라미터의 의미와 사용 전략

6.1 tf2::TimePointZero (시간 0)

tf2::TimePointZero 또는 rclpy에서 Time()을 사용하면, 버퍼에 저장된 최신 가용 변환(latest available transform)을 조회한다. 이 방법은 가장 일반적으로 사용되며, 특히 다음과 같은 상황에서 유용하다.

• 실시간 제어 루프에서 최신 로봇 상태를 조회할 때
• 변환 발행 시간이 일정하지 않은 경우
• 변환 지연(latency)이 무시할 수 있는 수준일 때

6.2 특정 시간 지정

센서 데이터의 타임스탬프와 정확하게 일치하는 변환이 필요한 경우, 해당 센서 데이터의 header.stamp를 시간 파라미터로 전달한다.

// 센서 메시지의 타임스탬프를 사용한 변환 조회
auto t = tf_buffer_->lookupTransform(
    "map",
    "camera_link",
    sensor_msg->header.stamp);

이 방식은 센서 퓨전(sensor fusion)이나 정밀 위치 추정에서 필수적이다. 다만, 지정된 시간의 변환이 버퍼에 존재하지 않을 경우 예외가 발생하므로, 적절한 예외 처리가 필요하다.

7. 예외 처리 패턴

lookupTransform()은 다양한 상황에서 예외를 발생시킬 수 있다. 각 예외의 원인과 적절한 처리 방법을 숙지하는 것이 중요하다.

7.1 주요 예외 유형

7.2 견고한 예외 처리 코드 패턴

void robustLookup()
{
    geometry_msgs::msg::TransformStamped transform;

    try {
        transform = tf_buffer_->lookupTransform(
            "map", "base_link",
            tf2::TimePointZero,
            tf2::durationFromSec(0.5));
    } catch (const tf2::LookupException &ex) {
        RCLCPP_ERROR(this->get_logger(),
            "Frame not found: %s", ex.what());
        return;
    } catch (const tf2::ConnectivityException &ex) {
        RCLCPP_ERROR(this->get_logger(),
            "Frames not connected: %s", ex.what());
        return;
    } catch (const tf2::ExtrapolationException &ex) {
        RCLCPP_WARN(this->get_logger(),
            "Extrapolation error: %s", ex.what());
        return;
    } catch (const tf2::TransformException &ex) {
        RCLCPP_WARN(this->get_logger(),
            "Transform error: %s", ex.what());
        return;
    }

    // 변환 사용
    processTransform(transform);
}

8. canTransform()과의 연계

lookupTransform()을 호출하기 전에 canTransform()을 사용하여 변환의 가용성을 사전에 확인할 수 있다. 그러나 canTransform()과 lookupTransform() 사이에 변환이 만료될 수 있으므로(TOCTOU 경합), 예외 처리를 항상 병행해야 한다.

if (tf_buffer_->canTransform("map", "base_link", tf2::TimePointZero)) {
    try {
        auto t = tf_buffer_->lookupTransform(
            "map", "base_link", tf2::TimePointZero);
        // 변환 사용
    } catch (const tf2::TransformException &ex) {
        // canTransform 이후 상태 변경 가능성에 대비
        RCLCPP_WARN(get_logger(), "%s", ex.what());
    }
}

9. 성능 고려사항

9.1 조회 빈도 최적화

lookupTransform()은 호출될 때마다 변환 트리를 탐색하고 보간을 수행하므로, 불필요하게 높은 빈도로 호출하면 CPU 부하가 증가한다. 일반적으로 다음과 같은 지침을 따른다.

• 제어 루프의 주기에 맞추어 조회한다(예: 100 Hz 제어 루프에서 100 Hz 조회).
• 동일한 프레임 쌍을 여러 곳에서 조회할 경우, 한 번 조회한 결과를 공유하는 것을 고려한다.
• 변환 결과를 캐싱하되, 캐싱된 결과의 유효 기간을 적절히 관리한다.

9.2 타임아웃 사용 시 주의사항

타임아웃을 지정한 lookupTransform() 호출은 블로킹 호출이 되어, 해당 스레드가 타임아웃 기간 동안 대기할 수 있다. 실시간 제어 루프나 콜백 내에서 긴 타임아웃을 사용하면 제어 주기를 위반할 수 있으므로, 다음과 같은 전략을 권장한다.

• 실시간 콜백에서는 tf2::TimePointZero와 함께 제로 타임아웃을 사용한다.
• 별도의 스레드에서 변환을 조회하고 결과를 공유한다.
• canTransform()으로 사전 확인 후, 변환이 가용한 경우에만 lookupTransform()을 호출한다.

10. 활용 사례

10.1 로봇 위치 모니터링

// 맵 좌표계에서 로봇의 현재 위치 조회
auto t = tf_buffer_->lookupTransform(
    "map", "base_link", tf2::TimePointZero);

double robot_x = t.transform.translation.x;
double robot_y = t.transform.translation.y;
double robot_yaw = std::atan2(
    2.0 * (t.transform.rotation.w * t.transform.rotation.z +
           t.transform.rotation.x * t.transform.rotation.y),
    1.0 - 2.0 * (t.transform.rotation.y * t.transform.rotation.y +
                  t.transform.rotation.z * t.transform.rotation.z));

10.2 센서 데이터 좌표 변환

// 카메라 프레임에서 검출된 물체 위치를 맵 프레임으로 변환
auto camera_to_map = tf_buffer_->lookupTransform(
    "map", "camera_link",
    detection_msg->header.stamp);

// tf2::doTransform()과 함께 사용하여 좌표 변환 수행
geometry_msgs::msg::PointStamped point_in_map;
tf2::doTransform(point_in_camera, point_in_map, camera_to_map);

10.3 다중 프레임 간의 상대 변환

// 좌우 카메라 사이의 상대 변환(스테레오 기선) 조회
auto stereo_transform = tf_buffer_->lookupTransform(
    "camera_left", "camera_right", tf2::TimePointZero);

11. 요약

lookupTransform()은 TF2 라이브러리의 핵심 API로서, 로봇 시스템에서 좌표 프레임 간의 변환을 조회하는 표준적인 방법을 제공한다. 이 함수는 변환 트리의 자동 탐색, 시간 보간, 변환 체인 합성 등의 복잡한 연산을 내부적으로 처리하여, 사용자에게는 간결한 인터페이스를 제공한다. 효과적인 사용을 위해서는 타깃과 소스 프레임의 순서, 시간 파라미터의 의미, 예외 처리 패턴을 정확히 이해하고, 성능 요구사항에 맞는 조회 전략을 수립하는 것이 중요하다.

참고 문헌 및 출처

• ROS 2 공식 문서, “tf2 Tutorials,” https://docs.ros.org/en/humble/Tutorials/Intermediate/Tf2/Tf2-Main.html (ROS 2 Humble Hawksbill)
• Open Robotics, “tf2_ros API Reference,” https://docs.ros2.org/latest/api/tf2_ros/
• Foote, T., “tf: The Transform Library,” Proceedings of IEEE Conference on Technologies for Practical Robot Applications (TePRA), 2013.
• REP 103, “Standard Units of Measure and Coordinate Conventions,” https://www.ros.org/reps/rep-0103.html
• REP 105, “Coordinate Frames for Mobile Platforms,” https://www.ros.org/reps/rep-0105.html