#!/usr/bin/env python
import thread
import time
import copy
import rospy
from tf2_msgs.msg import TFMessage
from geometry_msgs.msg import *

def disp_pos(transform_temp):
    x=transform_temp.x*1000
    y=transform_temp.y*1000
    z=transform_temp.z*1000
    print("\rposition = %.02f   %.02f   %.02f"%(x,y,z)),
    #rospy.loginfo(rospy.get_caller_id() + " position\n %s______%s______%s"%(x,y,z))

def callback(data):
    transform_temp = copy.copy(data.transforms[0].transform.translation)
    disp_pos(transform_temp)
    #print float('%.12f'%(rospy.get_time()*10))
    #if(int(rospy.get_time()*100) % 10==0):
    #    disp_pos(transform_temp)

def listener():

    # In ROS, nodes are uniquely named. If two nodes with the same
    # name are launched, the previous one is kicked off. The
    # anonymous=True flag means that rospy will choose a unique
    # name for our 'listener' node so that multiple listeners can
    # run simultaneously.
    rospy.init_node('pos_display_node', anonymous=True)

    rospy.Subscriber("tf", TFMessage, callback)

    # spin() simply keeps python from exiting until this node is stopped
    rospy.spin()

if __name__ == '__main__':
    listener()
