def find_cluster_bounding_box_center(self, cluster): fcbb = FindClusterBoundingBoxRequest() fcbb.cluster = cluster res = self.find_cluster_bbox(fcbb) if res.error_code == FindClusterBoundingBoxResponse.TF_ERROR: rospy.logerr('Unable to find bounding box for requested point cluster') return None return res.pose.pose.position
def find_cluster_bounding_box_center(self, cluster): fcbb = FindClusterBoundingBoxRequest() fcbb.cluster = cluster res = self.find_cluster_bbox(fcbb) if res.error_code == FindClusterBoundingBoxResponse.TF_ERROR: rospy.logerr( 'Unable to find bounding box for requested point cluster') return None return res.pose.pose.position
def call_find_cluster_bounding_box(cluster): req = FindClusterBoundingBoxRequest() req.cluster = cluster service_name = "find_cluster_bounding_box" rospy.loginfo("waiting for find_cluster_bounding_box service") rospy.wait_for_service(service_name) rospy.loginfo("service found") serv = rospy.ServiceProxy(service_name, FindClusterBoundingBox) try: res = serv(req) except rospy.ServiceException, e: rospy.logerr("error when calling find_cluster_bounding_box: %s"%e) return 0
def detect_bounding_box(self, cluster=None, cluster_choser="find_random_cluster"): """Finds the bounding box of a PointCloud. If no PointCloud is passed then it will use self.try_to_detect() to find an object. The resulting FindClusterBoundingBoxResponse msg is stored in self.last_box_msg. Example usage: >> res = detector.detect_narrow() #check res is not None >> cluster = detector.find_biggest_cluster(res.detection.clusters) #any other cluster selector is ok >> box_msg = detector.detect_bounding_box(cluster) Parameters: cluster: a PointCloud msg. It can be returned by one of the detect_* methods. If none an object will be searched for. cluster_choser: if cluster is None, use this choser to select when detecting. Default to find_random_cluster. Return: a FindClusterBoundingBoxResponse msg. """ try: finder = self.__getattribute__(cluster_choser) except AttributeError: rospy.logwarn( "Cluster choser %s does not exist, using the random one" % cluster_choser) finder = self.find_random_cluster if cluster is None: detection_result = self.try_to_detect() if detection_result is None: rospy.logerr("No way there is an object in front of me!") self.last_box_msg = None return None cluster = finder(detection_result.detection.clusters) req = FindClusterBoundingBoxRequest() req.cluster = cluster try: self.last_box_msg = self.box_detector(req) except rospy.ServiceException, e: rospy.logerr("error when calling find_cluster_bounding_box: %s" % e) return None
def detect_bounding_box(self, cluster = None, cluster_choser = "find_random_cluster"): """Finds the bounding box of a PointCloud. If no PointCloud is passed then it will use self.try_to_detect() to find an object. The resulting FindClusterBoundingBoxResponse msg is stored in self.last_box_msg. Example usage: >> res = detector.detect_narrow() #check res is not None >> cluster = detector.find_biggest_cluster(res.detection.clusters) #any other cluster selector is ok >> box_msg = detector.detect_bounding_box(cluster) Parameters: cluster: a PointCloud or PointCloud2 msg. It can be returned by one of the detect_* methods. If none an object will be searched for. cluster_choser: if cluster is None, use this choser to select when detecting. Default to find_random_cluster. Return: a FindClusterBoundingBoxResponse msg. """ try: finder = self.__getattribute__(cluster_choser) except AttributeError: rospy.logwarn("Cluster choser %s does not exist, using the random one" % cluster_choser) finder = self.find_random_cluster if cluster is None: detection_result = self.try_to_detect() if detection_result is None: rospy.logerr("No way there is an object in front of me!") self.last_box_msg = None return None cluster = finder(detection_result.detection.clusters) req = FindClusterBoundingBoxRequest() #using only PointCloud2 now if type(cluster) is PointCloud: cluster = PointCloud_to_PointCloud2(cluster) req.cluster = cluster try: self.last_box_msg = self.box_detector(req) except rospy.ServiceException, e: rospy.logerr("error when calling find_cluster_bounding_box: %s"%e) return None
def call_find_cluster_bounding_box(self, cluster): req = FindClusterBoundingBoxRequest() req.cluster = cluster service_name = "find_cluster_bounding_box" rospy.loginfo("waiting for find_cluster_bounding_box service") rospy.wait_for_service(service_name) rospy.loginfo("service found") serv = rospy.ServiceProxy(service_name, FindClusterBoundingBox) try: res = serv(req) except rospy.ServiceException, e: rospy.logerr("error when calling find_cluster_bounding_box: %s" % e) return 0
def pickBook(book): #global pullDone #if pullDone == False: # return global bounding_box_srv, find_bounding_box_name bb_req = FindClusterBoundingBoxRequest() bb_req.cluster = book.cluster #print 'displaying target book cloud' #draw_pts(df,book.cluster.points) try: bb_res = bounding_box_srv(bb_req) except rospy.ServiceException, e: rospy.logerr("error when calling %s: %s"%(find_bounding_box_name, e)) sys.exit()
def pullBook(book): if len(book.cluster.points) == 0: rospy.loginfo('bad pullbook request, too few points') return rospy.loginfo('pulling book') global pullDone if pullDone == True: return global bounding_box_srv, find_bounding_box_name bb_req = FindClusterBoundingBoxRequest() bb_req.cluster = book.cluster try: bb_res = bounding_box_srv(bb_req) except rospy.ServiceException, e: rospy.logerr("error when calling %s: %s"%(find_bounding_box_name, e)) sys.exit()