import rclpy
from rclpy.node import Node
import roboticstoolbox as rtb

class GET_URDF(Node):
    def __init__(self):
        super().__init__('GET_URDF')

        # Declare parameter and get it
        self.urdf_value = self.get_parameter('robot_description')


        if not self.urdf_value:
            self.get_logger().error("URDF is empty! Check if robot_state_publisher is correctly publishing it.")
            self.urdf_string = None
        else:
            #self.urdf_string = urdf_value
            self.get_logger().info("Successfully got URDF from ROS 2.")

    def get_urdf(self):
        return self.urdf_value

def main():
    rclpy.init()
    node = GET_URDF()
    
    print(node.get_urdf())
    """
    if urdf_string:
        # Print the first 500 characters to check
        print(f"URDF (first 500 chars):\n{urdf_string[:500]}")

        # Save to file and use in Robotic Toolbox
        urdf_path = "/tmp/robot.urdf"
        with open(urdf_path, "w") as file:
            file.write(urdf_string)

        print(f"URDF saved to {urdf_path}")

        # Load into Robotic Toolbox
        robot = rtb.ERobot.URDF(urdf_path)
        print(robot)
"""
    node.destroy_node()
    rclpy.shutdown()

if __name__ == "__main__":
    main()
