# tf2 Listener 예제(C++/Python) tf2는 ROS2에서 좌표계(Frames) 간 변환(Transform)을 주고받기 위한 핵심 패키지다. 이를 통해 여러 센서나 로봇의 링크(Link) 간 위치 관계를 효율적으로 관리할 수 있다. tf2 Listener는 주어진 시점에서 특정 좌표계 A에서 B로의 변환 정보를 받아올 때 사용한다. 예를 들어, 카메라 좌표계에서 로봇 바닥 좌표계로, 또는 로봇 본체 좌표계에서 로봇 팔의 말단 좌표계로 변환이 필요한 경우, tf2 Listener를 활용하여 변환 관계를 쉽게 얻을 수 있다. #### tf2 Listener를 이해하기 위한 사전 개념 * **Frames**: 로봇, 센서 등 각 요소가 가진 좌표계를 의미한다. 예: `base_link`, `camera_link`, `world`, 등 * **Transforms**: Frame A에서 Frame B로 이동하거나 회전하는 변환 관계다. 이를 일반적으로 4×4 행렬 형태로 표현할 수 있다. * **Transform Tree**: 여러 Frame이 서로 부모-자식 관계를 맺으면서 이루어지는 트리 구조. tf2는 전체적인 트리 형태로 관리한다. tf2 Listener 예제를 살펴보기 전, 변환 행렬에 대한 일반적인 수식을 복습해 두면 좋다. 예를 들어, 위치 벡터 $\mathbf{p}*\text{source}$를 target 좌표계로 변환할 때는 다음과 같은 4×4 동차 좌표(Homogeneous coordinates) 행렬 $\mathbf{T}*{\text{target} \leftarrow \text{source}}$를 사용할 수 있다. $$ \mathbf{p}*\text{target} = \mathbf{T}*{\text{target} \leftarrow \text{source}} \cdot \mathbf{p}\_\text{source} $$ 여기서 $$ \mathbf{T}*{target \leftarrow source} = \begin{bmatrix} \mathbf{R} & \mathbf{t} \ \mathbf{0} & 1 \end{bmatrix}, \quad \mathbf{p}*{source} = \begin{bmatrix} x \ y \ z \ 1 \end{bmatrix} $$ 이고, $\mathbf{R}$은 3×3 회전 행렬, $\mathbf{t}$은 평행 이동 벡터다. #### ROS2 tf2 Listener 기본 동작 원리 tf2 Listener는 ROS2 Topic을 통해 현재까지 브로드캐스팅된(Frame 간) 변환 정보를 수신한다. 내부적으로는 tf2 Buffer를 사용하여 저장하고, 사용자가 특정 시점에 특정 Frame 간 변환을 요청하면 해당 변환 행렬 또는 변환 메시지를 반환한다. 실제 동작을 살펴보면 크게 다음 단계를 거친다. 1. **Buffer 객체 준비** Listener가 받은 변환 정보를 모두 저장하고, 필요 시 탐색할 수 있는 Buffer 객체를 준비한다. 2. **Transform Listener 노드 생성** tf2\_ros 패키지에서 제공하는 TransformListener 클래스를 활용한다. 이 때, Buffer 객체를 생성자 인자로 전달한다. 3. **변환 정보 요청** 사용자는 필요할 때마다 ‘Frame A에서 Frame B로 가는 변환’을 요청한다. 4. **결과 처리** 요청이 성공하면 행렬(또는 변환 메시지)을 반환받아, 임의의 점이나 Pose에 곱해 최종 좌표 변환을 수행한다. 다음은 C++와 Python에서 각각 tf2 Listener를 구현하고 활용하는 예제를 살펴본다. #### C++ 예제 - tf2 Listener 아래 예제는 ROS2 Humble 환경에서 rclcpp, tf2\_ros, geometry\_msgs 등을 활용하여 구현한 tf2 Listener 노드다. 기본적인 구조는 다음과 같다. ```cpp #include #include #include #include #include #include class TF2ListenerExample : public rclcpp::Node { public: TF2ListenerExample() : Node("tf2_listener_example"), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) { // 필요한 경우 Timer나 Subscription 설정 timer_ = this->create_wall_timer( std::chrono::seconds(1), std::bind(&TF2ListenerExample::timerCallback, this) ); } private: void timerCallback() { try { // "target_frame"에서 "source_frame"으로 가는 변환 요청 geometry_msgs::msg::TransformStamped transform_stamped = tf_buffer_.lookupTransform("target_frame", "source_frame", tf2::TimePointZero); RCLCPP_INFO(this->get_logger(), "Got transform! %f, %f, %f", transform_stamped.transform.translation.x, transform_stamped.transform.translation.y, transform_stamped.transform.translation.z); // 좌표 변환 예시 geometry_msgs::msg::PointStamped point_in_source; point_in_source.header.frame_id = "source_frame"; point_in_source.point.x = 1.0; point_in_source.point.y = 0.0; point_in_source.point.z = 0.0; geometry_msgs::msg::PointStamped point_in_target = tf_buffer_.transform(point_in_source, "target_frame", tf2::TimePointZero); RCLCPP_INFO(this->get_logger(), "Point in target: (%f, %f, %f)", point_in_target.point.x, point_in_target.point.y, point_in_target.point.z); } catch (tf2::TransformException &ex) { RCLCPP_WARN(this->get_logger(), "Could not transform: %s", ex.what()); } } tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; rclcpp::TimerBase::SharedPtr timer_; }; int main(int argc, char** argv) { rclcpp::init(argc, argv); auto node = std::make_shared(); rclcpp::spin(node); rclcpp::shutdown(); return 0; } ``` 위 C++ 예제에서 핵심 포인트는 다음과 같다. * `tf2_ros::Buffer tf_buffer_`를 멤버로 두어 변환 정보를 저장한다. * `tf2_ros::TransformListener tf_listener_`로 실제 tf2 토픽을 구독하여 `tf_buffer_`를 채운다. * `lookupTransform("target_frame", "source_frame", tf2::TimePointZero)`를 통해 `target_frame` ← `source_frame` 변환 정보를 가져온다. * 가져온 변환 정보를 이용해 `tf_buffer_.transform()` 메서드로 좌표 변환을 수행한다. #### Python 예제 - tf2 Listener Python의 경우도 개념은 동일하며, rclpy와 tf2\_ros Python 바인딩을 활용한다. 예제 코드는 다음과 같다. ```python import rclpy from rclpy.node import Node from geometry_msgs.msg import TransformStamped, PointStamped from tf2_ros import Buffer, TransformListener, LookupException, ConnectivityException, ExtrapolationException class TF2ListenerExample(Node): def __init__(self): super().__init__('tf2_listener_example') self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self) self.timer = self.create_timer(1.0, self.timer_callback) def timer_callback(self): try: transform_stamped = self.tf_buffer.lookup_transform( 'target_frame', 'source_frame', rclpy.time.Time() ) self.get_logger().info( f'Got transform! {transform_stamped.transform.translation.x}, ' f'{transform_stamped.transform.translation.y}, ' f'{transform_stamped.transform.translation.z}' ) point_in_source = PointStamped() point_in_source.header.frame_id = 'source_frame' point_in_source.point.x = 1.0 point_in_source.point.y = 0.0 point_in_source.point.z = 0.0 point_in_target = self.tf_buffer.transform( point_in_source, 'target_frame', rclpy.time.Time() ) self.get_logger().info( f'Point in target: ({point_in_target.point.x}, ' f'{point_in_target.point.y}, ' f'{point_in_target.point.z})' ) except (LookupException, ConnectivityException, ExtrapolationException) as ex: self.get_logger().warn(f'Could not transform: {ex}') def main(args=None): rclpy.init(args=args) node = TF2ListenerExample() rclpy.spin(node) rclpy.shutdown() if __name__ == '__main__': main() ``` 이 Python 예제에서도 C++ 예제와 유사하게 `tf2_ros.Buffer`와 `tf2_ros.TransformListener`를 사용한다. 일정 주기로 `lookup_transform`을 수행하여 Frame 간 변환 데이터를 얻고, 해당 변환을 이용해 임의의 점(Point)의 좌표를 다른 Frame으로 변환할 수 있다. #### tf2 Listener 고급 활용 **예외 처리와 시간 파라미터** tf2는 내부적으로 시간에 따른 변환 기록(Transform History)을 유지한다. 따라서 특정 시점의 변환이 필요할 때, tf2 Listener(또는 Buffer)는 해당 시점의 변환이 존재하는지 확인하고, 없을 경우 예외를 던진다. 흔히 발생하는 예외들은 다음과 같다. * **tf2::LookupException / tf2\_ros::LookupException** 요청한 변환 정보를 찾을 수 없을 때 발생한다. * **tf2::ConnectivityException / tf2\_ros::ConnectivityException** 두 Frame이 서로 연결되어 있지 않을 때 발생한다. 예: 하나는 `world` 트리, 다른 하나는 `odom` 트리에 속해 있으나 상호 연결이 전혀 없는 경우. * **tf2::ExtrapolationException / tf2\_ros::ExtrapolationException** 요청한 시간의 변환 데이터가 Buffer에 존재하지 않을 때(너무 오래된 시간이거나 아직 들어오지 않은 미래 시점 등) 발생한다. * **tf2::TimeoutException** 주어진 시간 안에 변환 정보를 구하지 못했을 경우 발생한다(주로 비동기 함수를 사용할 때). 아래와 같은 코드를 통해 예외 상황을 세분화해서 처리할 수 있다(C++ 예시). ```cpp try { geometry_msgs::msg::TransformStamped transform_stamped = tf_buffer_.lookupTransform("target_frame", "source_frame", tf2::TimePointZero); // 변환 성공 시 추가 로직... } catch (tf2::LookupException &ex) { RCLCPP_WARN(this->get_logger(), "LookupException: %s", ex.what()); } catch (tf2::ConnectivityException &ex) { RCLCPP_WARN(this->get_logger(), "ConnectivityException: %s", ex.what()); } catch (tf2::ExtrapolationException &ex) { RCLCPP_WARN(this->get_logger(), "ExtrapolationException: %s", ex.what()); } catch (tf2::TransformException &ex) { RCLCPP_ERROR(this->get_logger(), "TransformException: %s", ex.what()); } ``` Python에서도 비슷하게 예외 클래스를 구분해서 처리할 수 있다. ```python from tf2_ros import (LookupException, ConnectivityException, ExtrapolationException) try: transform_stamped = self.tf_buffer.lookup_transform( 'target_frame', 'source_frame', rclpy.time.Time() ) # 변환 성공 시 추가 로직... except LookupException as ex: self.get_logger().warn(f'LookupException: {ex}') except ConnectivityException as ex: self.get_logger().warn(f'ConnectivityException: {ex}') except ExtrapolationException as ex: self.get_logger().warn(f'ExtrapolationException: {ex}') except Exception as ex: self.get_logger().error(f'Exception: {ex}') ``` 또한 tf2의 시간 관련 파라미터를 이해해야 정확한 시점의 변환을 가져올 수 있다. 예를 들어, 다음과 같은 방식으로 특정 ROS2 시간(예: 현재 시각에서 0.1초 전 시점)을 기준으로 변환을 요청할 수도 있다. ```cpp auto now = this->get_clock()->now(); rclcpp::Time time_with_offset = now - rclcpp::Duration(0, 100000000); // 0.1초 전 geometry_msgs::msg::TransformStamped transform_stamped = tf_buffer_.lookupTransform("target_frame", "source_frame", time_with_offset); ``` tf2가 관리하는 변환 히스토리 내에 해당 시간대의 변환 기록이 있어야 하므로, 브로드캐스팅 노드에서 `static_transform_publisher`나 `tf2_ros::TransformBroadcaster`가 꾸준히 데이터를 보내주는지 확인해야 한다. **좌표 변환 수식 응용** lookupTransform으로 얻은 변환 정보(회전, 평행이동)를 수식으로 정리하면 다음과 같다. 예를 들어, 소스 좌표계(Source frame)에서 벡터 $\mathbf{p}\_\text{source}$를 타깃 좌표계(Target frame)로 변환하려면, $$ \mathbf{p}*\text{target} = \mathbf{R}*{\text{target} \leftarrow \text{source}} \mathbf{p}*\text{source} + \mathbf{t}*{\text{target} \leftarrow \text{source}} $$ 회전 행렬(3×3) $\mathbf{R}*{\text{target} \leftarrow \text{source}}$와 평행 이동 벡터(3×1) $\mathbf{t}*{\text{target} \leftarrow \text{source}}$는 TransformStamped 메시지의 `transform.rotation`, `transform.translation` 필드에서 얻을 수 있다. 특히 rotation은 쿼터니언(Quaternion) 형태($x, y, z, w$)로 제공되므로, 변환 전 3×3 회전 행렬로 바꾸어야 한다. (tf2 라이브러리에 이미 변환 함수 존재) **TF2 Debugging Tool** ROS2 TF를 시각화하거나 디버깅할 때 `rviz2`를 사용하면 Frame 간 관계를 직관적으로 확인할 수 있다. 또한 command line에서 `tf2_echo` 같은 유틸리티로 현재 브로드캐스팅되고 있는 transform을 확인할 수도 있다. ```bash ros2 run tf2_ros tf2_echo [source_frame] [target_frame] ``` 지속적으로 변환 정보를 출력하므로, Listener 노드에서 예외가 발생했을 때 TF 트리에서 프레임이 어떻게 연결되어 있는지 확인하는 데 유용하다. #### 추가적인 설계 고려사항 **Transform이 누락되는 경우** ROS2 시스템에서 여러 노드가 동작하다 보면, 특정 시점에 필요한 Transform이 누락될 수 있다. 예를 들어, 브로드캐스터 노드가 종료되거나, 특정 프레임이 올바르게 브로드캐스팅되지 않는 경우 등이다. 이런 상황을 방지하기 위한 방법은 다음과 같다. * **모든 프레임이 정규화된 트리 구조에 포함되도록 설계** 예: `world` → `map` → `odom` → `base_link` → `camera_link` → ... 전체 프레임이 하나의 트리 구조로 연결되어 있어야 tf2가 이들을 서로 변환할 수 있다. * **Static Transform Broadcaster 활용** 고정된(변하지 않는) 변환 관계가 있다면 `static_transform_publisher` 또는 `tf2_ros::StaticTransformBroadcaster`를 통해 영구적으로 브로드캐스팅해 주어야 한다. 예를 들어, 센서 위치가 물리적으로 고정되어 있다면 한 번 선언해 두면 된다. * **tf2 Buffer의 Time Synchronization** 분산된 로봇 시스템에서 시계(Clock) 동기화가 제대로 되지 않으면, 예기치 못한 ExtrapolationException이 발생할 수 있다. 필요하다면 `chrony`나 `ntpd`와 같은 NTP 기반 동기화 방식을 고려하거나, ROS2에서 제공하는 다양한 Clock 옵션을 검토해야 한다. **고빈도 LookupTransform 사용 주의** `lookupTransform()`를 매우 짧은 주기로 고빈도로 호출할 경우, CPU 부하가 커질 수 있다. 특히 로봇 움직임이 빠르지 않다면 초당 1\~10Hz 정도의 요청 주기로도 충분한 경우가 많다. 너무 자주 호출하는 대신 아래 방식을 고려할 수 있다. * **주기 조절**: 타이머나 콜백에서 호출 간격을 늘린다. * **Cache 사용**: 한 번 얻은 transform을 잠시(수 ms\~수십 ms) 캐싱해 두고, 큰 변경사항이 없는 한 재활용한다. * **필요할 때만 호출**: 지속적으로 폴링(polling)하기보다는 이벤트가 발생할 때 호출하도록 설계한다. **복수 노드에서 Buffer 공유 vs. 독립 사용** ROS2에서 tf2 Buffer는 일반적으로 하나의 노드 안에 존재한다. 그러나, 여러 노드가 하나의 Buffer를 공유해야 하는 상황이라면 다음과 같은 설계 선택지가 있다. 1. **단일 tf2 Listener 노드 + Service/Action** 한 노드에서만 모든 transform 정보를 관리하고, 나머지 노드들이 서비스 호출 등을 통해 변환을 요청한다. 2. **각 노드별 독립 Buffer** 각 노드가 독립된 Buffer와 Listener를 두고, 동일한 tf2 토픽을 구독한다. 이 경우 네트워크/토픽 트래픽이 늘어날 수 있으나, 노드 간 결합도는 낮아진다. **Custom Message 변환** tf2는 `geometry_msgs` 계열의 메시지(Point, Pose, PoseStamped 등)에 대한 변환 함수를 기본 제공하지만, 사용자 정의 메시지 타입(Custom message)을 사용해야 하는 경우도 있다. 이런 경우 tf2에 변환 함수를 등록해 두면, 동일한 방식으로 `tf2::doTransform()` 등을 활용하여 좌표 변환을 수행할 수 있다. 예를 들어, `my_robot_interfaces/msg/CustomPoint.msg`와 같은 메시지 타입을 정의하고, 이를 변환하기 위한 스페셜라이제이션을 작성할 수 있다(C++ 예제). ```cpp // somewhere in a .hpp or .cpp #include #include #include #include namespace tf2 { template<> inline void doTransform( const my_robot_interfaces::msg::CustomPoint &point_in, my_robot_interfaces::msg::CustomPoint &point_out, const geometry_msgs::msg::TransformStamped &transform_stamped) { // 직접 회전/평행이동을 계산하거나 // geometry_msgs::msg::Point 변환 로직을 재사용하는 등 구현 // 예시) geometry_msgs::msg::Point temp_point; temp_point.x = point_in.x; temp_point.y = point_in.y; temp_point.z = point_in.z; geometry_msgs::msg::Point transformed_point; tf2::doTransform(temp_point, transformed_point, transform_stamped); point_out.x = transformed_point.x; point_out.y = transformed_point.y; point_out.z = transformed_point.z; } } // namespace tf2 ``` 그 후 사용자는 `tf_buffer_.transform(custom_point, "target_frame")`처럼 표준 메시지와 동일한 방식으로 좌표 변환을 호출할 수 있다. **tf2의 주요 함수 요약** * **lookupTransform(target\_frame, source\_frame, time)** 특정 시점(time)에서 target\_frame ← source\_frame 변환을 가져온다. * **transform(msg, target\_frame, time)** 메시지(msg)를 target\_frame 기준으로 변환한다. (Pose, Point, Quaternion, 등 다양한 메시지 타입을 지원) * **canTransform(target\_frame, source\_frame, time)** 변환이 가능한지(즉, tf2 Buffer에 해당 정보가 존재하는지) 미리 확인한다. 부울(bool) 반환. * **setUsingDedicatedThread(bool)** TransformListener 초기화 시, 전용 스레드를 사용해 tf를 구독할지 결정한다(C++). Python에선 내부적으로 스레드 처리된다.