#!/usr/bin/env python

import rospy
import actionlib

import roslib
roslib.load_manifest('realsense_gazebo_plugin')

from trajectory_msgs.msg import *
from moveit_msgs.msg import *
from realsense_gazebo_plugin.msg import *

import math

waypoints = MultiDOFJointTrajectory() 
waypoints_received = False
resend_goal_counter = 0

# callback to receive waypoints from MoveIt
def get_waypoints(data):
	global waypoints, waypoints_received
	waypoints = data.trajectory[0].multi_dof_joint_trajectory
	waypoints_received = True

if __name__ == '__main__':
	rospy.init_node('client')

	#initialize and actionlib client
	client = actionlib.SimpleActionClient('waypoint', waypointAction)

	# waqit for actionlib server
	client.wait_for_server()

	#subscribe to the waypoints generated by moveit
	rospy.Subscriber('move_group/display_planned_path', DisplayTrajectory , get_waypoints)

	while not rospy.is_shutdown():
		while not waypoints_received:
			# wait for waypoints from MoveIt
			pass

		waypoints_received = False # reset the flag

		for i in waypoints.points:
			resend_goal_counter = 0
			
			if waypoints_received:
				# if new waypoints are received, break
				break

			# set the goal and send it to server
			goal = waypointGoal()
			goal.waypoint.translation = i.transforms[0].translation
			goal.waypoint.rotation = i.transforms[0].rotation
			client.send_goal(goal)
			
			# wait for 10 seconds for the server to complete its action
			while not client.wait_for_result(rospy.Duration(10)) and resend_goal_counter < 5 and not waypoints_received:
				# if server is not done within 10 seconds, resend the goal if the goal has been resent less than 5 times
				resend_goal_counter = resend_goal_counter + 1
				client.send_goal(goal)