#!/usr/bin/env python

import rospy
import actionlib
import sys

from tiago_bartender_msgs.msg import PickAction, PickGoal

if __name__ == '__main__':
	rospy.init_node('test_pick')
	client = actionlib.SimpleActionClient('tiago_pick', PickAction)
	client.wait_for_server()

	if(len(sys.argv) > 1):
		goal = PickGoal(object_id= sys.argv[1])
	else:
		goal = PickGoal(object_id= "orange")

	client.send_goal(goal)
	client.wait_for_result(rospy.Duration(0.0))

	print("State:")
	print(client.get_state())
	print()
	print("Result:")
	print(client.get_result())
