$search
00001 #!/usr/bin/python 00002 ''' 00003 Test client for point_cluster_grasp_planner. 00004 Requires tabletop_node.launch to be running (in tabletop_object_detector). 00005 Add Markers of topic 'grasp_markers' to see the poses being tested. 00006 ''' 00007 00008 from __future__ import division 00009 import roslib 00010 roslib.load_manifest('pr2_pick_and_place_demos') 00011 import rospy 00012 import actionlib 00013 from object_manipulation_msgs.srv import FindClusterBoundingBox, FindClusterBoundingBoxRequest 00014 from tabletop_object_detector.srv import TabletopDetection, TabletopDetectionRequest 00015 from object_manipulation_msgs.srv import GraspPlanning, GraspPlanningRequest 00016 from object_manipulation_msgs.msg import GraspPlanningAction, GraspPlanningGoal 00017 from pr2_gripper_grasp_planner_cluster.srv import SetPointClusterGraspParams, SetPointClusterGraspParamsRequest 00018 from visualization_msgs.msg import Marker 00019 import tf.transformations 00020 import numpy 00021 import scipy 00022 import time 00023 import object_manipulator.draw_functions as draw_functions 00024 from object_manipulator.convert_functions import * 00025 00026 00027 #call the tabletop object detector to get the table and clusters in the scene 00028 def call_object_detector(): 00029 00030 req = TabletopDetectionRequest() 00031 req.return_clusters = 1 00032 req.return_models = 0 00033 service_name = "/object_detection" 00034 rospy.loginfo("waiting for object_detection service") 00035 rospy.wait_for_service(service_name) 00036 serv = rospy.ServiceProxy(service_name, TabletopDetection) 00037 try: 00038 res = serv(req) 00039 except rospy.ServiceException, e: 00040 rospy.logerr("error when calling object_detection: %s"%e) 00041 return 0 00042 return (res.detection.table, res.detection.clusters) 00043 00044 00045 #call the service to set the params for the cluster planner 00046 def call_set_params(side_step = 0.02, palm_step = 0.005, overhead_grasps_only = False, side_grasps_only = False, include_high_point_grasps = True, pregrasp_just_outside_box = False, backoff_depth_steps = 5): 00047 00048 req = SetPointClusterGraspParamsRequest() 00049 req.height_good_for_side_grasps = 0.05 00050 req.gripper_opening = 0.083 00051 req.side_step = side_step 00052 req.palm_step = palm_step 00053 req.overhead_grasps_only = overhead_grasps_only 00054 req.side_grasps_only = side_grasps_only 00055 req.include_high_point_grasps = include_high_point_grasps 00056 req.pregrasp_just_outside_box = pregrasp_just_outside_box 00057 req.backoff_depth_steps = backoff_depth_steps 00058 req.randomize_grasps = False 00059 rospy.loginfo("waiting for set params service") 00060 rospy.wait_for_service("set_point_cluster_grasp_params") 00061 serv = rospy.ServiceProxy("set_point_cluster_grasp_params", SetPointClusterGraspParams) 00062 try: 00063 res = serv(req) 00064 except rospy.ServiceException, e: 00065 rospy.logerr("error when calling set params: %s"%e) 00066 return 0 00067 return 1 00068 00069 00070 #call plan_point_cluster_grasp to get candidate grasps for a cluster 00071 def call_plan_point_cluster_grasp(cluster): 00072 00073 req = GraspPlanningRequest() 00074 req.target.reference_frame_id = "/base_link" 00075 req.target.cluster = cluster 00076 req.arm_name = "right_arm" 00077 req.collision_support_surface_name = "table" 00078 service_name = "plan_point_cluster_grasp" 00079 rospy.loginfo("waiting for plan_point_cluster_grasp service") 00080 rospy.wait_for_service(service_name) 00081 rospy.loginfo("service found") 00082 serv = rospy.ServiceProxy(service_name, GraspPlanning) 00083 try: 00084 res = serv(req) 00085 except rospy.ServiceException, e: 00086 rospy.logerr("error when calling plan_point_cluster_grasp: %s"%e) 00087 return 0 00088 if res.error_code.value != 0: 00089 return [] 00090 00091 return res.grasps 00092 00093 00094 #call plan_point_cluster_grasp_action to get candidate grasps for a cluster 00095 def call_plan_point_cluster_grasp_action(cluster): 00096 00097 goal = GraspPlanningGoal() 00098 goal.target.reference_frame_id = "/base_link" 00099 goal.target.cluster = cluster 00100 goal.arm_name = "right_arm" 00101 goal.collision_support_surface_name = "table" 00102 action_name = "plan_point_cluster_grasp" 00103 rospy.loginfo("waiting for plan_point_cluster_grasp action") 00104 client = actionlib.SimpleActionClient(action_name, GraspPlanningAction) 00105 client.wait_for_server() 00106 rospy.loginfo("action found") 00107 client.send_goal(goal) 00108 client.wait_for_result() 00109 result = client.get_result() 00110 if not result or result.error_code.value != 0: 00111 return [] 00112 return result.grasps 00113 00114 00115 #call evaluate_point_cluster_grasps to evaluate grasps on a cluster 00116 def call_evaluate_point_cluster_grasps(cluster, grasps): 00117 00118 req = GraspPlanningRequest() 00119 req.target.reference_frame_id = "/base_link" 00120 req.target.cluster = cluster 00121 req.arm_name = "right_arm" 00122 req.collision_support_surface_name = "table" 00123 req.grasps_to_evaluate = grasps 00124 service_name = "evaluate_point_cluster_grasps" 00125 rospy.loginfo("waiting for evaluate_point_cluster_grasps service") 00126 rospy.wait_for_service(service_name) 00127 rospy.loginfo("service found") 00128 serv = rospy.ServiceProxy(service_name, GraspPlanning) 00129 try: 00130 res = serv(req) 00131 except rospy.ServiceException, e: 00132 rospy.logerr("error when calling evaluate_point_cluster_grasps: %s"%e) 00133 return 0 00134 if res.error_code.value != 0: 00135 return [] 00136 00137 probs = [grasp.success_probability for grasp in res.grasps] 00138 return probs 00139 00140 00141 #call find_cluster_bounding_box to get the bounding box for a cluster 00142 def call_find_cluster_bounding_box(cluster): 00143 00144 req = FindClusterBoundingBoxRequest() 00145 req.cluster = cluster 00146 service_name = "find_cluster_bounding_box" 00147 rospy.loginfo("waiting for find_cluster_bounding_box service") 00148 rospy.wait_for_service(service_name) 00149 rospy.loginfo("service found") 00150 serv = rospy.ServiceProxy(service_name, FindClusterBoundingBox) 00151 try: 00152 res = serv(req) 00153 except rospy.ServiceException, e: 00154 rospy.logerr("error when calling find_cluster_bounding_box: %s"%e) 00155 return 0 00156 if not res.error_code: 00157 return (res.pose, res.box_dims) 00158 else: 00159 return (None, None) 00160 00161 00162 if __name__ == "__main__": 00163 00164 #initialize the node, tf listener and broadcaster, and rviz drawing helper class 00165 rospy.init_node('test_point_cluster_grasp_planner', anonymous=True) 00166 tf_broadcaster = tf.TransformBroadcaster() 00167 tf_listener = tf.TransformListener() 00168 draw_functions = draw_functions.DrawFunctions('grasp_markers') 00169 00170 #set params for planner (change to try different settings) 00171 call_set_params(overhead_grasps_only = False, side_grasps_only = False, include_high_point_grasps = False, pregrasp_just_outside_box = False, backoff_depth_steps = 1) 00172 00173 #call the tabletop object detector to detect clusters on the table 00174 (table, clusters) = call_object_detector() 00175 #print "table:\n", table 00176 00177 #keep going until the user says to quit (or until Ctrl-C is pressed) 00178 while(not rospy.is_shutdown()): 00179 00180 #for each cluster in view in turn 00181 for cluster in clusters: 00182 draw_functions.clear_grasps(num=1000) 00183 00184 #get the bounding box for this cluster and draw it in blue 00185 (box_pose, box_dims) = call_find_cluster_bounding_box(cluster) 00186 if box_pose == None: 00187 continue 00188 box_mat = pose_to_mat(box_pose.pose) 00189 box_ranges = [[-box_dims.x/2, -box_dims.y/2, -box_dims.z/2], 00190 [box_dims.x/2, box_dims.y/2, box_dims.z/2]] 00191 draw_functions.draw_rviz_box(box_mat, box_ranges, 'base_link', \ 00192 ns = 'bounding box', \ 00193 color = [0,0,1], opaque = 0.25, duration = 60) 00194 00195 print "Run the blue cluster? Enter y to run, enter to go to the next, q to exit" 00196 c = raw_input() 00197 00198 if c == 'y': 00199 draw_functions.clear_rviz_points('grasp_markers') 00200 00201 #call the grasp planner (both the service and the action in turn) 00202 grasps = call_plan_point_cluster_grasp(cluster) 00203 grasps = call_plan_point_cluster_grasp_action(cluster) 00204 grasp_poses = [grasp.grasp_pose for grasp in grasps] 00205 #pregrasp_poses = [grasp.pre_grasp_pose for grasp in grasps] 00206 00207 #print the returned success probs 00208 original_probs = [grasp.success_probability for grasp in grasps] 00209 print "original probs:", pplist(original_probs) 00210 00211 #zero out the original success probabilities 00212 for grasp in grasps: 00213 grasp.success_probability = 0 00214 raw_input('press enter to evalute returned grasps') 00215 00216 #ask the grasp planner to evaluate the grasps 00217 probs = call_evaluate_point_cluster_grasps(cluster, grasps) 00218 print "new probs: ", pplist(probs) 00219 #print '\n'.join([ppmat(pose_to_mat(grasp_pose)) for grasp_pose in grasp_poses]) 00220 00221 #draw the resulting grasps (all at once, or one at a time) 00222 print "number of grasps returned:", len(grasps) 00223 print "drawing grasps: press p to pause after each one in turn, enter to show all grasps at once" 00224 c = raw_input() 00225 if c == 'p': 00226 #for (pregrasp_pose, grasp_pose) in zip(pregrasp_poses, grasp_poses): 00227 # draw_functions.draw_grasps([pregrasp_pose, grasp_pose], cluster.header.frame_id, pause = 1) 00228 draw_functions.draw_grasps(grasp_poses, cluster.header.frame_id, pause = 1) 00229 else: 00230 draw_functions.draw_grasps(grasp_poses, cluster.header.frame_id, pause = 0) 00231 print "done drawing grasps, press enter to continue" 00232 raw_input() 00233 00234 #clear out the grasps drawn 00235 draw_functions.clear_grasps(num = 1000) 00236 00237 elif c == 'q': 00238 break 00239 else: 00240 continue 00241 break 00242