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
class ActionServerHandler(object): """ Interface to action server which is more useful for behaviors. """ def __init__(self, action_name, action_type): self.goal_queue = Queue(1) self.result_queue = Queue(1) self.lock = Blackboard().lock self.action_name = action_name self.cancel_next_goal = False self._as = SimpleActionServer(action_name, action_type, execute_cb=self.execute_cb, auto_start=False) self._as.register_preempt_callback(self.cancel_cb) self._as.start() def cancel_cb(self): self.cancel_next_goal = self._as.is_new_goal_available() 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 get_goal(self): try: goal = self.goal_queue.get_nowait() return goal except Empty: return None def has_goal(self): return not self.goal_queue.empty() def send_preempted(self, result=None): def call_me_now(): self._as.set_preempted(result) self.result_queue.put(call_me_now) def send_aborted(self, result=None): def call_me_now(): self._as.set_aborted(result) self.result_queue.put(call_me_now) def send_result(self, result=None): """ :type result: MoveResult """ def call_me_now(): self._as.set_succeeded(result) self.result_queue.put(call_me_now) def is_preempt_requested(self): return self._as.is_preempt_requested()