def direct_reply(self, result, reply_delay=None, marker=None):
"""
Reply to the current goal with the given result, after `reply_delay` amount of seconds.
:param result: a ...Result of the type associated with the type of this server
:param reply_delay: how long to 'reply_delay/calculate' on this goal before sending the reply
:param marker: A str that is printed in the output for easy reference between different replies
"""
assert self._waiting_for == 'direct_reply', "reply cannot follow an 'await_goal', use reply_directly"
if reply_delay is None:
reply_delay = self.default_reply_delay
self._next_reply = result
# The second {} will be filled by countdown_sleep
countdown_sleep(reply_delay, text="{}.direct_reply{}: Think for {}s. "
.format(self._name, '({})'.format(marker) if marker else '', reply_delay) + "Remaining {}s...")
self._reply.set()
self._sent.wait()
self._sent.clear()
self._waiting_for = None
print("{}.direct_reply{}: Finished replying".format(self._name, '({})'.format(marker) if marker else ''))
def reply_conditionally(self, condition, true_result, false_result, timeout=None, reply_delay=None, marker=None):
"""
Reply one of two possibilities, based on a condition. This is a callable that, given a Goal, returns a bool
If True, then reply with the true_reply and vice versa.
An AssertionError is raised when a goal is not received within the given timeout.
:param condition: callable(...Goal) -> bool
:param true_result: a ...Result
:param false_result: a ...Result
:param timeout: seconds to wait for the goal. Defaults to None to wait indefinitely
:param reply_delay: Delay the reply by this amount of seconds
:param marker: A str that is printed in the output for easy reference between different replies
:return: bool
"""
if reply_delay is None:
reply_delay = self.default_reply_delay
print('\n######## {}.reply_conditionally{} ###########'
.format(self._name, '({})'.format(marker) if marker else ''))
assert self._waiting_for is None, "reply_conditionally{} cannot follow an 'await_goal', use reply_directly".format('({})'.format(marker) if marker else '')
self.default_reply = None
print("{}.reply_conditionally{}: Waiting {}for goal..."
.format(self._name, '({})'.format(marker) if marker else '',
str(timeout)+'s ' if timeout is not None else ''))
assert self._request.wait(timeout), "{}.reply_conditionally{} did not get a goal in time"\
.format(self._name, '({})'.format(marker) if marker else '')
self._request.clear()
print("{}.reply_conditionally{}: Got goal: {}"
.format(self._name, '({})'.format(marker) if marker else '', self.goal_formatter(self._current_goal)))
print("{}: Think for {}s...".format(self._name, reply_delay))
match = condition(self._current_goal)
if match:
self._next_reply = true_result
else:
self._next_reply = false_result
countdown_sleep(reply_delay, text="{}.reply_conditionally{}: Think for {}s. "
.format(self._name, '({})'.format(marker) if marker else '', reply_delay) + "Remaining {}s...")
# raw_input("Press the any-key to continue: ")
self._reply.set()
self._sent.wait()
self._sent.clear()
print("{}.reply_conditionally{}: Finished replying"
.format(self._name, '({})'.format(marker) if marker else ''))
return match
def _execute_cb(self):
"""
Called when the underlying action server receives a goal.
If the default_result is None, it will wait for a custom result to be set via reply* otherwise
return the default_result after the given default_result_delay
In the reply-case,it then notifies self.reply* (which should be called by a test script outside this class),
after which self.reply* determines the result to the goal.
Then it notifies _execute_cb that the result has been determined so that _execute_cb can send it
"""
print('\n######## {}.execute ###########'.format(self._name))
self._current_goal = self._as.accept_new_goal()
self._received_goals.append(self._current_goal)
try:
goal_str = self.goal_formatter(self._current_goal)
except Exception as e:
rospy.logerr("goal_formatter of {} raised an exception: {}".format(
self._name, e))
goal_str = self._current_goal
print('{}.execute: From {} to Goal: {}'.format(self._name,
self.pose_bl, goal_str))
if self.default_reply is not None:
countdown_sleep(self._default_reply_delay,
text="{}.execute: Wait for {}s. ".format(
self._name, self._default_reply_delay) +
"Remaining {}s...")
self._call_pre_reply_callbacks(self._current_goal,
self.default_reply)
self._send_result(self.default_reply)
else:
self._request.set()
# Now, wait for to be called, which sets the _reply event AND the _next_reply
self._reply.wait()
self._reply.clear()
self._call_pre_reply_callbacks(self._current_goal,
self._next_reply)
self._send_result(self._next_reply)
self._next_reply = None
self._current_goal = None
self._sent.set()
def _execute_cb(self, request):
"""
Called when the underlying service receives a goal.
If the default_result is None, it will wait for a custom result to be set via reply* otherwise
return the default_result after the given default_result_delay
In the reply-case,it then notifies self.reply* (which should be called by a test script outside this class),
after which self.reply* determines the result to the goal.
Then it notifies _execute_cb that the result has been determined so that _execute_cb can send it
"""
self._current_goal = request
try:
request_str = self.goal_formatter(self._current_goal)
except Exception as e:
rospy.logerr(
"request_formatter of {} raised an exception: {}".format(
self._name, e))
request_str = self._current_goal
print('{}.execute: Request: {}'.format(self._name, request_str))
if self.default_reply is not None:
result = self.default_reply
countdown_sleep(self._default_reply_delay,
text="{}.execute: Wait for {}s. ".format(
self._name, self._default_reply_delay) +
"Remaining {}s...")
else:
self._request.set()
# Now, wait for to be called, which sets the _reply event AND the _next_reply
self._reply.wait()
self._reply.clear()
result = self._next_reply
self._next_reply = None
self._current_goal = None
self._sent.set()
self._call_pre_reply_callbacks(request, result)
return result
def reply(self, result, timeout=None, reply_delay=None, marker=None):
"""
Reply to the next goal with the given result, after `reply_delay` amount of seconds.
An AssertionError is raised when a goal is not received within the given timeout.
:param result: an ...ActionResult of the type associated with the Action-type of this server
:param timeout: how long to wait for the goal? Defaults to None to wait indefinitely
:param reply_delay: how to to reply_delay/calculate on this goal before sending the reply
:param marker: A str that is printed in the output for easy reference between different replies
:return: None
"""
if reply_delay is None:
reply_delay = self.default_reply_delay
print('\n######## {}.reply{} ###########'.format(self._name, '({})'.format(marker) if marker else ''))
assert self._waiting_for is None, "reply{} cannot follow an 'await_goal', use reply_directly".format('({})'.format(marker) if marker else '')
self.default_reply = None
print("{}.reply{}: Waiting {}for goal...".format(self._name, '({})'.format(marker) if marker else '',
str(timeout)+'s ' if timeout is not None else ''))
assert self._request.wait(timeout), \
"{}.reply{} did not get a goal in time".format(self._name, '({})'.format(marker) if marker else '')
self._request.clear()
print("{}.reply{}: Got goal: {}"
.format(self._name, '({})'.format(marker) if marker else '', self.goal_formatter(self._current_goal)))
self._next_reply = result
# The second {} will be filled by countdown_sleep
countdown_sleep(reply_delay, text="{}.reply{}: Think for {}s. ".format(self._name, '({})'.format(marker) if marker else '', reply_delay) + "Remaining {}s...")
self._reply.set()
self._sent.wait()
self._sent.clear()
print("{}.reply{}: Finished replying"
.format(self._name, '({})'.format(marker) if marker else ''))