# 좌표계 생성과 브로드캐스터 구현 #### 브로드캐스터(Transform Broadcaster)란 무엇인가 ROS2에서 tf2는 서로 다른 노드들이 서로 이해할 수 있는 공통의 좌표계를 유지하기 위해 사용하는 핵심 라이브러리이다. tf2는 프레임(frame)이라고 불리는 좌표계를 전파하고, 이를 통해 다양한 센서 혹은 로봇의 링크(link) 간 상대 위치 및 자세(orientation)을 손쉽게 다룰 수 있게 한다. 그리고 이러한 좌표계를 지속적으로 생성 및 갱신해주는 것이 바로 브로드캐스터(broadcaster)의 역할이다. 브로드캐스터는 크게 두 가지 종류가 존재한다. 하나는 "정적" 좌표계를 송신하는 목적의 `StaticTransformBroadcaster`이며, 다른 하나는 "동적" 좌표계를 송신하는 `TransformBroadcaster`다. 정적 브로드캐스터는 시간에 따라 변하지 않는 좌표계를 방송하는 데 사용하고, 동적 브로드캐스터는 로봇 조인트나 센서가 움직일 때처럼 시간이 흐름에 따라 변하는 좌표계를 방송하는 데 사용한다. #### 좌표계 표현 기초 tf2는 로봇 또는 센서가 놓여 있는 물리 세계의 3차원 좌표 정보를 다음과 같은 요소로 나누어 표현한다. 1. 위치(Position): 3차원 공간에서의 점을 나타내며, 보통 벡터 $\mathbf{t} = \begin{bmatrix} x \ y \ z \end{bmatrix}$ 형태로 표현한다. 2. 자세(Orientation): 3차원 공간 상의 회전을 나타내며, 주로 쿼터니언(Quaternion) 혹은 회전 행렬(Rotation matrix)로 표현한다. tf2에서는 오일러 각(Euler angles)을 사용하기도 하지만, 내부적으로는 주로 쿼터니언으로 연산한다. 이를 종합하여, 어떤 프레임 A에서 프레임 B로의 좌표 변환(transform)을 $^A\mathbf{T}\_B$라고 표현하면, $$ ^A\mathbf{T}*B = \begin{bmatrix} \mathbf{R}*{AB} & \mathbf{t}\_{AB} \ 0 & 1 \end{bmatrix} $$ 와 같이 4x4 변환 행렬을 통해 나타낼 수 있다. 여기서 $\mathbf{R}*{AB}$는 3x3 회전 행렬, $\mathbf{t}*{AB}$는 3x1 변환 벡터이다. 실제 tf2 메시지에서는 이를 geometry\_msgs/msg/TransformStamped 형태로 송수신한다. 결과적으로 tf2에서 중요한 점은 "내가 원하는 대상 프레임(child\_frame)에 대해 어떤 기준 프레임(parent\_frame)으로부터의 변환 관계를 지속적으로 방송"하는 것이다. 예를 들어 로봇의 베이스 좌표계(base\_link)와 레이저 센서 좌표계(laser\_link), 카메라 좌표계(camera\_link) 간의 상대 위치 및 자세 정보를 공개해주어야 다른 노드들이 필요할 때 정확히 좌표 변환을 질의할 수 있다. #### tf2\_ros 라이브러리 구성 tf2 기능은 보통 ROS2 패키지 중 `tf2_ros`를 통해 제공된다. 이 라이브러리에는 다음과 같은 노드 또는 클래스를 사용할 수 있다. * `tf2_ros::TransformBroadcaster`: 동적으로 변하는 좌표계를 방송 * `tf2_ros::StaticTransformBroadcaster`: 변하지 않는 정적 좌표계를 방송 * `tf2_ros::Buffer`와 `tf2_ros::TransformListener`: 브로드캐스팅된 좌표계를 수신하고, 좌표 변환 연산(lookupTransform, transform 등)을 제공 사용자는 일반적으로 C++ 또는 Python 노드 내에서 `TransformBroadcaster` 혹은 `StaticTransformBroadcaster` 객체를 생성해, 특정 주기에 맞추어 `TransformStamped` 메시지를 퍼블리시(publish)한다. #### Transform 메시지 구조 `geometry_msgs/msg/TransformStamped` 메시지를 살펴보면, 좌표 변환을 표현하기 위한 필드가 포함되어 있다. 핵심 필드는 다음과 같다. * `header.stamp`: 이 변환 정보가 유효한 시점(Time) * `header.frame_id`: 기준 프레임(parent\_frame, 예: "world" 또는 "map") * `child_frame_id`: 변환 대상 프레임(child\_frame, 예: "base\_link" 또는 "camera\_link") * `transform.translation`: 위치 변환 벡터(예: x, y, z) * `transform.rotation`: 자세 변환 쿼터니언(예: x, y, z, w) 이를 코드로 작성해 브로드캐스트하게 되면, tf2는 자동으로 여러 노드가 이 정보를 활용할 수 있도록 퍼블리싱한다. #### 예시 코드: StaticTransformBroadcaster (Python) ROS2에서 좌표계를 정적으로 정의하고 싶을 때는 `StaticTransformBroadcaster`를 사용하면 편리하다. 예를 들어, 로봇의 기본 좌표계인 `base_link`로부터 카메라 좌표계인 `camera_link`가 항상 고정되어 있다고 가정하자. 이를 방송하기 위한 예시 파이썬 코드는 다음과 같다. ```python import rclpy from rclpy.node import Node from geometry_msgs.msg import TransformStamped from tf2_ros import StaticTransformBroadcaster import math class StaticFramePublisher(Node): def __init__(self): super().__init__('static_tf_broadcaster') self._tf_publisher = StaticTransformBroadcaster(self) self.publish_static_transform() def publish_static_transform(self): t = TransformStamped() t.header.stamp = self.get_clock().now().to_msg() t.header.frame_id = 'base_link' t.child_frame_id = 'camera_link' # 위치 (x, y, z) t.transform.translation.x = 0.5 t.transform.translation.y = 0.0 t.transform.translation.z = 1.0 # 카메라가 로봇 전방을 바라보는 것으로 가정 # RPY = (0, 0, 0) -> 쿼터니언 (0, 0, 0, 1) # 필요하면 math 라이브러리 등을 이용해 오일러->쿼터니언 변환 t.transform.rotation.x = 0.0 t.transform.rotation.y = 0.0 t.transform.rotation.z = 0.0 t.transform.rotation.w = 1.0 # 브로드캐스트 self._tf_publisher.sendTransform(t) def main(args=None): rclpy.init(args=args) node = StaticFramePublisher() try: rclpy.spin(node) except KeyboardInterrupt: pass rclpy.shutdown() if __name__ == '__main__': main() ``` 위 코드는 정적 좌표계 `base_link -> camera_link`를 항상 방송한다. launch 파일을 작성하여 백그라운드에서 구동시켜 놓으면, 다른 노드에서 자동으로 변환 정보를 획득할 수 있다. #### 동적 좌표계 브로드캐스터(TransformBroadcaster) 정적 좌표계는 한 번 정의하면 시간에 따라 변하지 않는 경우에 주로 사용한다. 하지만 로봇이나 센서가 움직이면서 시시각각 변하는 좌표계를 다루고 싶다면, `TransformBroadcaster`를 사용하여 끊임없이 변환 정보를 갱신해주어야 한다. 예를 들어 이동 로봇의 바퀴 회전으로 인해 로봇 본체(base\_link)가 주행 중이라면, 현재 로봇 위치를 나타내는 프레임(예: "odom" 또는 "map" 프레임에 대한 base\_link 프레임)을 매 순간 업데이트할 필요가 있다. #### 구현 방식 개요 `TransformBroadcaster`는 다음 과정을 통해 동적 좌표계를 방송한다. 1. 주기적(peridoc)으로 혹은 이벤트가 발생할 때마다 `TransformStamped` 메시지를 생성한다. 2. 메시지에 현재 시각 및 프레임 변환 정보를 채워 넣는다. 3. `TransformBroadcaster` 객체의 `sendTransform(...)` 메서드를 호출해 전송한다. 4. tf2는 이 메시지를 받아, 로봇 운영 중에 발생하는 좌표계 변화를 축적한다. #### 동적 좌표계 브로드캐스터(Python) 예시 아래는 시간에 따라 이동하는 "turtle" 프레임을 예시로 한 파이썬 코드이다. 이 예시에서는 시뮬레이션 또는 임의의 계산을 통해 $x, y, \theta$가 변한다고 가정하고, 해당 정보를 바탕으로 좌표 변환 메시지를 생성해 방송한다. ```python import rclpy from rclpy.node import Node import math from geometry_msgs.msg import TransformStamped from tf2_ros import TransformBroadcaster class TurtleTfBroadcaster(Node): def __init__(self): super().__init__('turtle_tf_broadcaster') self._br = TransformBroadcaster(self) self._timer = self.create_timer(0.1, self.timer_callback) # 가상의 이동체 좌표 초기화 self.x = 0.0 self.y = 0.0 self.theta = 0.0 def timer_callback(self): # 간단한 예시로 일정 속도로 회전, 전진한다고 가정 self.x += 0.01 self.y += 0.0 self.theta += 0.01 # TransformStamped 메시지 생성 t = TransformStamped() t.header.stamp = self.get_clock().now().to_msg() t.header.frame_id = 'world' t.child_frame_id = 'turtle' # 위치 변환 t.transform.translation.x = self.x t.transform.translation.y = self.y t.transform.translation.z = 0.0 # 오일러 각 -> 쿼터니언 변환 qx, qy, qz, qw = euler_to_quaternion(0.0, 0.0, self.theta) t.transform.rotation.x = qx t.transform.rotation.y = qy t.transform.rotation.z = qz t.transform.rotation.w = qw # 전송 self._br.sendTransform(t) def euler_to_quaternion(roll, pitch, yaw): # ROS 등에서 많이 쓰이는 오일러 -> 쿼터니언 변환 # 수식: # qx = sin(roll/2)*cos(pitch/2)*cos(yaw/2) - cos(roll/2)*sin(pitch/2)*sin(yaw/2) # qy = cos(roll/2)*sin(pitch/2)*cos(yaw/2) + sin(roll/2)*cos(pitch/2)*sin(yaw/2) # qz = cos(roll/2)*cos(pitch/2)*sin(yaw/2) - sin(roll/2)*sin(pitch/2)*cos(yaw/2) # qw = cos(roll/2)*cos(pitch/2)*cos(yaw/2) + sin(roll/2)*sin(pitch/2)*sin(yaw/2) cy = math.cos(yaw * 0.5) sy = math.sin(yaw * 0.5) cp = math.cos(pitch * 0.5) sp = math.sin(pitch * 0.5) cr = math.cos(roll * 0.5) sr = math.sin(roll * 0.5) qw = cr * cp * cy + sr * sp * sy qx = sr * cp * cy - cr * sp * sy qy = cr * sp * cy + sr * cp * sy qz = cr * cp * sy - sr * sp * cy return (qx, qy, qz, qw) def main(args=None): rclpy.init(args=args) node = TurtleTfBroadcaster() try: rclpy.spin(node) except KeyboardInterrupt: pass rclpy.shutdown() if __name__ == '__main__': main() ``` 위 노드가 실행되면, `world -> turtle` 변환 정보가 0.1초 간격으로 퍼블리시되어 시시각각 갱신된다. 이를 다른 노드에서 받아서, `tf2_ros::Buffer` 또는 rviz2 등 시각화 도구를 통해 실시간으로 변환 확인이 가능하다. #### 동적 좌표계 브로드캐스터(C++) 예시 C++에서도 유사한 방식으로 구현할 수 있다. `tf2_ros::TransformBroadcaster` 인스턴스를 생성하고, 타이머 콜백이나 메인 루프에서 주기적으로 `TransformStamped` 메시지를 생성하여 퍼블리시하면 된다. ```cpp #include #include #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include "tf2_ros/transform_broadcaster.h" class TurtleTfBroadcaster : public rclcpp::Node { public: TurtleTfBroadcaster() : Node("turtle_tf_broadcaster"), x_(0.0), y_(0.0), theta_(0.0) { br_ = std::make_unique(this); timer_ = this->create_wall_timer( std::chrono::milliseconds(100), std::bind(&TurtleTfBroadcaster::timerCallback, this)); } private: void timerCallback() { // 이동체 좌표 업데이트 (가정) x_ += 0.01; theta_ += 0.01; geometry_msgs::msg::TransformStamped t; t.header.stamp = this->now(); t.header.frame_id = "world"; t.child_frame_id = "turtle"; // 위치 변환 t.transform.translation.x = x_; t.transform.translation.y = y_; t.transform.translation.z = 0.0; // 오일러 -> 쿼터니언 auto q = eulerToQuaternion(0.0, 0.0, theta_); t.transform.rotation.x = q[0]; t.transform.rotation.y = q[1]; t.transform.rotation.z = q[2]; t.transform.rotation.w = q[3]; br_->sendTransform(t); } std::array eulerToQuaternion(double roll, double pitch, double yaw) { double cy = std::cos(yaw * 0.5); double sy = std::sin(yaw * 0.5); double cp = std::cos(pitch * 0.5); double sp = std::sin(pitch * 0.5); double cr = std::cos(roll * 0.5); double sr = std::sin(roll * 0.5); double qw = cr * cp * cy + sr * sp * sy; double qx = sr * cp * cy - cr * sp * sy; double qy = cr * sp * cy + sr * cp * sy; double qz = cr * cp * sy - sr * sp * cy; return {qx, qy, qz, qw}; } rclcpp::TimerBase::SharedPtr timer_; std::unique_ptr br_; double x_, y_, theta_; }; int main(int argc, char** argv) { rclcpp::init(argc, argv); auto node = std::make_shared(); rclcpp::spin(node); rclcpp::shutdown(); return 0; } ``` 이 코드는 파이썬 예제와 거의 동일한 논리로 구성되며, 매 100ms마다 이동체(가상의 "turtle")가 위치를 갱신하고 그에 따른 `TransformStamped` 메시지를 브로드캐스팅한다. #### 좌표계와 프레임 구조 설계 시 고려 사항 1. **프레임 계층(Transform Tree) 정의**: ROS TF에서 모든 좌표계는 트리(tree) 구조를 이룬다. 일반적으로 제일 상위 프레임(예: "map" 또는 "world")을 두고, 그 아래에 로봇 base\_link, 센서 링크 등을 차례로 이어간다. 2. **프레임 이름 규칙**: "base\_link", "odom", "map", "camera\_link", "lidar\_link" 등이 관례적으로 사용된다. 언더바(\_) 대신 슬래시(/)를 사용하면 tf tree 구조 파악이 복잡해질 수 있으므로, 노드 이름과 헷갈리지 않도록 주의한다. 3. **시간 동기화**: 헤더에 기록되는 `stamp` 필드는 이 변환이 정확히 언제 발생했는지를 나타낸다. 실시간 센서 데이터와 변환 정보가 어긋나지 않도록 적절한 시간 동기화가 중요하다. 4. **쿼터니언 정규화**: 오일러 각을 쿼터니언으로 변환할 때, 혹은 쿼터니언을 직접 계산할 때는 0에 근접한 오차로 인해 정규화(norm) 과정을 거치는 것이 안정적이다. #### 좌표 변환 확인 및 시각화 tf2는 여러 노드가 퍼블리시하는 좌표계(프레임)들의 관계를 통합 관리한다. 이를 확인하거나 디버깅하기 위해서는 다음과 같은 도구들을 활용할 수 있다. 1. `tf2_echo`를 통한 간단 확인 2. RViz2에서 TF 플러그인을 통한 시각적 확인 #### tf2\_echo를 통한 확인 ROS2에서는 `tf2_ros` 패키지의 `tf2_echo` 노드를 통해 두 프레임 간 변환 정보를 실시간으로 터미널에서 조회할 수 있다. ```bash # usage: ros2 run tf2_ros tf2_echo ros2 run tf2_ros tf2_echo world turtle ``` 위 명령어는 `world` 프레임에서 `turtle` 프레임으로의 변환(위치, 쿼터니언 등)을 주기적으로 출력한다. 브로드캐스터가 정상적으로 동작하고 있다면, 출력 정보가 시간에 따라 갱신되는 것을 확인할 수 있다. #### RViz2를 통한 시각적 확인 RViz2에서 TF를 시각화하려면, Display 항목에서 "TF" 또는 "TF2"를 추가하면 된다. 이때 * "TF" Display를 활성화하면, 현재 tf 트리에 존재하는 모든 프레임과 그 연결 관계가 3D 공간 위에 표시된다. * 사용자는 마우스로 화면을 회전시키며 센서나 로봇 링크의 좌표축이 어떻게 배치되는지 시각적으로 파악할 수 있다. 아래는 예시로, "map -> odom -> base\_link -> laser\_link -> camera\_link" 형태의 tf 트리를 머메이드(mermaid) 다이어그램으로 나타낸 것이다. {% @mermaid/diagram content="flowchart LR A\[map] --> B\[odom] B --> C\[base\_link] C --> D\[laser\_link] C --> E\[camera\_link]" %} 이런 식으로 트리가 구성되어 있으면, 상위 프레임에서 하위 프레임으로 이어지는 변환(회전/평행이동)이 순차적으로 적용된다. RViz2의 3D 화면에서 각 축(x, y, z)을 확인하고, 노드 간의 간격이나 회전 각도를 바로 눈으로 확인할 수 있다. #### 올바른 TF 트리 유지 ROS2 tf2가 올바르게 동작하기 위해서는 모든 노드가 “트리” 구조를 유지해야 한다. * \*\*루프(Loop)\*\*가 있으면 안 된다. 예: `map -> base_link`를 브로드캐스팅하고, 동시에 어떤 다른 노드가 `base_link -> map`을 브로드캐스팅하는 경우. * \*\*유일한 경로(Unique Path)\*\*가 보장되어야 한다. 예: `camera_link`가 동시에 `base_link`와 `lidar_link` 모두로부터 직접 부모-자식 관계를 가지면 곤란하다(의도적으로 중복 선언하는 것은 매우 특별한 경우가 아니면 지양). #### tf2 시간 관리 tf2는 "시간" 정보를 매우 엄격하게 취급한다. 예를 들어, 센서 메시지의 타임스탬프와 TF 변환 메시지의 타임스탬프가 크게 어긋나면, 변환을 조회(lookupTransform)할 때 "Extrapolation into the future" 오류나 "Lookup would require extrapolation into the past" 등의 예외가 발생할 수 있다. * **타임 스탬프 맞추기**: 센서 또는 odometry 메시지의 시간과 가능한 한 일치하는 시점에 TF도 갱신되도록 구성. * **tf\_buffer\_duration**: `tf2_ros::Buffer`에 대한 캐시 시간도 적절히 설정. 이는 특히 로봇이 빠르게 움직이거나 센서 주기가 높은 경우 중요하다. #### tf2 관행적 프레임 명칭 * **map**: 세계 좌표, 지도 등과 같은 전역 좌표 * **odom**: 주행에 따라 누적 오차가 조금씩 발생할 수 있는 위치 추정 좌표 * **base\_link**: 로봇 자체의 기준 프레임 * **camera\_link**, **lidar\_link**, **imu\_link** 등: 센서 장치 부착 위치 이러한 계층 구조를 체계적으로 유지하는 것이 tf2 사용에 있어서 핵심이다.