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()