"""
Calibrate with the ROS package aruco_detect
"""

import rospy
import roslib

from geometry_msgs.msg import Transform

class ROSArUcoCalibrate:

    def __init__(self, aruco_tag_len=0.0795):
        print("Please roslaunch roslaunch aruco_detect aruco_detect.launch before you run!")
        self.aruco_tf_topic = "/fiducial_transforms"
        self._aruco_tf_info_sub = rospy.Subscriber(self.aruco_tf_topic, Transform, self._tfCb)
        self.aruco_tf = None

    def _tfCb(self, tf_msg):
        if tf_msg is None:
            rospy.logwarn("_tfCb: tf_msg is None!")
        
        self.aruco_tf = tf_msg
    
    def get_tf(self):
        aruco_tf = self.aruco_tf
        return aruco_tf


[((15, 34, 15, 94), 'rospy.Subscriber', 'rospy.Subscriber', ({(15, 51, 15, 70): 'self.aruco_tf_topic', (15, 72, 15, 81): 'Transform', (15, 83, 15, 93): 'self._tfCb'}, {}), '(self.aruco_tf_topic, Transform, self._tfCb)', False, 'import rospy\n'), ((20, 12, 20, 51), 'rospy.logwarn', 'rospy.logwarn', ({(20, 26, 20, 50): '"""_tfCb: tf_msg is None!"""'}, {}), "('_tfCb: tf_msg is None!')", False, 'import rospy\n')]