pr2_grasp_behaviors: test_grasp_action.py Source File

Go to the documentation of this file.
00001 #! /usr/bin/python
00002 import random
00003 import sys
00004 
00005 import roslib; roslib.load_manifest("pr2_grasp_behaviors")
00006 import rospy
00007 import actionlib
00008 
00009 from pr2_grasp_behaviors.msg import *
00010 #from pr2_playpen.srv import *
00011             
00012 
00013 def main():
00014     args = sys.argv
00015     rospy.init_node('test_grasping')
00016     if args[1] not in ['r', 'l']:
00017         print "First arg should be 'r' or 'l'"
00018         return
00019     grasp_client = actionlib.SimpleActionClient(args[1] + '_overhead_grasp', OverheadGraspAction)
00020     grasp_client.wait_for_server()
00021     grasp_setup_client = actionlib.SimpleActionClient(args[1] + '_overhead_grasp_setup', OverheadGraspSetupAction)
00022     grasp_setup_client.wait_for_server()
00023     print "grasp_client ready"
00024     if args[2] == 'visual' or args[2] == 'manual':
00025         results_dict = {}
00026         obj_in_hand = False
00027         while not rospy.is_shutdown():
00028             goal = OverheadGraspSetupGoal()
00029             #goal.disable_head = True
00030             goal.open_gripper = True
00031             grasp_setup_client.send_goal(goal)
00032             #grasp_setup_client.wait_for_result()
00033             rospy.sleep(1.0)
00034 
00035             if not obj_in_hand:
00036                 print "Grasp started"
00037 
00038                 ############################################################
00039                 # Creating grasp goal
00040                 goal = OverheadGraspGoal()
00041                 goal.is_grasp = True
00042                 goal.disable_head = False
00043                 goal.disable_coll = False
00044                 if args[2] == 'visual':
00045                     goal.grasp_type = OverheadGraspGoal.VISION_GRASP
00046                 else:
00047                     goal.grasp_type = OverheadGraspGoal.MANUAL_GRASP
00048                 goal.x = 0.5 + random.uniform(-0.1, 0.1)
00049                 goal.y = 0.0 + random.uniform(-0.1, 0.1)
00050                 goal.behavior_name = "overhead_grasp"
00051                 goal.sig_level = 0.9975
00052                 ############################################################
00053 
00054                 rospy.sleep(1.0)
00055                 grasp_client.send_goal(goal)
00056                 rospy.sleep(1.0)
00057                 grasp_client.wait_for_result()
00058                 result = grasp_client.get_result()
00059                 if result.grasp_result not in results_dict:
00060                     results_dict[result.grasp_result] = 0
00061                 results_dict[result.grasp_result] += 1
00062                 print "Last result:", result.grasp_result, "Totals:", results_dict
00063                 obj_in_hand = result.grasp_result == "Object grasped"
00064             if obj_in_hand:
00065 
00066                 ############################################################
00067                 # Creating place goal
00068                 goal = OverheadGraspGoal()
00069                 goal.is_grasp = False
00070                 goal.disable_head = False
00071                 goal.disable_coll = False
00072                 goal.grasp_type = OverheadGraspGoal.MANUAL_GRASP
00073                 goal.x = 0.55 + random.uniform(-0.1, 0.1)
00074                 goal.y = 0.0 + random.uniform(-0.2, 0.2)
00075                 goal.behavior_name = "overhead_grasp"
00076                 goal.sig_level = 0.9975
00077                 ############################################################
00078 
00079                 rospy.sleep(1.0)
00080                 grasp_client.send_goal(goal)
00081                 rospy.sleep(1.0)
00082                 grasp_client.wait_for_result()
00083                 result = grasp_client.get_result()
00084                 if result.grasp_result not in results_dict:
00085                     results_dict[result.grasp_result] = 0
00086                 results_dict[result.grasp_result] += 1
00087                 obj_in_hand = result.grasp_result != "Object placed"
00088                 print "Last result:", result.grasp_result, "Totals:", results_dict
00089         return
00090     print "Format: python test_grasp_action.py [r|l] [manual|visual]"
00091 
00092 if __name__ == '__main__':
00093     main()