#!/usr/bin/env python

import rospy, math
from ar_track_alvar_msgs.msg import AlvarMarkers
from tf.transformations import euler_from_quaternion

arData = {"DX":0.0,"DY":0.0,"DZ":0.0,"AX":0.0,"AY":0.0,"AZ":0.0,"AW":0.0}
roll, pitch, yaw = 0, 0, 0

def callback(msg):
	global arData
	for i in msg.markers:
		arData["DX"] = i.pose.pose.position.x
		arData["DY"] = i.pose.pose.position.y
		arData["DZ"] = i.pose.pose.position.z

		arData["AX"] = i.pose.pose.orientation.x
		arData["AY"] = i.pose.pose.orientation.y
		arData["AZ"] = i.pose.pose.orientation.z
		arData["AW"] = i.pose.pose.orientation.w

rospy.init_node('guide')
rospy.Subscriber('ar_pose_marker', AlvarMarkers, callback)

while not rospy.is_shutdown():
	(roll, pitch, yaw) = euler_from_quaternion((arData["AX"], arData["AY"], arData["AZ"], arData["AW"]))

	roll = math.degrees(roll)
	pitch = math.degrees(pitch)
	yaw = math.degrees(yaw)

	print(" roll : "+ str(roll))
	print("pitch : "+ str(pitch))
	print(" yaw : "+ str(yaw))

	print("  x  : "+ str(arData["DX"]))
	print("  y  : "+ str(arData["DY"]))
	print("  z  : "+ str(arData["DZ"]))
