659.35 lookupTransform()의 파라미터와 반환값

1. 개요

lookupTransform()은 TF2 라이브러리에서 좌표 프레임 간의 변환을 조회하는 핵심 메서드이다. 이 함수는 여러 가지 오버로드(overload) 형태를 제공하며, 각 오버로드는 서로 다른 파라미터 조합을 통해 다양한 변환 조회 시나리오를 지원한다. 본 절에서는 lookupTransform()의 각 파라미터의 의미, 자료형, 제약 조건을 상세히 분석하고, 반환값인 TransformStamped 메시지의 구조를 체계적으로 기술한다.

2. rclcpp에서의 함수 시그니처

2.1 기본 오버로드

tf2_ros::Buffer 클래스는 다음과 같은 기본 오버로드를 제공한다.

geometry_msgs::msg::TransformStamped lookupTransform(
    const std::string & target_frame,
    const std::string & source_frame,
    const tf2::TimePoint & time) const;

2.1.1 파라미터 상세

target_frame: 변환의 목적지 프레임을 지정한다. 반환되는 변환을 적용하면, 소스 프레임에서 표현된 좌표가 이 타깃 프레임에서의 좌표로 변환된다. 빈 문자열("")을 전달하면 tf2::InvalidArgumentException이 발생한다. 프레임 이름 앞에 슬래시(/)를 붙이지 않는 것이 ROS 2의 규약이다.

source_frame: 변환의 출발지 프레임을 지정한다. 변환하고자 하는 데이터가 현재 표현되어 있는 좌표 프레임의 이름을 전달한다. 마찬가지로 빈 문자열은 예외를 유발한다.

time: 변환을 조회할 시간을 지정한다. tf2::TimePoint는 std::chrono::time_point<std::chrono::system_clock, std::chrono::nanoseconds> 타입의 별칭(alias)이다. 다음과 같은 값을 사용할 수 있다.

• tf2::TimePointZero 또는 tf2::TimePoint(): 버퍼에 저장된 최신 가용 변환을 조회한다.
• tf2::timeFromSec(double seconds): 에포크(epoch) 기준 초 단위 시간을 tf2::TimePoint로 변환한다.
• rclcpp::Time에서의 변환: tf2_ros::fromMsg(rclcpp_time) 또는 직접 rclcpp::Time 객체를 전달할 수 있다(내부적으로 자동 변환).

2.2 타임아웃을 포함한 오버로드

geometry_msgs::msg::TransformStamped lookupTransform(
    const std::string & target_frame,
    const std::string & source_frame,
    const tf2::TimePoint & time,
    const tf2::Duration & timeout) const;

timeout: tf2::Duration은 std::chrono::nanoseconds 타입의 별칭이다. 지정된 시간 동안 버퍼에 변환이 도착하기를 대기한다. 다음과 같이 생성한다.

// 1초 타임아웃
tf2::Duration timeout = tf2::durationFromSec(1.0);

// 500밀리초 타임아웃
tf2::Duration timeout = std::chrono::milliseconds(500);

// 제로 타임아웃 (즉시 반환)
tf2::Duration timeout = tf2::durationFromSec(0.0);

타임아웃이 0이면 블로킹 없이 즉시 조회를 시도하며, 변환이 없으면 즉시 예외를 발생시킨다.

2.3 고급 오버로드: 시간 여행 (Advanced Time Travel)

두 프레임이 서로 다른 시간에서의 변환을 합성해야 하는 경우를 위한 6-인자 오버로드이다.

geometry_msgs::msg::TransformStamped lookupTransform(
    const std::string & target_frame,
    const tf2::TimePoint & target_time,
    const std::string & source_frame,
    const tf2::TimePoint & source_time,
    const std::string & fixed_frame) const;

이 오버로드는 다음과 같은 수학적 연산을 수행한다.

T = T_{\text{fixed}}^{\text{target}}(t_{\text{target}}) \cdot \left( T_{\text{fixed}}^{\text{source}}(t_{\text{source}}) \right)^{-1}

여기서 T_{\text{fixed}}^{\text{target}}(t_{\text{target}})는 target_time 시점에서의 fixed_frame에서 target_frame으로의 변환이고, T_{\text{fixed}}^{\text{source}}(t_{\text{source}})는 source_time 시점에서의 fixed_frame에서 source_frame으로의 변환이다.

이 기능은 주로 과거 시점에 관측된 데이터를 현재 시점의 좌표 프레임으로 변환할 때 사용한다. 예를 들어, 카메라가 과거 시점 t_0에서 물체를 관측했고, 현재 시점 t_1에서의 맵 좌표로 변환하려면 다음과 같이 호출한다.

auto t = tf_buffer_->lookupTransform(
    "map",            // 타깃 프레임
    now,              // 타깃 시간 (현재)
    "camera_link",    // 소스 프레임
    past_time,        // 소스 시간 (관측 시점)
    "odom");          // 고정 프레임

6-인자 오버로드에도 타임아웃을 추가한 7-인자 오버로드가 존재한다.

geometry_msgs::msg::TransformStamped lookupTransform(
    const std::string & target_frame,
    const tf2::TimePoint & target_time,
    const std::string & source_frame,
    const tf2::TimePoint & source_time,
    const std::string & fixed_frame,
    const tf2::Duration & timeout) const;

3. rclpy에서의 함수 시그니처

3.1 기본 오버로드

def lookup_transform(
    self,
    target_frame: str,
    source_frame: str,
    time: rclpy.time.Time,
    timeout: rclpy.duration.Duration = Duration()
) -> geometry_msgs.msg.TransformStamped:

3.1.1 파라미터 상세

Python에서 시간을 지정하는 방법은 다음과 같다.

from rclpy.time import Time
from rclpy.duration import Duration

# 최신 가용 변환 조회
time = Time()                          # 시간 0 = 최신 가용

# 현재 노드 시간
time = node.get_clock().now()

# 특정 시간 지정
time = Time(seconds=1234567890, nanoseconds=0)

3.2 고급 오버로드: 시간 여행

def lookup_transform_full(
    self,
    target_frame: str,
    target_time: rclpy.time.Time,
    source_frame: str,
    source_time: rclpy.time.Time,
    fixed_frame: str,
    timeout: rclpy.duration.Duration = Duration()
) -> geometry_msgs.msg.TransformStamped:

Python에서는 시간 여행 기능을 lookup_transform_full() 메서드로 분리하여 제공한다. 이는 rclcpp의 6-인자 오버로드에 대응한다.

4. 반환값: TransformStamped 메시지

lookupTransform()은 geometry_msgs::msg::TransformStamped 메시지를 반환한다. 이 메시지의 구조는 다음과 같다.

4.1 메시지 구조

geometry_msgs/msg/TransformStamped
├── std_msgs/msg/Header header
│   ├── builtin_interfaces/msg/Time stamp
│   │   ├── int32 sec
│   │   └── uint32 nanosec
│   └── string frame_id
├── string child_frame_id
└── geometry_msgs/msg/Transform transform
    ├── geometry_msgs/msg/Vector3 translation
    │   ├── float64 x
    │   ├── float64 y
    │   └── float64 z
    └── geometry_msgs/msg/Quaternion rotation
        ├── float64 x
        ├── float64 y
        ├── float64 z
        └── float64 w

4.2 반환값의 각 필드 해석

4.3 header.stamp의 해석

header.stamp은 반환된 변환이 유효한 시간을 나타낸다. 이 값의 해석은 조회 시 지정한 시간 파라미터에 따라 달라진다.

• 특정 시간을 지정한 경우: header.stamp은 지정된 조회 시간과 동일하다.
• tf2::TimePointZero를 지정한 경우: header.stamp은 버퍼에서 가용한 최신 변환의 시간이다. 이 시간은 현재 시간이 아니라, 가장 최근에 발행된 변환의 타임스탬프이다.

4.4 header.frame_id와 child_frame_id의 관계

반환된 TransformStamped에서:

• header.frame_id는 lookupTransform()에 전달한 target_frame에 대응한다.
• child_frame_id는 lookupTransform()에 전달한 source_frame에 대응한다.

즉, 반환된 변환 T는 child_frame_id(소스)에서 표현된 점을 frame_id(타깃)에서의 표현으로 변환하는 관계를 나타낸다.

4.5 쿼터니언 정규화 상태

반환되는 쿼터니언은 항상 정규화(normalized) 상태이며, 단위 쿼터니언(unit quaternion)의 조건을 만족한다.

\sqrt{x^2 + y^2 + z^2 + w^2} = 1

단위 행렬에 해당하는 변환(항등 변환)의 경우, 쿼터니언은 (x, y, z, w) = (0, 0, 0, 1)이다.

5. 반환값 활용 예시

5.1 반환값에서 위치와 자세 추출

auto t = tf_buffer_->lookupTransform(
    "map", "base_link", tf2::TimePointZero);

// 위치 (병진) 추출
double x = t.transform.translation.x;
double y = t.transform.translation.y;
double z = t.transform.translation.z;

// 자세 (회전) 추출 - 쿼터니언
double qx = t.transform.rotation.x;
double qy = t.transform.rotation.y;
double qz = t.transform.rotation.z;
double qw = t.transform.rotation.w;

// 쿼터니언에서 오일러 각(RPY)으로 변환
tf2::Quaternion q(qx, qy, qz, qw);
tf2::Matrix3x3 m(q);
double roll, pitch, yaw;
m.getRPY(roll, pitch, yaw);

5.2 반환값을 이용한 거리 계산

auto t = tf_buffer_->lookupTransform(
    "base_link", "target_object", tf2::TimePointZero);

double distance = std::sqrt(
    std::pow(t.transform.translation.x, 2) +
    std::pow(t.transform.translation.y, 2) +
    std::pow(t.transform.translation.z, 2));

RCLCPP_INFO(get_logger(),
    "Distance to target: %.3f m", distance);

5.3 반환된 시간 정보 활용

auto t = tf_buffer_->lookupTransform(
    "map", "base_link", tf2::TimePointZero);

// 변환의 유효 시간 확인
rclcpp::Time transform_time = t.header.stamp;
rclcpp::Time current_time = this->get_clock()->now();
rclcpp::Duration age = current_time - transform_time;

if (age.seconds() > 1.0) {
    RCLCPP_WARN(get_logger(),
        "Transform is %.2f seconds old, may be stale",
        age.seconds());
}

6. 파라미터 유효성 검증 규칙

lookupTransform()은 내부적으로 다음과 같은 파라미터 유효성 검증을 수행한다.

1. 프레임 이름 비공백 검증: target_frame과 source_frame이 빈 문자열이 아닌지 확인한다. 빈 문자열이 전달되면 tf2::InvalidArgumentException을 발생시킨다.
2. 프레임 존재 검증: 지정된 프레임이 변환 트리에 등록되어 있는지 확인한다. 미등록 프레임은 tf2::LookupException을 발생시킨다.
3. 연결성 검증: 두 프레임 사이에 유효한 경로가 존재하는지 확인한다. 경로가 없으면 tf2::ConnectivityException을 발생시킨다.
4. 시간 범위 검증: 지정된 시간이 캐시에 저장된 변환의 시간 범위 내에 있는지 확인한다. 범위를 벗어나면 tf2::ExtrapolationException을 발생시킨다.

7. 요약

lookupTransform()은 3-인자, 4-인자(타임아웃 포함), 5-인자(시간 여행), 6-인자(시간 여행 + 타임아웃)의 네 가지 오버로드를 제공하며, 각각의 사용 시나리오에 최적화되어 있다. 반환값은 TransformStamped 메시지로, 변환의 시간, 프레임 이름, 병진 벡터, 회전 쿼터니언을 포함한다. 파라미터의 순서(타깃 먼저, 소스 나중)와 시간 파라미터의 의미를 정확히 이해하는 것이 올바른 사용의 전제 조건이다.

참고 문헌 및 출처

• ROS 2 공식 문서, “tf2_ros::Buffer Class Reference,” https://docs.ros2.org/latest/api/tf2_ros/classtf2__ros_1_1Buffer.html (ROS 2 Humble Hawksbill)
• Open Robotics, “geometry_msgs/msg/TransformStamped,” https://docs.ros2.org/latest/api/geometry_msgs/msg/TransformStamped.html
• Foote, T., “tf: The Transform Library,” Proceedings of IEEE Conference on Technologies for Practical Robot Applications (TePRA), 2013.
• ROS 2 공식 문서, “tf2 Tutorials,” https://docs.ros.org/en/humble/Tutorials/Intermediate/Tf2/Tf2-Main.html