import rclpy
from rclpy.node import Node
from geometry_msgs.msg import TransformStamped
import tf2_ros
from rclpy.time import Time

class TestTF(Node):
    def __init__(self):
        super().__init__("test_tf")

        self.buffer = tf2_ros.Buffer()
        self.listener = tf2_ros.TransformListener(self.buffer, self)

        self.create_timer(0.5, self.check)

    def check(self):
        try:
            tf = self.buffer.lookup_transform(
                'openarm_body_link0',
                'openarm_left_link7',
                Time()
            )
            print("Transform:", tf)
            self.get_logger().info("TF OK")
        except:
            self.get_logger().warn("TF not available yet")

def main():
    rclpy.init()
    node = TestTF()
    rclpy.spin(node)
    rclpy.shutdown()
