import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
from rclpy.executors import SingleThreadedExecutor

class JointStateReader(Node):
    def __init__(self):
        super().__init__('JointStateReader')
        self.sub = self.create_subscription(JointState, 'joint_states', self.sub_callback, 1)
        self.joint_names = []
        self.got_msg = False
    
    def sub_callback(self, msg):
        """Callback to read joint names from the first received message."""
        if not self.joint_names:
            self.got_msg=True
            self.joint_names = list(msg.name)

def joint_names():
    """Fetch joint names using an existing ROS2 context and ensure proper cleanup."""
    rclpy.init()
    node = JointStateReader()
    executor = SingleThreadedExecutor()
    executor.add_node(node)
    while not node.got_msg:
        executor.spin_once(timeout_sec=0)
    joint_list = node.joint_names  # Store names before shutting down

    node.destroy_node()  # Properly destroy the node
    rclpy.shutdown()  # Shutdown ROS2 context to free memory

    return joint_list  # Return the joint names safely

def main():
    """Main function to fetch and print joint names."""
    joints = joint_names()
    
    for i, joint_name in enumerate(joints, start=1):
        print(f"Joint No. {i}: {joint_name}")

if __name__ == '__main__':
    main()