1 #!/usr/bin/python
   2 '''
   3 Test client for point_cluster_grasp_planner.  
   4 Requires tabletop_node.launch to be running (in tabletop_object_detector).  
   5 Add Markers of topic 'grasp_markers' to see the poses being tested.
   6 '''
   7 
   8 from __future__ import division
   9 import roslib
  10 roslib.load_manifest('pr2_pick_and_place_demos')
  11 import rospy
  12 from object_manipulation_msgs.srv import FindClusterBoundingBox, FindClusterBoundingBoxRequest
  13 from tabletop_object_detector.srv import TabletopDetection, TabletopDetectionRequest
  14 from object_manipulation_msgs.srv import GraspPlanning, GraspPlanningRequest
  15 from visualization_msgs.msg import Marker
  16 import tf.transformations
  17 import numpy
  18 import scipy
  19 import time
  20 import object_manipulator.draw_functions as draw_functions
  21 from object_manipulator.convert_functions import *
  22 
  23 
  24 #call the tabletop object detector to get the table and clusters in the scene
  25 def call_object_detector():
  26 
  27     req = TabletopDetectionRequest()
  28     req.return_clusters = 1
  29     req.return_models = 0
  30     service_name = "/object_detection"
  31     rospy.loginfo("waiting for object_detection service")
  32     rospy.wait_for_service(service_name)
  33     serv = rospy.ServiceProxy(service_name, TabletopDetection)
  34     try:
  35         res = serv(req)
  36     except rospy.ServiceException, e:
  37         rospy.logerr("error when calling object_detection: %s"%e)  
  38         return 0
  39     return (res.detection.table, res.detection.clusters)
  40 
  41 
  42 #call plan_point_cluster_grasp to get candidate grasps for a cluster
  43 def call_plan_point_cluster_grasp(cluster):
  44     
  45     req = GraspPlanningRequest()
  46     req.target.cluster = cluster
  47     req.target.type = 2
  48     req.arm_name = "right_arm"
  49     req.collision_support_surface_name = "table"
  50     service_name = "plan_point_cluster_grasp"
  51     rospy.loginfo("waiting for plan_point_cluster_grasp service")
  52     rospy.wait_for_service(service_name)
  53     rospy.loginfo("service found")
  54     serv = rospy.ServiceProxy(service_name, GraspPlanning)
  55     try:
  56         res = serv(req)
  57     except rospy.ServiceException, e:
  58         rospy.logerr("error when calling plan_point_cluster_grasp: %s"%e)  
  59         return 0
  60     if res.error_code.value != 0:
  61         return []
  62     
  63     grasp_poses = [grasp.grasp_pose for grasp in res.grasps]
  64     return grasp_poses
  65 
  66 
  67 #call find_cluster_bounding_box to get the bounding box for a cluster
  68 def call_find_cluster_bounding_box(cluster):
  69     
  70     req = FindClusterBoundingBoxRequest()
  71     req.cluster = cluster
  72     service_name = "find_cluster_bounding_box"
  73     rospy.loginfo("waiting for find_cluster_bounding_box service")
  74     rospy.wait_for_service(service_name)
  75     rospy.loginfo("service found")
  76     serv = rospy.ServiceProxy(service_name, FindClusterBoundingBox)
  77     try:
  78         res = serv(req)
  79     except rospy.ServiceException, e:
  80         rospy.logerr("error when calling find_cluster_bounding_box: %s"%e)  
  81         return 0
  82     if not res.error_code:
  83         return (res.pose, res.box_dims)
  84     else:
  85         return (None, None)
  86 
  87 
  88 if __name__ == "__main__":
  89 
  90     #initialize the node, tf listener and broadcaster, and rviz drawing helper class
  91     rospy.init_node('test_point_cluster_grasp_planner', anonymous=True)
  92     tf_broadcaster = tf.TransformBroadcaster()
  93     tf_listener = tf.TransformListener()
  94     draw_functions = draw_functions.DrawFunctions('grasp_markers')
  95 
  96     #call the tabletop object detector to detect clusters on the table
  97     (table, clusters) = call_object_detector()
  98     print "table:\n", table
  99 
 100     #keep going until the user says to quit (or until Ctrl-C is pressed)
 101     while(not rospy.is_shutdown()):
 102 
 103         #for each cluster in view in turn
 104         for cluster in clusters:
 105             draw_functions.clear_grasps(num=1000)
 106 
 107             #get the bounding box for this cluster and draw it in blue
 108             (box_pose, box_dims) = call_find_cluster_bounding_box(cluster)
 109             if box_pose == None:
 110                 continue
 111             box_mat = pose_to_mat(box_pose.pose)
 112             box_ranges = [[-box_dims.x/2, -box_dims.y/2, -box_dims.z/2],
 113                           [box_dims.x/2, box_dims.y/2, box_dims.z/2]]
 114             draw_functions.draw_rviz_box(box_mat, box_ranges, 'base_link', \
 115                                          ns = 'bounding box', \
 116                                          color = [0,0,1], opaque = 0.25, duration = 60)
 117             
 118             print "Run the blue cluster? Enter y to run, enter to go to the next, q to exit"
 119             c = raw_input()
 120 
 121             if c == 'y':
 122                 draw_functions.clear_rviz_points('grasp_markers')
 123 
 124                 #call the grasp planner
 125                 grasps = call_plan_point_cluster_grasp(cluster)
 126 
 127                 #draw the resulting grasps (all at once, or one at a time)
 128                 print "number of grasps returned:", len(grasps)
 129                 print "drawing grasps: press p to pause after each one in turn, enter to show all grasps at once"
 130                 c = raw_input()
 131                 if c == 'p':
 132                     draw_functions.draw_grasps(grasps, cluster.header.frame_id, pause = 1)
 133                 else:
 134                     draw_functions.draw_grasps(grasps, cluster.header.frame_id, pause = 0)
 135                 print "done drawing grasps, press enter to continue"
 136                 raw_input()
 137 
 138                 #clear out the grasps drawn
 139                 draw_functions.clear_grasps(num = 1000)
 140 
 141             elif c == 'q':
 142                 break
 143         else:
 144             continue
 145         break