#!/usr/bin/env python

import rospy
import simple_moveit_interface as smi

#Requirements:
#- latest cob_bringup layer with lookat_component
#- sensor input for moveit enabled?
#- robot facing a wall in the background
#- lookat_action server and monitor started?
#- lookat works for both arm positions?
#
#- These positions currently are cob3-3 specific


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

	smi.add_ground()
	rospy.sleep(1.0)

	#while rospy.get_time() == 0.0: pass
	rospy.logerr('Are you using cob3-3? Did you make sure all requirements are met? Read the code file!')
	ans = raw_input('[y/n] ')
	if ans == 'y':
		while not rospy.is_shutdown():
			print "Going Left"
			success = smi.moveit_joint_goal("arm", [0.6132456177400849, -1.2667904686872227, -1.3351177346743512, 0.623757247043325, 2.6443916288424734, 0.2923237627622837, -2.121120037112681], True)
			#success = smi.moveit_joint_goal("arm", [0.4474927123777702, -1.4042363503909014, -1.3427710033793065, 0.8556133802524375, 2.8670612914249532, 0.2447635934094972, -1.8559613290891581], True)
			print "Going Right"
			success = smi.moveit_joint_goal("arm", [-0.4429381361070571, -2.027464518329273, 0.5376498408783335, -0.07512923290336566, -0.5249117648804152, 0.4386535797226253, -0.7085319372561121], True)
			#success = smi.moveit_joint_goal("arm", [-0.2875421191578158, -1.8672004116184473, 0.5368099709961187, -0.07435202034076506, -0.523647727633693, 0.43781158053874125, -0.709280142446791], True)
	elif ans == 'n':
		rospy.logerr('Please activate it!')


