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