EP6 : เริ่มต้นใช้งาน TF ใน ROS 2

รู้จักกับ TF ใน ROS 2

ระบบ TF ใน ROS 2 เป็นระบบที่ใช้จัดการการแปลงพิกัด (coordinate transform) ระหว่างเฟรม (frame) ต่าง ๆ ของหุ่นยนต์ เช่น จาก map ไปยัง odom หรือจาก base_link ไปยัง camera_link เป็นต้น

TF คืออะไร?

TF ย่อมาจาก “Transform” คือระบบที่ช่วยให้ ROS ทราบว่าแต่ละเฟรมอยู่ตรงไหนของโลกในช่วงเวลาต่าง ๆ การใช้ TF ทำให้สามารถติดตามตำแหน่งของหุ่นยนต์หรืออุปกรณ์เซ็นเซอร์ได้อย่างแม่นยำ

ตัวอย่าง TF Tree

ตัวอย่างโครงสร้าง TF ของหุ่นยนต์เคลื่อนที่พื้นจะมีลักษณะคล้ายต้นไม้:

• map → odom
• odom → base_link
• base_link → lidar_link, camera_link

การ Broadcast TF ใน ROS 2

การส่งข้อมูล TF ใช้ tf2_ros::TransformBroadcaster เช่น:

geometry_msgs::msg::TransformStamped t;
t.header.stamp = node->now();
t.header.frame_id = "base_link";
t.child_frame_id = "camera_link";
t.transform.translation.x = 0.1;
t.transform.rotation = tf2::toMsg(tf2::Quaternion(0, 0, 0, 1));

broadcaster->sendTransform(t);

การอ่าน TF ด้วย Listener

เพื่อรับค่าการแปลง สามารถใช้ tf2_ros::Buffer กับ TransformListener:

geometry_msgs::msg::TransformStamped transformStamped;
transformStamped = tf_buffer->lookupTransform(
    "base_link", "camera_link", tf2::TimePointZero);

ดู TF ผ่าน RViz

ใน RViz สามารถเพิ่ม Display ชื่อ "TF" เพื่อดูว่ามีเฟรมอะไรบ้าง และเฟรมเชื่อมกันอย่างไร

สรุป

TF คือองค์ประกอบสำคัญในการทำ localization, mapping, และ navigation การเข้าใจโครงสร้างของ TF และการใช้งานจะช่วยให้พัฒนาระบบหุ่นยนต์ใน ROS 2 ได้อย่างถูกต้องและแม่นยำ

---

สร้าง ROS 2 Package สำหรับใช้งาน TF ด้วย Python (ROS 2 Humble)

บทความนี้จะสอนคุณสร้างแพ็กเกจใน ROS 2 Humble สำหรับใช้งาน TF2 ด้วยภาษา Python ตั้งแต่เริ่มต้น พร้อมตัวอย่าง Broadcaster และ Listener ที่ใช้จริงในหุ่นยนต์

ขั้นตอนที่ 1: สร้างแพ็กเกจ

cd ~/ros2_ws/src
ros2 pkg create --build-type ament_python tf2_py_demo --dependencies rclpy tf2_ros geometry_msgs

โครงสร้างไฟล์ภายใน

tf2_py_demo/
├── tf2_py_demo/
│   ├── __init__.py
│   ├── static_broadcaster.py
│   └── listener.py
├── package.xml
├── setup.py
├── setup.cfg
└── resource/
    └── tf2_py_demo

Static TF Broadcaster (Python)

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster
from geometry_msgs.msg import TransformStamped
import tf_transformations

class StaticFramePublisher(Node):
    def __init__(self):
        super().__init__('static_tf2_broadcaster')
        self.broadcaster = StaticTransformBroadcaster(self)

        t = TransformStamped()
        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = 'base_link'
        t.child_frame_id = 'sensor_link'
        t.transform.translation.x = 0.0
        t.transform.translation.y = 0.5
        t.transform.translation.z = 0.2

        q = tf_transformations.quaternion_from_euler(0, 0, 0)
        t.transform.rotation.x = q[0]
        t.transform.rotation.y = q[1]
        t.transform.rotation.z = q[2]
        t.transform.rotation.w = q[3]

        self.broadcaster.sendTransform(t)

rclpy.init()
rclpy.spin(StaticFramePublisher())
rclpy.shutdown()

TF Listener (Python)

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from tf2_ros import Buffer, TransformListener

class TFListener(Node):
    def __init__(self):
        super().__init__('tf2_listener_node')
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)

        self.create_timer(1.0, self.lookup)

    def lookup(self):
        try:
            t = self.tf_buffer.lookup_transform('base_link', 'sensor_link', rclpy.time.Time())
            self.get_logger().info(f"Transform: {t.transform.translation.x:.2f}, {t.transform.translation.y:.2f}, {t.transform.translation.z:.2f}")
        except Exception as e:
            self.get_logger().warn(str(e))

rclpy.init()
rclpy.spin(TFListener())
rclpy.shutdown()

แก้ไข setup.py

entry_points={
    'console_scripts': [
        'static_broadcaster = tf2_py_demo.static_broadcaster:main',
        'listener = tf2_py_demo.listener:main',
    ],
},

Build แพ็กเกจ

cd ~/ros2_ws
colcon build
. install/setup.bash

เรียกใช้งาน

ros2 run tf2_py_demo static_broadcaster
ros2 run tf2_py_demo listener

เปิดดูใน RViz

1. เปิด RViz ด้วยคำสั่ง rviz2
2. เพิ่ม Display → TF
3. สังเกตลูกศรสีใน RViz เชื่อมโยง base_link กับ sensor_link

สรุป

TF เป็นเครื่องมือสำคัญในการระบุตำแหน่งและทิศทางของเฟรมในระบบหุ่นยนต์ การสร้างแพ็กเกจ TF ด้วย Python บน ROS 2 Humble จะช่วยให้คุณทำงานร่วมกับข้อมูลเซ็นเซอร์และ navigation ได้ง่ายขึ้น