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 from object_manipulation_msgs.srv import FindClusterBoundingBox, FindClusterBoundingBoxRequest
00013 from tabletop_object_detector.srv import TabletopDetection, TabletopDetectionRequest
00014 from object_manipulation_msgs.srv import GraspPlanning, GraspPlanningRequest
00015 from visualization_msgs.msg import Marker
00016 import tf.transformations
00017 import numpy
00018 import scipy
00019 import time
00020 import object_manipulator.draw_functions as draw_functions
00021 from object_manipulator.convert_functions import *
00022
00023
00024
00025 def call_object_detector():
00026
00027 req = TabletopDetectionRequest()
00028 req.return_clusters = 1
00029 req.return_models = 0
00030 service_name = "/object_detection"
00031 rospy.loginfo("waiting for object_detection service")
00032 rospy.wait_for_service(service_name)
00033 serv = rospy.ServiceProxy(service_name, TabletopDetection)
00034 try:
00035 res = serv(req)
00036 except rospy.ServiceException, e:
00037 rospy.logerr("error when calling object_detection: %s"%e)
00038 return 0
00039 return (res.detection.table, res.detection.clusters)
00040
00041
00042
00043 def call_plan_point_cluster_grasp(cluster):
00044
00045 req = GraspPlanningRequest()
00046 req.target.cluster = cluster
00047 req.target.type = 2
00048 req.arm_name = "right_arm"
00049 req.collision_support_surface_name = "table"
00050 service_name = "plan_point_cluster_grasp"
00051 rospy.loginfo("waiting for plan_point_cluster_grasp service")
00052 rospy.wait_for_service(service_name)
00053 rospy.loginfo("service found")
00054 serv = rospy.ServiceProxy(service_name, GraspPlanning)
00055 try:
00056 res = serv(req)
00057 except rospy.ServiceException, e:
00058 rospy.logerr("error when calling plan_point_cluster_grasp: %s"%e)
00059 return 0
00060 if res.error_code.value != 0:
00061 return []
00062
00063 return res.grasps
00064
00065
00066
00067 def call_evaluate_point_cluster_grasps(cluster, grasps):
00068
00069 req = GraspPlanningRequest()
00070 req.target.cluster = cluster
00071 req.target.type = 2
00072 req.arm_name = "right_arm"
00073 req.collision_support_surface_name = "table"
00074 req.grasps_to_evaluate = grasps
00075 service_name = "evaluate_point_cluster_grasps"
00076 rospy.loginfo("waiting for evaluate_point_cluster_grasps service")
00077 rospy.wait_for_service(service_name)
00078 rospy.loginfo("service found")
00079 serv = rospy.ServiceProxy(service_name, GraspPlanning)
00080 try:
00081 res = serv(req)
00082 except rospy.ServiceException, e:
00083 rospy.logerr("error when calling evaluate_point_cluster_grasps: %s"%e)
00084 return 0
00085 if res.error_code.value != 0:
00086 return []
00087
00088 probs = [grasp.success_probability for grasp in res.grasps]
00089 return probs
00090
00091
00092
00093 def call_find_cluster_bounding_box(cluster):
00094
00095 req = FindClusterBoundingBoxRequest()
00096 req.cluster = cluster
00097 service_name = "find_cluster_bounding_box"
00098 rospy.loginfo("waiting for find_cluster_bounding_box service")
00099 rospy.wait_for_service(service_name)
00100 rospy.loginfo("service found")
00101 serv = rospy.ServiceProxy(service_name, FindClusterBoundingBox)
00102 try:
00103 res = serv(req)
00104 except rospy.ServiceException, e:
00105 rospy.logerr("error when calling find_cluster_bounding_box: %s"%e)
00106 return 0
00107 if not res.error_code:
00108 return (res.pose, res.box_dims)
00109 else:
00110 return (None, None)
00111
00112
00113 if __name__ == "__main__":
00114
00115
00116 rospy.init_node('test_point_cluster_grasp_planner', anonymous=True)
00117 tf_broadcaster = tf.TransformBroadcaster()
00118 tf_listener = tf.TransformListener()
00119 draw_functions = draw_functions.DrawFunctions('grasp_markers')
00120
00121
00122 (table, clusters) = call_object_detector()
00123 print "table:\n", table
00124
00125
00126 while(not rospy.is_shutdown()):
00127
00128
00129 for cluster in clusters:
00130 draw_functions.clear_grasps(num=1000)
00131
00132
00133 (box_pose, box_dims) = call_find_cluster_bounding_box(cluster)
00134 if box_pose == None:
00135 continue
00136 box_mat = pose_to_mat(box_pose.pose)
00137 box_ranges = [[-box_dims.x/2, -box_dims.y/2, -box_dims.z/2],
00138 [box_dims.x/2, box_dims.y/2, box_dims.z/2]]
00139 draw_functions.draw_rviz_box(box_mat, box_ranges, 'base_link', \
00140 ns = 'bounding box', \
00141 color = [0,0,1], opaque = 0.25, duration = 60)
00142
00143 print "Run the blue cluster? Enter y to run, enter to go to the next, q to exit"
00144 c = raw_input()
00145
00146 if c == 'y':
00147 draw_functions.clear_rviz_points('grasp_markers')
00148
00149
00150 grasps = call_plan_point_cluster_grasp(cluster)
00151 grasp_poses = [grasp.grasp_pose for grasp in grasps]
00152
00153
00154 original_probs = [grasp.success_probability for grasp in grasps]
00155 print "original probs:", pplist(original_probs)
00156
00157
00158 for grasp in grasps:
00159 grasp.success_probability = 0
00160
00161
00162 probs = call_evaluate_point_cluster_grasps(cluster, grasps)
00163 print "new probs: ", pplist(probs)
00164
00165
00166 print "number of grasps returned:", len(grasps)
00167 print "drawing grasps: press p to pause after each one in turn, enter to show all grasps at once"
00168 c = raw_input()
00169 if c == 'p':
00170 draw_functions.draw_grasps(grasp_poses, cluster.header.frame_id, pause = 1)
00171 else:
00172 draw_functions.draw_grasps(grasp_poses, cluster.header.frame_id, pause = 0)
00173 print "done drawing grasps, press enter to continue"
00174 raw_input()
00175
00176
00177 draw_functions.clear_grasps(num = 1000)
00178
00179 elif c == 'q':
00180 break
00181 else:
00182 continue
00183 break
00184