def plan_point_cluster_grasp_callback(self, req):
rospy.loginfo("planning grasps for a point cluster")
resp = GraspPlanningResponse()
(grasps, error_code) = self.plan_point_cluster_grasps(req.target, req.arm_name)
resp.grasps = grasps
resp.error_code = error_code
return resp
def evaluate_point_cluster_grasps_callback(self, req):
#rospy.loginfo("evaluating grasps for a point cluster")
#find the cluster bounding box and relevant frames, and transform the cluster
if len(req.target.cluster.points) > 0:
self.pcgp.init_cluster_grasper(req.target.cluster)
cluster_frame = req.target.cluster.header.frame_id
else:
self.pcgp.init_cluster_grasper(req.target.region.cloud)
cluster_frame = req.target.region.cloud.header.frame_id
#evaluate the grasps on the cluster
probs = self.pcgp.evaluate_point_cluster_grasps(req.grasps_to_evaluate, cluster_frame)
#return the same grasps with the qualities added
for (grasp, prob) in zip(req.grasps_to_evaluate, probs):
grasp.success_probability = prob
#fill in the response
resp = GraspPlanningResponse()
resp.error_code.value = 0
resp.grasps = req.grasps_to_evaluate
return resp
