659.43 변환 외삽 (Extrapolation)과 허용 범위
1. 개요
변환 외삽(extrapolation)은 TF2 버퍼에 저장된 변환 데이터의 시간 범위 밖에서 변환을 요청하는 상황을 지칭한다. 보간(interpolation)이 두 데이터 사이의 값을 추정하는 반면, 외삽은 데이터 범위를 넘어서는 값을 예측해야 하므로 본질적으로 신뢰도가 낮다. TF2는 안전성을 위해 외삽을 기본적으로 금지하며, 외삽이 시도되면 tf2::ExtrapolationException을 발생시킨다. 본 절에서는 외삽이 발생하는 원인, TF2의 외삽 처리 정책, 그리고 실제 시스템에서의 대응 전략을 체계적으로 기술한다.
2. 외삽의 정의와 유형
2.1 보간과 외삽의 구분
캐시에 저장된 변환 데이터의 시간 범위를 [t_{\text{earliest}}, t_{\text{latest}}]라 하면, 조회 시간 t_q에 따라 다음과 같이 분류된다.
| 조건 | 분류 | TF2 동작 |
|---|---|---|
| t_{\text{earliest}} \leq t_q \leq t_{\text{latest}} | 보간 (Interpolation) | 정상 처리 |
| t_q < t_{\text{earliest}} | 과거 외삽 (Past Extrapolation) | 예외 발생 |
| t_q > t_{\text{latest}} | 미래 외삽 (Future Extrapolation) | 예외 발생 |
2.2 과거 외삽 (Past Extrapolation)
조회 시간이 캐시에 저장된 가장 오래된 변환보다 이전일 때 발생한다. 주요 원인은 다음과 같다.
- 캐시 기간 초과: 매우 오래된 센서 데이터를 처리할 때, 해당 시점의 변환이 이미 캐시에서 제거되었다.
- 시스템 시작 직후: 버퍼가 생성된 직후에는 저장된 변환이 없으므로, 시스템 시작 이전의 시간을 조회하면 과거 외삽이 발생한다.
- 시간 불일치: rosbag 재생 시
use_sim_time이 제대로 설정되지 않으면, 현재 시스템 시간과 데이터 시간 사이에 큰 차이가 발생하여 과거 외삽이 발생한다.
2.3 미래 외삽 (Future Extrapolation)
조회 시간이 캐시에 저장된 가장 최신 변환보다 이후일 때 발생한다. 주요 원인은 다음과 같다.
- 변환 발행 지연: 센서 데이터는 도착했으나, 대응하는 변환이 아직 발행되지 않았다.
- 시간 동기화 오류: 센서 노드와 변환 발행 노드의 시간이 동기화되지 않아, 센서 타임스탬프가 변환 타임스탬프보다 앞서 있다.
- 현재 시간 사용:
lookupTransform()에this->get_clock()->now()를 전달할 때, 변환 발행의 통신 지연으로 인해 현재 시간의 변환이 아직 수신되지 않았다.
3. ExtrapolationException
3.1 예외 메시지 형식
TF2가 외삽을 감지하면, 다음 형태의 예외 메시지와 함께 tf2::ExtrapolationException을 발생시킨다.
과거 외삽:
Lookup would require extrapolation into the past.
Requested time 1616000000.000000000 but the earliest data is
at time 1616000005.000000000, when looking up transform from
frame [base_link] to frame [map].
미래 외삽:
Lookup would require extrapolation into the future.
Requested time 1616000015.000000000 but the latest data is
at time 1616000010.000000000, when looking up transform from
frame [base_link] to frame [map].
3.2 예외 메시지에서 추출할 수 있는 정보
| 정보 | 설명 |
|---|---|
| 요청 시간(Requested time) | lookupTransform()에 전달한 조회 시간 |
| 가용 시간(earliest/latest data) | 캐시에 저장된 변환의 시간 경계 |
| 프레임 정보 | 소스 프레임과 타깃 프레임의 이름 |
| 외삽 방향 | 과거(past) 또는 미래(future) |
4. TF2의 외삽 허용 정책
4.1 기본 정책: 제로 허용 (Zero Tolerance)
TF2의 기본 외삽 정책은 외삽 불허이다. 조회 시간이 캐시 범위를 조금이라도 벗어나면 즉시 예외를 발생시킨다. 이 엄격한 정책은 다음과 같은 설계 철학에 기반한다.
- 안전성 우선: 부정확한 외삽 결과로 인한 로봇의 위험한 동작을 방지한다.
- 오류 가시성: 시스템의 시간 동기화 문제나 변환 발행 문제를 조기에 발견할 수 있다.
- 예측 불가능성: 외삽은 데이터 범위 밖의 예측이므로, 로봇의 운동이 비선형적인 경우 심각한 오차를 초래할 수 있다.
4.2 캐시 단일 항목 시의 특수 동작
캐시에 변환이 단일 항목만 존재하는 경우(예: 시스템 시작 직후), t_q가 해당 항목의 시간과 약간의 차이가 있더라도 외삽으로 판정된다. 이 경우에도 예외가 발생하므로, 시스템 초기화 단계에서는 적절한 대기 메커니즘이 필요하다.
4.3 정적 변환의 외삽 면제
정적 변환(static transform)은 시간에 독립적이므로 외삽의 개념이 적용되지 않는다. 정적 변환은 한 번 수신되면 모든 시간에 대해 유효하다.
5. 외삽 발생 시나리오별 대응 전략
5.1 시나리오 1: 미래 외삽 - 변환 발행 지연
가장 흔한 외삽 상황으로, 센서 데이터의 타임스탬프에 대한 변환이 아직 수신되지 않은 경우이다.
대응 전략 1: tf2::TimePointZero 사용
// 문제: 현재 시간 사용 시 미래 외삽 발생
auto t = tf_buffer_->lookupTransform(
"map", "base_link", this->get_clock()->now()); // 외삽 위험!
// 해결: TimePointZero로 최신 가용 변환 사용
auto t = tf_buffer_->lookupTransform(
"map", "base_link", tf2::TimePointZero); // 안전
대응 전략 2: 타임아웃을 활용한 대기
try {
auto t = tf_buffer_->lookupTransform(
"map", "base_link",
sensor_msg->header.stamp,
tf2::durationFromSec(0.1)); // 100ms 대기
} catch (const tf2::ExtrapolationException & ex) {
RCLCPP_WARN(get_logger(),
"Transform still not available: %s", ex.what());
}
대응 전략 3: MessageFilter 활용
// 변환이 가용해질 때까지 자동 대기
tf_filter_ = std::make_shared<tf2_ros::MessageFilter<
sensor_msgs::msg::LaserScan>>(
scan_sub_, *tf_buffer_, "map", 50,
get_node_logging_interface(),
get_node_clock_interface(),
tf2::durationFromSec(0.5));
5.2 시나리오 2: 과거 외삽 - 캐시 기간 초과
센서 데이터의 처리 지연이 커서, 데이터의 타임스탬프가 캐시 기간 밖에 놓이는 경우이다.
대응 전략: 캐시 기간 확장
// 기본 캐시 기간 (10초)을 확장
tf_buffer_ = std::make_unique<tf2_ros::Buffer>(
this->get_clock(),
tf2::durationFromSec(30.0)); // 30초로 확장
5.3 시나리오 3: 시스템 시작 직후
변환이 아직 발행되지 않은 시스템 초기화 단계에서 발생한다.
대응 전략: 초기화 대기 루프
bool waitForInitialTransform(
const std::string & target_frame,
const std::string & source_frame,
double timeout_sec)
{
rclcpp::Time start = this->get_clock()->now();
while (rclcpp::ok()) {
if (tf_buffer_->canTransform(
target_frame, source_frame,
tf2::TimePointZero)) {
return true;
}
double elapsed =
(this->get_clock()->now() - start).seconds();
if (elapsed > timeout_sec) {
return false;
}
rclcpp::sleep_for(std::chrono::milliseconds(100));
}
return false;
}
5.4 시나리오 4: 시간 동기화 오류
노드 간 시간 동기화가 불완전하여, 센서 타임스탬프와 변환 타임스탬프 사이에 체계적인 편차(offset)가 존재하는 경우이다.
대응 전략: 시간 동기화 확인
# NTP 동기화 상태 확인
timedatectl status
# ROS 2 노드 간 시간 차이 확인
ros2 topic echo /clock
6. 외삽 오차의 수학적 분석
외삽이 허용된다고 가정했을 때의 오차를 분석한다. 등속 직선 운동을 가정하여 외삽한 경우, 실제 운동이 가속도 \mathbf{a}를 포함한다면 외삽 오차는 다음과 같다.
\epsilon_{\text{extrapolation}} = \frac{1}{2} |\mathbf{a}| \cdot (\Delta t_{\text{ext}})^2
여기서 \Delta t_{\text{ext}}는 캐시 경계에서 조회 시간까지의 시간 차이이다. 외삽 구간이 길어질수록 오차가 제곱으로 증가하므로, 외삽은 본질적으로 위험하다.
6.1 수치 예시
| 외삽 구간 | 가속도 1 m/s² 시 오차 | 가속도 5 m/s² 시 오차 |
|---|---|---|
| 10 ms | 0.05 mm | 0.25 mm |
| 50 ms | 1.25 mm | 6.25 mm |
| 100 ms | 5.0 mm | 25.0 mm |
| 500 ms | 125.0 mm | 625.0 mm |
| 1 s | 500.0 mm | 2500.0 mm |
위 표에서 볼 수 있듯이, 100 ms 이상의 외삽은 센서 정밀도에 비해 상당한 오차를 초래할 수 있으며, 이는 TF2가 외삽을 금지하는 주요 근거이다.
7. 견고한 예외 처리 패턴
void processWithFallback(
const sensor_msgs::msg::PointCloud2::SharedPtr cloud)
{
geometry_msgs::msg::TransformStamped transform;
try {
// 1차 시도: 정확한 센서 시간으로 조회
transform = tf_buffer_->lookupTransform(
"map", cloud->header.frame_id,
cloud->header.stamp,
tf2::durationFromSec(0.1));
} catch (const tf2::ExtrapolationException &) {
try {
// 2차 시도: 최신 가용 변환으로 대체
transform = tf_buffer_->lookupTransform(
"map", cloud->header.frame_id,
tf2::TimePointZero);
rclcpp::Duration age(
this->get_clock()->now() -
rclcpp::Time(transform.header.stamp));
if (age.seconds() > 1.0) {
RCLCPP_WARN(get_logger(),
"Using stale transform (%.2f s old)",
age.seconds());
return; // 너무 오래된 변환은 사용하지 않음
}
} catch (const tf2::TransformException & ex) {
RCLCPP_ERROR(get_logger(),
"All transform lookups failed: %s", ex.what());
return;
}
}
processCloud(cloud, transform);
}
8. 외삽 허용이 필요한 경우
일부 특수한 상황에서는 제한적인 외삽이 유용할 수 있다. TF2 자체는 외삽 허용 기능을 제공하지 않지만, 사용자 코드에서 다음과 같은 방법으로 유사한 효과를 구현할 수 있다.
8.1 최신 변환 유지 (Hold-Last-Value)
가장 안전한 외삽 대안으로, tf2::TimePointZero를 사용하여 최신 가용 변환을 그대로 사용한다. 이는 0차 홀드(zero-order hold) 외삽에 해당하며, 변환이 최근에 변경되지 않은 경우(준정적 상태)에는 합리적인 근사이다.
8.2 선형 예측
최근 두 변환의 속도를 계산하여 미래 위치를 예측하는 방법이다. 이는 TF2 외부에서 별도로 구현해야 한다.
// 선형 예측 (TF2 외부 구현)
auto t1 = tf_buffer_->lookupTransform(
"map", "base_link", tf2::TimePointZero);
auto t0 = tf_buffer_->lookupTransform(
"map", "base_link",
rclcpp::Time(t1.header.stamp) - rclcpp::Duration::from_seconds(0.1));
// 속도 추정
double vx = (t1.transform.translation.x - t0.transform.translation.x) / 0.1;
double vy = (t1.transform.translation.y - t0.transform.translation.y) / 0.1;
// 미래 위치 예측
double dt = (now - rclcpp::Time(t1.header.stamp)).seconds();
double predicted_x = t1.transform.translation.x + vx * dt;
double predicted_y = t1.transform.translation.y + vy * dt;
이 방법은 TF2의 안전 메커니즘을 우회하므로, 예측 오차에 대한 충분한 고려와 안전 마진(safety margin)의 설정이 필수적이다.
9. 요약
TF2는 외삽을 기본적으로 금지하며, 조회 시간이 캐시 범위를 벗어나면 tf2::ExtrapolationException을 발생시킨다. 이 정책은 로봇 안전성을 보장하고, 시간 동기화 문제를 조기에 발견하기 위한 설계적 선택이다. 외삽이 발생하는 주요 원인은 변환 발행 지연, 캐시 기간 초과, 시스템 초기화 미완료, 시간 동기화 오류이며, 각 상황에 맞는 대응 전략(TimePointZero 사용, 타임아웃, 캐시 확장, MessageFilter)을 적용하여 외삽 없이 안정적인 변환 조회를 구현할 수 있다.
참고 문헌 및 출처
- ROS 2 공식 문서, “tf2_ros::Buffer API Reference,” https://docs.ros2.org/latest/api/tf2_ros/classtf2__ros_1_1Buffer.html (ROS 2 Humble Hawksbill)
- ROS 2 소스 코드, “tf2/buffer_core.cpp,” https://github.com/ros2/geometry2 (geometry2 리포지토리)
- Foote, T., “tf: The Transform Library,” Proceedings of IEEE Conference on Technologies for Practical Robot Applications (TePRA), 2013.
- ROS 2 공식 문서, “Troubleshooting tf2,” https://docs.ros.org/en/humble/Tutorials/Intermediate/Tf2/Debugging-Tf2-Problems.html