#!/usr/bin/env python

import sys
import math
import json

import tf2_geometry_msgs
import rospy
import tf2_ros 
from tf.transformations import quaternion_from_euler, euler_from_quaternion
from geometry_msgs.msg import TransformStamped, Vector3, PoseStamped
from aruco_msgs.msg import MarkerArray



def get_aruco_marker(aruco):
    # Extract markers from /aruco/markers
    for m in aruco.markers:
        tf = transform_from_marker(m)
        if tf:
            broadcaster.sendTransform(tf)



def transform_from_marker(m):
    global tf_buf

    # Create marker
    marker = PoseStamped()
    marker.pose = m.pose.pose
    marker.header.frame_id = 'camera_link'
    marker.header.stamp = rospy.Time.now()

    # Check if tranform if avaliable 
    if not tf_buf.can_transform(marker.header.frame_id, 'map', marker.header.stamp):
        rospy.logwarn_throttle(5.0, 'No transform from %s to map' % marker.header.frame_id)
        return

    # Transform from camera_link to map 
    poseInMap = tf_buf.transform(marker, 'map')

    # Create transformedStamped to publish
    tfArucoInMap = TransformStamped()
    tfArucoInMap.header.frame_id = 'map'
    tfArucoInMap.child_frame_id = 'aruco/detected' + str(m.id)
    tfArucoInMap.transform.translation = poseInMap.pose.position
    tfArucoInMap.transform.rotation = poseInMap.pose.orientation

    return tfArucoInMap



if __name__ == "__main__":
    rospy.init_node('detectaruco')
    aruco_sub = rospy.Subscriber('/aruco/markers',MarkerArray,get_aruco_marker)
    broadcaster = tf2_ros.TransformBroadcaster()
    tf_buf = tf2_ros.Buffer()
    tf_listner = tf2_ros.TransformListener(tf_buf)

    rospy.spin()





