import sys
import rospy

from marker_pos_angle.msg import id_pos_angle
from fiducial_msgs.msg import FiducialTransformArray
from tf.transformations import euler_from_quaternion

pub = rospy.Publisher('marker_id_pos_angle', id_pos_angle, queue_size=10)
'''
This callback function listens to fiducial messages that describe the translation and rotation of markers w.r.t. to the 
camera and transforms them to a position in the 2d plane and the angle of the robot in the plane
'''
def callback(data):
    for d in data.transforms:
        id = d.fiducial_id

        q = d.transform.rotation
        t = d.transform.translation

        euler = euler_from_quaternion([q.x, q.y, q.z, q.w])

        m = id_pos_angle()
        m.id = id
        m.x = t.x
        m.y = -t.y
        m.angle = -euler[2]

        pub.publish(m)

def main(args):
    rospy.init_node('euler_quaternion_node', anonymous=True)

    quaternion_sub = rospy.Subscriber("/fiducial_transforms", FiducialTransformArray, callback)

    rospy.spin()

if __name__ == '__main__':
    main(sys.argv)