#!/usr/bin/env python2
# license removed for brevity
import time

import rospy
from sensor_msgs.msg import JointState
from std_msgs.msg import Header
from visualization_msgs.msg import Marker
import tf


def talker():
    rospy.init_node('following_marker', anonymous=True)

    pub_marker = rospy.Publisher('visualization_marker', Marker, queue_size=10)
    rate = rospy.Rate(20)


    listener = tf.TransformListener()

    marker_ = Marker()
    marker_.header.frame_id = '/joint1'
    marker_.ns = 'basic_cube'

    print('publishing ...')
    while not rospy.is_shutdown():
        now = rospy.Time.now() - rospy.Duration(0.1)
        # now = rospy.Time.now() 
        # print(now)

        try:
            trans, rot = listener.lookupTransform('joint1', 'basic_shapes', now)
        except Exception as e:
            print(e)
            continue

        print(type(trans), trans)
        print(type(rot), rot)

        # marker
        marker_.header.stamp = now
        marker_.type = marker_.CUBE
        marker_.action = marker_.ADD
        marker_.scale.x = 0.04
        marker_.scale.y = 0.04
        marker_.scale.z = 0.04

        # marker position initial
        marker_.pose.position.x = trans[0]
        marker_.pose.position.y = trans[1]
        marker_.pose.position.z = trans[2] - 0.02
        marker_.pose.orientation.x = rot[0]
        marker_.pose.orientation.y = rot[1]
        marker_.pose.orientation.z = rot[2]
        marker_.pose.orientation.w = rot[3]

        marker_.color.a = 1.0
        marker_.color.g = 1.0
        pub_marker.publish(marker_)

        rate.sleep()


if __name__ == '__main__':
    try:
        talker()
    except rospy.ROSInterruptException:
        pass