def stop_perception(self, interrupted): result = DetectFacingsResult() if interrupted: result.error = DetectFacingsResult.ABORTED print_with_prefix('interrupted', self.prefix) else: result.error = DetectFacingsResult.SUCCESS separators = self.get_robosherlock().stop_separator_detection( self.current_goal.id) barcodes = self.get_robosherlock().stop_barcode_detection( self.current_goal.id) update_shelf_system_pose(self.get_knowrob(), self.current_goal.id, separators) self.get_knowrob().update_shelf_layer_position( self.current_goal.id, separators) self.get_knowrob().create_unknown_barcodes(barcodes) self.get_knowrob().add_separators_and_barcodes( self.current_goal.id, separators, barcodes) result.ids = self.get_knowrob().get_facing_ids_from_layer( self.current_goal.id).keys() print_with_prefix('finished', self.prefix) return result
def cluster(self, visualize=False): """ :param visualize: whether or not the debug plut should be shown !this might result in a exception because pyplot does not like multithreading! :type visualize: bool :return: list of PoseStamped :rtype: list """ data = np.array(self.detections) separators = [] # old_frame_id = self.get_frame_id() if len(data) == 0: print_with_prefix('no separators detected', self.prefix) else: clusters = DBSCAN(eps=self.max_dist, min_samples=self.min_samples).fit(data) labels = np.unique(clusters.labels_) print_with_prefix('detected {} separators'.format(len(labels)), self.prefix) for i, label in enumerate(labels): if label != -1: separator = PoseStamped() separator.header.frame_id = 'map' separator.pose.position = Point(*self.cluster_to_separator( data[clusters.labels_ == label])) separator.pose.orientation = Quaternion( *quaternion_about_axis(-np.pi / 2, [0, 0, 1])) separators.append(separator) if visualize: detections = np.array(self.detections) self.visualize_detections(clusters.labels_, detections, self.pose_list_to_np(separators)) return separators
def stop_listening(self): """ :return: list of PoseStamped :rtype: list """ self.listen = False separators = self.cluster() # separators.extend(self.get_edge_separators()) print_with_prefix('stopped', self.prefix) return separators
def query_shelf_systems_cb(self, data): """ :type data: QueryShelfSystemsRequest :rtype: QueryShelfSystemsResponse """ prefix = 'query_shelf_systems' print_with_prefix('called', prefix) r = QueryShelfSystemsResponse() r.ids = self.get_knowrob().get_shelf_system_ids() self.wait_for_update() return r
def start_perception(self, goal): """ :type goal: DetectShelfLayersGoal """ result = DetectShelfLayersResult() print_with_prefix('started', self.prefix) if not self.get_knowrob().shelf_system_exists(goal.id): result.error = DetectShelfLayersResult.INVALID_ID result.error_msg = 'invalid shelf id: {}'.format(goal.id) print_with_prefix('invalid id', self.prefix) return result self.get_robosherlock().start_detect_shelf_layers(goal.id) self.current_goal = goal
def start_perception(self, goal): """ :type goal: DetectFacingsGoal """ print_with_prefix('started', self.prefix) result = DetectFacingsResult() if not self.get_knowrob().shelf_layer_exists(goal.id): result.error = DetectFacingsResult.INVALID_ID result.error_msg = 'invalid layer id: {}'.format(goal.id) return result self.get_robosherlock().start_separator_detection(goal.id) self.get_robosherlock().start_barcode_detection(goal.id) self.current_goal = goal
def load_initial_beliefstate(self): self.initial_beliefstate = rospy.get_param('~initial_beliefstate') self.clear_beliefstate(self.initial_beliefstate) if self.start_episode(self.initial_beliefstate): print_with_prefix( 'loaded initial beliefstate {}'.format( self.initial_beliefstate), self.prefix) self.reset_object_state_publisher.call(TriggerRequest()) return True else: print_with_prefix( 'error loading initial beliefstate {}'.format( self.initial_beliefstate), self.prefix) return False
def start_listening_separators(self, shelf_layer_id): """ :type shelf_layer_id: str """ self.hanging = False self.current_shelf_layer_id = shelf_layer_id self.detections = [] self.marker_ns = 'separator_{}'.format(shelf_layer_id) self.current_shelf_layer_width = self.knowrob.get_shelf_layer_width( shelf_layer_id) self.current_frame_id = self.knowrob.get_perceived_frame_id( self.current_shelf_layer_id) self.T_map___layer = lookup_transform(self.current_frame_id, 'map') self.listen = True print_with_prefix('started', self.prefix)
def stop_perception(self, interrupted): result = DetectShelfLayersResult() if interrupted: result.error = DetectShelfLayersResult.ABORTED print_with_prefix('interrupted', self.prefix) else: result.error = DetectShelfLayersResult.SUCCESS shelf_layer_heights = self.get_robosherlock( ).stop_detect_shelf_layers(self.current_goal.id) self.get_knowrob().add_shelf_layers(self.current_goal.id, shelf_layer_heights) result.ids = self.get_knowrob().get_shelf_layer_from_system( self.current_goal.id).keys() print_with_prefix('finished', self.prefix) return result
def query_facings_cb(self, data): """ :type data: QueryFacingsRequest :rtype: QueryFacingsResponse """ prefix = 'query_facings' print_with_prefix('called', prefix) r = QueryFacingsResponse() if self.get_knowrob().shelf_layer_exists(data.id): r.error = QueryFacingsResponse.SUCCESS r.ids = self.get_knowrob().get_facing_ids_from_layer(data.id).keys() else: print_with_prefix('invalid id', prefix) r.error = QueryFacingsResponse.INVALID_ID self.wait_for_update() return r
def execute_cb(self, goal): """ :type goal: MoveGoal """ with self.lock.acquire_timeout(0) as got_lock: if got_lock and not self.cancel_next_goal: self.my_state = Status.RUNNING self.goal_queue.put(goal) self.result_queue.get()() else: self.my_state = Status.FAILURE r = self._as.action_server.ActionResultType() r.error = DetectShelfLayersResult.SERVER_BUSY print_with_prefix('rejected goal because server busy server busy', self.action_name) self._as.set_aborted(r) self.cancel_next_goal = False
def query_detect_facings_path_cb(self, data): """ :type data: QueryDetectFacingsPathRequest :rtype: QueryDetectFacingsPathResponse """ print_with_prefix('called', 'query_detect_facings_path') r = QueryDetectFacingsPathResponse() if self.get_knowrob().shelf_layer_exists(data.id): r.error = QueryDetectFacingsPathResponse.SUCCESS try: r.path = self.paths.get_detect_facings_path(data.id) except: rospy.logerr('path error') else: r.error = QueryDetectFacingsPathResponse.INVALID_ID self.wait_for_update() return r
def query_count_products_posture_cb(self, data): """ :type data: QueryCountProductsPostureRequest :rtype: QueryCountProductsPostureResponse """ print_with_prefix('called', 'query_count_products_posture') r = QueryCountProductsPostureResponse() if self.get_knowrob().facing_exists(data.id): r.error = QueryCountProductsPostureResponse.SUCCESS # try: r.posture = self.paths.get_count_product_posture(data.id) # except: # rospy.logerr('path error') else: r.error = QueryCountProductsPostureResponse.INVALID_ID self.wait_for_update() return r
def start_perception(self, goal): """ :type goal: CountProductsGoal """ print_with_prefix('started', self.prefix) result = CountProductsResult() if self.get_knowrob().facing_exists(goal.id): result.error = CountProductsResult.SUCCESS result.count = self.get_robosherlock().count_product(goal.id) self.get_knowrob().add_objects(goal.id, result.count) print_with_prefix('counted {} times'.format(result.count), self.prefix) print_with_prefix('finished', self.prefix) return result else: result.error = DetectFacingsResult.INVALID_ID result.error_msg = 'invalid facing id: {}'.format(goal.id) print_with_prefix('invalid id', self.prefix) return result
def finish_perception_cb(data): """ :type data: FinishPerceptionRequest :rtype: FinishPerceptionResponse """ prefix = 'finish_perception' print_with_prefix('called', prefix) lock = Blackboard().lock # type: TimeoutLock result = FinishPerceptionResponse() with lock.acquire_timeout(0) as got_lock: if not got_lock: Blackboard().finished = True result.error = FinishPerceptionResponse.SUCCESS else: result.error = FinishPerceptionResponse.NO_RUNNING_JOB print_with_prefix('no running job', prefix) print_with_prefix('finished', prefix) return result
def print_with_prefix(self, msg): print_with_prefix(msg, self.prefix)
def print_with_prefix(self, msg): """ :type msg: str """ print_with_prefix(msg, self.prefix)
def canceled(self): result = DetectFacingsResult() result.error = DetectShelfLayersResult.ABORTED print_with_prefix('canceled', self.prefix) return result