def _mirror_structure_callback(self, msg):
Logger.localinfo("--> Creating behavior structure for mirror...")
self._pub.publish('flexbe/mirror/structure',
self._build_structure_msg())
Logger.localinfo("<-- Sent behavior structure for mirror.")
# enable control of states since a mirror is listening
self._enable_ros_control()
def _set_autonomy_level(self, msg):
""" Sets the current autonomy level. """
if OperatableStateMachine.autonomy_level != msg.data:
Logger.localinfo('--> Autonomy changed to %d' % msg.data)
if msg.data < 0:
self.preempt()
else:
OperatableStateMachine.autonomy_level = msg.data
self._pub.publish('flexbe/command_feedback',
CommandFeedback(command="autonomy", args=[]))
def _attach_callback(self, msg):
Logger.localinfo("--> Enabling control...")
# set autonomy level
OperatableStateMachine.autonomy_level = msg.data
# enable control of states
self._enable_ros_control()
self._inner_sync_request = True
# send command feedback
cfb = CommandFeedback(command="attach")
cfb.args.append(self.name)
self._pub.publish('flexbe/command_feedback', cfb)
Logger.localinfo("<-- Sent attach confirm.")
def _sync_callback(self, msg):
Logger.localinfo("--> Synchronization requested...")
msg = BehaviorSync()
msg.behavior_id = self.id
# make sure we are already executing
self.wait(condition=lambda: self.get_deep_state() is not None)
msg.current_state_checksum = zlib.adler32(
self.get_deep_state().path.encode()) & 0x7fffffff
self._pub.publish('flexbe/mirror/sync', msg)
self._pub.publish('flexbe/command_feedback',
CommandFeedback(command="sync", args=[]))
Logger.localinfo("<-- Sent synchronization message for mirror.")
def _operatable_execute(self, *args, **kwargs):
outcome = self.__execute(*args, **kwargs)
if self._is_controlled:
# reset previously requested outcome if applicable
if self._last_requested_outcome is not None and outcome is None:
self._pub.publish(
self._request_topic,
OutcomeRequest(outcome=255, target=self.path))
self._last_requested_outcome = None
# request outcome because autonomy level is too low
if not self._force_transition and (
not self.parent.is_transition_allowed(self.name, outcome)
or outcome is not None and self.is_breakpoint):
if outcome != self._last_requested_outcome:
self._pub.publish(
self._request_topic,
OutcomeRequest(outcome=self.outcomes.index(outcome),
target=self.path))
Logger.localinfo("<-- Want result: %s > %s" %
(self.name, outcome))
StateLogger.log(
'flexbe.operator',
self,
type='request',
request=outcome,
autonomy=self.parent.autonomy_level,
required=self.parent.get_required_autonomy(outcome))
self._last_requested_outcome = outcome
outcome = None
# autonomy level is high enough, report the executed transition
elif outcome is not None and outcome in self.outcomes:
Logger.localinfo("State result: %s > %s" %
(self.name, outcome))
self._pub.publish(self._outcome_topic,
UInt8(self.outcomes.index(outcome)))
self._pub.publish(self._debug_topic,
String("%s > %s" % (self.path, outcome)))
if self._force_transition:
StateLogger.log('flexbe.operator',
self,
type='forced',
forced=outcome,
requested=self._last_requested_outcome)
self._last_requested_outcome = None
self._force_transition = False
return outcome
def _preemptable_execute(self, *args, **kwargs):
if self._is_controlled and self._sub.has_msg(self._preempt_topic):
self._sub.remove_last_msg(self._preempt_topic)
self._pub.publish(self._feedback_topic,
CommandFeedback(command="preempt"))
PreemptableState.preempt = True
Logger.localinfo("--> Behavior will be preempted")
if PreemptableState.preempt:
if not self._is_controlled:
Logger.localinfo("Behavior will be preempted")
self._force_transition = True
return self._preempted_name
return self.__execute(*args, **kwargs)
def _execute_unlock(self, target):
if target == self.path or (self._locked and target == ''):
target = self.path
found_target = True
self._locked = False
else:
found_target = self._parent.unlock(target)
# provide feedback about unlock
self._pub.publish(
self._feedback_topic,
CommandFeedback(command="unlock",
args=[target, target if found_target else ""]))
if not found_target:
Logger.logwarn(
"Wanted to unlock %s, but could not find it in current path %s."
% (target, self.path))
else:
Logger.localinfo("--> Unlocking in state %s" % target)
def _manually_transitionable_execute(self, *args, **kwargs):
if self._is_controlled and self._sub.has_buffered(
self._transition_topic):
command_msg = self._sub.get_from_buffer(self._transition_topic)
self._pub.publish(
self._feedback_topic,
CommandFeedback(command="transition",
args=[command_msg.target, self.name]))
if command_msg.target != self.name:
Logger.logwarn(
"Requested outcome for state %s but active state is %s" %
(command_msg.target, self.name))
else:
self._force_transition = True
outcome = self.outcomes[command_msg.outcome]
Logger.localinfo(
"--> Manually triggered outcome %s of state %s" %
(outcome, self.name))
return outcome
# otherwise, return the normal outcome
self._force_transition = False
return self.__execute(*args, **kwargs)
def _event_execute(self, *args, **kwargs):
if self._is_controlled and self._sub.has_msg(self._pause_topic):
msg = self._sub.get_last_msg(self._pause_topic)
self._sub.remove_last_msg(self._pause_topic)
if msg.data:
Logger.localinfo("--> Pausing in state %s", self.name)
self._pub.publish(self._feedback_topic,
CommandFeedback(command="pause"))
self._last_active_container = PriorityContainer.active_container
# claim priority to propagate pause event
PriorityContainer.active_container = self.path
self._paused = True
else:
Logger.localinfo("--> Resuming in state %s", self.name)
self._pub.publish(self._feedback_topic,
CommandFeedback(command="resume"))
PriorityContainer.active_container = self._last_active_container
self._last_active_container = None
self._paused = False
if self._paused and not PreemptableState.preempt:
self._notify_skipped()
return None
if self._entering:
self._entering = False
self.on_enter(*args, **kwargs)
if self._skipped and not PreemptableState.preempt:
self._skipped = False
self.on_resume(*args, **kwargs)
outcome = self.__execute(*args, **kwargs)
repeat = False
if self._is_controlled and self._sub.has_msg(self._repeat_topic):
Logger.localinfo("--> Repeating state %s", self.name)
self._sub.remove_last_msg(self._repeat_topic)
self._pub.publish(self._feedback_topic,
CommandFeedback(command="repeat"))
repeat = True
if repeat or outcome is not None and not PreemptableState.preempt:
self._entering = True
self.on_exit(*args, **kwargs)
return outcome
