00001
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
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
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
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
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
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
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
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
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
00174 (table, clusters) = call_object_detector()
00175
00176
00177
00178 while(not rospy.is_shutdown()):
00179
00180
00181 for cluster in clusters:
00182 draw_functions.clear_grasps(num=1000)
00183
00184
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
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
00206
00207
00208 original_probs = [grasp.success_probability for grasp in grasps]
00209 print "original probs:", pplist(original_probs)
00210
00211
00212 for grasp in grasps:
00213 grasp.success_probability = 0
00214 raw_input('press enter to evalute returned grasps')
00215
00216
00217 probs = call_evaluate_point_cluster_grasps(cluster, grasps)
00218 print "new probs: ", pplist(probs)
00219
00220
00221
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
00227
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
00235 draw_functions.clear_grasps(num = 1000)
00236
00237 elif c == 'q':
00238 break
00239 else:
00240 continue
00241 break
00242