00001
00002
00003 """
00004 Segments out clusters of points with normals roughly orthogonal
00005 to the direction of grasping, and runs the cluster planner on
00006 them to plan grasps
00007 """
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043 from __future__ import division
00044 import roslib
00045 roslib.load_manifest('segmented_clutter_grasp_planner')
00046 import rospy
00047 import actionlib
00048 import time
00049 from object_manipulator.convert_functions import *
00050 from object_manipulator.image_region_functions import *
00051 from manipulation_msgs.srv import GraspPlanning, GraspPlanningResponse, \
00052 GraspPlanningRequest
00053 from manipulation_msgs.msg import GraspPlanningAction, GraspPlanningResult, \
00054 GraspPlanningErrorCode
00055 from pr2_gripper_grasp_planner_cluster.srv import SetPointClusterGraspParams, \
00056 SetPointClusterGraspParamsRequest
00057 from object_manipulation_msgs.msg import FindContainerAction, FindContainerGoal
00058 from arm_navigation_msgs.srv import SetPlanningSceneDiff
00059
00060 class SegmentedClutterGraspPlanner(object):
00061 """
00062 A grasp planning service (and an equivalent action) for finding good
00063 top and/or side grasps for objects in clutter, based only on point clouds.
00064
00065 The grasp planner works by segmenting clusters of points (in a PointCloud2)
00066 within a region of interest that have normals roughly orthogonal
00067 to each desired grasp direction, and then running the cluster planner on
00068 each cluster in turn to plan grasps.
00069 """
00070
00071 def __init__(self, advertise_service = False, advertise_action = True):
00072
00073
00074 self._container_client = \
00075 actionlib.SimpleActionClient('find_container_action',
00076 FindContainerAction)
00077 rospy.loginfo("Waiting for find_container_action...")
00078 self._container_client.wait_for_server()
00079
00080
00081 service_name = "plan_point_cluster_grasp"
00082 rospy.loginfo("waiting for plan_point_cluster_grasp service")
00083 rospy.wait_for_service(service_name)
00084 rospy.loginfo("service found")
00085 self._cluster_grasp_srv = rospy.ServiceProxy(service_name,
00086 GraspPlanning)
00087
00088
00089 service_name = "set_point_cluster_grasp_params"
00090 rospy.loginfo("waiting for set_point_cluster_grasp_params service")
00091 rospy.wait_for_service(service_name)
00092 rospy.loginfo("service found")
00093 self._cluster_grasp_params_srv = rospy.ServiceProxy(service_name,
00094 SetPointClusterGraspParams)
00095
00096
00097 planning_scene_srv_name = "environment_server/set_planning_scene_diff"
00098 rospy.loginfo("waiting for %s service"%planning_scene_srv_name)
00099 rospy.wait_for_service(planning_scene_srv_name)
00100 self._planning_scene_srv = rospy.ServiceProxy(planning_scene_srv_name,
00101 SetPlanningSceneDiff)
00102 rospy.loginfo("service found")
00103
00104
00105
00106
00107
00108 self._opening_dir_list = rospy.get_param("~opening_dir_list",
00109 [[0,0,1], [-1,0,0]])
00110
00111
00112
00113 self._pregrasp_just_outside_box = \
00114 rospy.get_param("~pregrasp_just_outside_box", False)
00115
00116
00117 self._cluster_planner_name = rospy.get_param("cluster_planner_name",
00118 "point_cluster_grasp_planner")
00119
00120
00121 if advertise_service:
00122 rospy.Service('plan_segmented_clutter_grasps', GraspPlanning,
00123 self.plan_sc_grasps_cb)
00124 rospy.loginfo("segmented clutter grasp planner is ready")
00125
00126
00127 if advertise_action:
00128 self._as = actionlib.SimpleActionServer(rospy.get_name(),
00129 GraspPlanningAction,
00130 execute_cb = self.sc_grasps_execute_cb)
00131 self._as.start()
00132 rospy.loginfo(rospy.get_name()+" is ready for queries")
00133
00134 def plan_sc_grasps_cb(self, req):
00135 """service callback for the plan_segmented_clutter_grasps service"""
00136 rospy.loginfo("planning grasps for a point cloud")
00137 resp = GraspPlanningResponse()
00138 (grasps, error_code) = self.plan_grasps_for_target(req.target,
00139 req.arm_name)
00140 resp.grasps = grasps
00141 resp.error_code = error_code
00142 return resp
00143
00144 def sc_grasps_execute_cb(self, goal):
00145 """action callback for the segmented_clutter_grasps_action"""
00146 result = GraspPlanningResult()
00147 (grasps, error_code) = self.plan_grasps_for_target(goal.target,
00148 goal.arm_name)
00149 result.grasps = grasps
00150 result.error_code = error_code
00151 self._as.set_succeeded(result)
00152
00153 def plan_grasps_for_target(self, target, arm_name):
00154 """return grasps for either a service or action request"""
00155
00156 self._opening_dir_list = rospy.get_param("~opening_dir_list",
00157 [[0,0,1], [-1,0,0]])
00158
00159
00160
00161 grasps = []
00162 for opening_dir in self._opening_dir_list:
00163 some_grasps = self.find_grasps(target.region.cloud,
00164 target.region.roi_box_pose,
00165 target.region.roi_box_dims,
00166 arm_name,
00167 target.reference_frame_id,
00168 opening_dir)
00169 grasps.extend(some_grasps)
00170
00171 error_code = GraspPlanningErrorCode()
00172 error_code.value = error_code.SUCCESS
00173 if grasps == None:
00174 error_code.value = error_code.OTHER_ERROR
00175 return ([], error_code)
00176
00177 return (grasps, error_code)
00178
00179 def process_point_cloud(self, point_cloud, box_pose, box_dims,
00180 opening_dir):
00181 """
00182 call the find_container_action to remove parallel-to-container-side
00183 points and cluster the contents
00184 """
00185 container_goal = FindContainerGoal()
00186 container_goal.cloud = point_cloud
00187 container_goal.box_pose = box_pose
00188 container_goal.box_dims = box_dims
00189 set_xyz(container_goal.opening_dir, opening_dir)
00190
00191 success = self._container_client.send_goal_and_wait(container_goal,
00192 rospy.Duration(10.0))
00193
00194 if not success:
00195 rospy.logerr("Timed out while processing find_container_action")
00196 return []
00197
00198 result = self._container_client.get_result()
00199 return result.clusters
00200
00201 def call_plan_point_cluster_grasp(self, cluster, arm_name,
00202 reference_frame_id):
00203 """
00204 call plan_point_cluster_grasp to get candidate grasps for a cluster
00205 """
00206 req = GraspPlanningRequest()
00207 req.target.reference_frame_id = reference_frame_id
00208 req.target.region.cloud = cluster
00209 req.arm_name = arm_name
00210 try:
00211 res = self._cluster_grasp_srv(req)
00212 except rospy.ServiceException as err:
00213 rospy.logerr("error in calling plan_point_cluster_grasp: %s"%err)
00214 return 0
00215 if res.error_code.value != 0:
00216 return []
00217 return res.grasps
00218
00219 def call_set_pc_grasp_params(self, side = False,
00220 pregrasp_just_outside_box = False):
00221 """
00222 call the set_point_cluster_grasp_params service with appropriate params
00223 """
00224 req = SetPointClusterGraspParamsRequest()
00225 req.height_good_for_side_grasps = 0.02
00226 req.gripper_opening = 0.083
00227 req.side_step = 0.04
00228 req.palm_step = 0.02
00229 if side:
00230 req.side_grasps_only = True
00231 req.overhead_grasps_only = False
00232 else:
00233 req.side_grasps_only = False
00234 req.overhead_grasps_only = True
00235 req.include_high_point_grasps = False
00236 req.pregrasp_just_outside_box = pregrasp_just_outside_box
00237 req.backoff_depth_steps = 1
00238 req.disable_grasp_neighbor_check = True
00239 try:
00240 self._cluster_grasp_params_srv(req)
00241 except rospy.ServiceException as err:
00242 rospy.logerr("error calling set_cluster_grasp_params: %s"% err)
00243 return 0
00244 return 1
00245
00246 def find_grasps(self, point_cloud, box_pose, box_dims, arm_name,
00247 reference_frame_id, opening_dir):
00248 """find grasps on a point cloud inside a bounding box"""
00249 start_time = time.time()
00250
00251
00252 clusters = self.process_point_cloud(point_cloud, box_pose,
00253 box_dims, opening_dir)
00254 if len(clusters) == 0:
00255 return []
00256
00257
00258
00259 self._pregrasp_just_outside_box = \
00260 rospy.get_param("~pregrasp_just_outside_box", False)
00261
00262
00263 side_opening = False
00264 if opening_dir[2] == 0:
00265 side_opening = True
00266 self.call_set_pc_grasp_params(side_opening,
00267 self._pregrasp_just_outside_box)
00268
00269
00270 grasp_plan_start_time = time.time()
00271 grasps = []
00272 rospy.loginfo("computing grasps on %d clusters"%len(clusters))
00273 for (ind, cluster) in enumerate(clusters):
00274 cluster_grasps = self.call_plan_point_cluster_grasp(cluster,
00275 arm_name,
00276 reference_frame_id)
00277 grasps.extend(cluster_grasps)
00278 rospy.loginfo("%d grasps from cluster %d"%(len(cluster_grasps),
00279 ind))
00280 grasp_plan_end_time = time.time()
00281 rospy.loginfo("total cluster planning time elapsed: %.2f"%
00282 (grasp_plan_end_time-grasp_plan_start_time))
00283
00284
00285 sorted(grasps, key=lambda t:t.grasp_quality, reverse=True)
00286
00287
00288 filtered_grasps = []
00289 if side_opening:
00290 for grasp in grasps:
00291 grasp_mat = pose_to_mat(grasp.grasp_pose.pose)
00292
00293
00294
00295 if grasp_mat[0, 0] < 0.7:
00296 continue
00297 filtered_grasps.append(grasp)
00298 else:
00299 filtered_grasps = grasps
00300
00301 end_time = time.time()
00302 print "total grasp planning time elapsed: %.2f"% (end_time -
00303 start_time)
00304 print "returning with %d grasps"% len(filtered_grasps)
00305 return filtered_grasps
00306
00307
00308 if __name__ == '__main__':
00309 rospy.init_node('segmented_clutter_grasp_planner_server', anonymous=False)
00310
00311 SCGP = SegmentedClutterGraspPlanner(advertise_service = False,
00312 advertise_action = True)
00313 rospy.spin()