def _notify_skipped(self):
# make sure we dont miss a preempt even if not being executed
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
def _sync_callback(self, msg):
rospy.loginfo("--> Synchronization requested...")
msg = BehaviorSync()
msg.behavior_id = self.id
msg.current_state_checksum = zlib.adler32(self._get_deep_state()._get_path())
self._pub.publish('/flexbe/mirror/sync', msg)
self._pub.publish('/flexbe/command_feedback', CommandFeedback(command="sync", args=[]))
rospy.loginfo("<-- Sent synchronization message for mirror.")
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
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 test_event_state(self):
state = self._create()
fb_topic = 'flexbe/command_feedback'
sub = ProxySubscriberCached({fb_topic: CommandFeedback})
rospy.sleep(0.2) # wait for pub/sub
# enter during first execute
self._execute(state)
self.assertListEqual(['on_enter'], state.last_events)
self._execute(state)
self.assertListEqual([], state.last_events)
# pause and resume as commanded
state._sub._callback(Bool(True), 'flexbe/command/pause')
self._execute(state)
self.assertListEqual(['on_pause'], state.last_events)
self.assertMessage(sub, fb_topic, CommandFeedback(command="pause"))
state.result = 'error'
outcome = self._execute(state)
state.result = None
self.assertIsNone(outcome)
state._sub._callback(Bool(False), 'flexbe/command/pause')
self._execute(state)
self.assertListEqual(['on_resume'], state.last_events)
self.assertMessage(sub, fb_topic, CommandFeedback(command="resume"))
# repeat triggers exit and enter again
state._sub._callback(Empty(), 'flexbe/command/repeat')
self._execute(state)
self.assertListEqual(['on_exit'], state.last_events)
self.assertMessage(sub, fb_topic, CommandFeedback(command="repeat"))
self._execute(state)
self.assertListEqual(['on_enter'], state.last_events)
self._execute(state)
self.assertListEqual([], state.last_events)
# exit during last execute when returning an outcome
state.result = 'done'
outcome = self._execute(state)
self.assertListEqual(['on_exit'], state.last_events)
self.assertEqual('done', outcome)
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 _attach_callback(self, msg):
rospy.loginfo("--> 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)
rospy.loginfo("<-- Sent attach confirm.")
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)
if msg.data:
self._sub.remove_last_msg(self._pause_topic)
rospy.loginfo("--> 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._get_path()
self._paused = True
if self._paused and not PreemptableState.preempt:
self._notify_skipped()
return self._loopback_name
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)
execute_outcome = self.__execute(*args, **kwargs)
repeat = False
if self._is_controlled and self._sub.has_msg(self._repeat_topic):
rospy.loginfo("--> 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 execute_outcome != self._loopback_name and not PreemptableState.switching and not PreemptableState.preempt \
or repeat:
self._entering = True
self.on_exit(*args, **kwargs)
return execute_outcome
def test_lockable_state(self):
state = self._create()
fb_topic = 'flexbe/command_feedback'
sub = ProxySubscriberCached({fb_topic: CommandFeedback})
rospy.sleep(0.2) # wait for pub/sub
# lock and unlock as commanded, return outcome after unlock
state._sub._callback(String('/subject'), 'flexbe/command/lock')
state.result = 'done'
outcome = self._execute(state)
self.assertIsNone(outcome)
self.assertTrue(state._locked)
self.assertMessage(
sub, fb_topic,
CommandFeedback(command='lock', args=['/subject', '/subject']))
state.result = None
state._sub._callback(String('/subject'), 'flexbe/command/unlock')
outcome = self._execute(state)
self.assertEqual(outcome, 'done')
self.assertMessage(
sub, fb_topic,
CommandFeedback(command='unlock', args=['/subject', '/subject']))
# lock and unlock without target
state._sub._callback(String(''), 'flexbe/command/lock')
state.result = 'done'
outcome = self._execute(state)
self.assertIsNone(outcome)
self.assertMessage(
sub, fb_topic,
CommandFeedback(command='lock', args=['/subject', '/subject']))
state._sub._callback(String(''), 'flexbe/command/unlock')
outcome = self._execute(state)
self.assertEqual(outcome, 'done')
self.assertMessage(
sub, fb_topic,
CommandFeedback(command='unlock', args=['/subject', '/subject']))
# reject invalid lock command
state._sub._callback(String('/invalid'), 'flexbe/command/lock')
outcome = self._execute(state)
self.assertEqual(outcome, 'done')
self.assertMessage(
sub, fb_topic,
CommandFeedback(command='lock', args=['/invalid', '']))
# reject generic unlock command when not locked
state._sub._callback(String(''), 'flexbe/command/unlock')
self._execute(state)
self.assertMessage(sub, fb_topic,
CommandFeedback(command='unlock', args=['', '']))
# do not transition out of locked container
state.parent._locked = True
outcome = self._execute(state)
self.assertIsNone(outcome)
state.parent._locked = False
state.result = None
outcome = self._execute(state)
self.assertEqual(outcome, 'done')
def _execute_unlock(self, target):
found_target = False
if target == self._get_path():
found_target = True
self._locked = False
else:
found_target = self._parent.unlock(target)
self._pub.publish(self._feedback_topic, CommandFeedback(command="unlock", args=[target, target if found_target else ""]))
if not found_target:
rospy.logwarn("--> Wanted to unlock %s, but could not find it in current path %s.", target, self._get_path())
else:
rospy.loginfo("--> Unlocking in state %s", target)
def test_manually_transitionable_state(self):
state = self._create()
fb_topic = 'flexbe/command_feedback'
sub = ProxySubscriberCached({fb_topic: CommandFeedback})
rospy.sleep(0.2) # wait for pub/sub
# return requested outcome
state._sub._callback(OutcomeRequest(target='subject', outcome=1),
'flexbe/command/transition')
outcome = self._execute(state)
self.assertEqual(outcome, 'error')
self.assertMessage(
sub, fb_topic,
CommandFeedback(command='transition', args=['subject', 'subject']))
# reject outcome request for different target
state._sub._callback(OutcomeRequest(target='invalid', outcome=1),
'flexbe/command/transition')
outcome = self._execute(state)
self.assertIsNone(outcome)
self.assertMessage(
sub, fb_topic,
CommandFeedback(command='transition', args=['invalid', 'subject']))
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 _notify_skipped(self):
if not self._skipped:
self.on_pause()
self._skipped = True
if self._is_controlled and self._sub.has_msg(self._pause_topic):
msg = self._sub.get_last_msg(self._pause_topic)
if not msg.data:
self._sub.remove_last_msg(self._pause_topic)
rospy.loginfo("--> 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
super(EventState, self)._notify_skipped()
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 _preemptable_execute(self, *args, **kwargs):
preempting = False
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"))
preempting = True
PreemptableState.preempt = True
rospy.loginfo("--> Behavior will be preempted")
if PreemptableState.preempt:
preempting = True
rospy.loginfo("Behavior will be preempted")
if preempting:
self.service_preempt()
self._force_transition = True
return self._preempted_name
return self.__execute(*args, **kwargs)
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 _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:
rospy.logwarn("--> Requested outcome for state " +
command_msg.target + " but active state is " +
self.name)
else:
self._force_transition = True
outcome = self._outcome_list[command_msg.outcome]
rospy.loginfo("--> Manually triggered outcome " + outcome +
" of state " + self.name)
return outcome
# return the normal outcome
self._force_transition = False
return self.__execute(*args, **kwargs)
def test_preemptable_state(self):
state = self._create()
fb_topic = 'flexbe/command_feedback'
sub = ProxySubscriberCached({fb_topic: CommandFeedback})
rospy.sleep(0.2) # wait for pub/sub
# preempt when trigger variable is set
PreemptableState.preempt = True
outcome = self._execute(state)
self.assertEqual(outcome, PreemptableState._preempted_name)
self.assertRaises(StateMachineError,
lambda: state.parent.current_state)
PreemptableState.preempt = False
outcome = self._execute(state)
self.assertIsNone(outcome)
# preempt when command is received
state._sub._callback(Empty(), 'flexbe/command/preempt')
outcome = self._execute(state)
self.assertEqual(outcome, PreemptableState._preempted_name)
self.assertRaises(StateMachineError,
lambda: state.parent.current_state)
self.assertMessage(sub, fb_topic, CommandFeedback(command='preempt'))
PreemptableState.preempt = False
def _behavior_execution(self, msg):
# sending a behavior while one is already running is considered as switching
if self._running:
Logger.loginfo('--> Initiating behavior switch...')
self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['received']))
else:
Logger.loginfo('--> Starting new behavior...')
# construct the behavior that should be executed
be = self._prepare_behavior(msg)
if be is None:
Logger.logerr('Dropped behavior start request because preparation failed.')
if self._running:
self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['failed']))
else:
rospy.loginfo('\033[92m--- Behavior Engine ready! ---\033[0m')
return
# perform the behavior switch if required
with self._switch_lock:
self._switching = True
if self._running:
self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['start']))
# ensure that switching is possible
if not self._is_switchable(be):
Logger.logerr('Dropped behavior start request because switching is not possible.')
self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['not_switchable']))
return
# wait if running behavior is currently starting or stopping
rate = rospy.Rate(100)
while not rospy.is_shutdown():
active_state = self.be.get_current_state()
if active_state is not None or not self._running:
break
rate.sleep()
# extract the active state if any
if active_state is not None:
rospy.loginfo("Current state %s is kept active.", active_state.name)
try:
be.prepare_for_switch(active_state)
self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['prepared']))
except Exception as e:
Logger.logerr('Failed to prepare behavior switch:\n%s' % str(e))
self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['failed']))
return
# stop the rest
rospy.loginfo('Preempting current behavior version...')
self.be.preempt_for_switch()
# execute the behavior
with self._run_lock:
self._switching = False
self.be = be
self._running = True
result = None
try:
rospy.loginfo('Behavior ready, execution starts now.')
rospy.loginfo('[%s : %s]', be.name, msg.behavior_checksum)
self.be.confirm()
args = [self.be.requested_state_path] if self.be.requested_state_path is not None else []
self._pub.publish(self.status_topic,
BEStatus(behavior_id=self.be.id, code=BEStatus.STARTED, args=args))
result = self.be.execute()
if self._switching:
self._pub.publish(self.status_topic,
BEStatus(behavior_id=self.be.id, code=BEStatus.SWITCHING))
else:
self._pub.publish(self.status_topic,
BEStatus(behavior_id=self.be.id, code=BEStatus.FINISHED, args=[str(result)]))
except Exception as e:
self._pub.publish(self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.FAILED))
Logger.logerr('Behavior execution failed!\n%s' % str(e))
import traceback
Logger.loginfo(traceback.format_exc())
result = result or "exception" # only set result if not executed
# done, remove left-overs like the temporary behavior file
try:
if not self._switching:
self._clear_imports()
self._cleanup_behavior(msg.behavior_checksum)
except Exception as e:
rospy.logerr('Failed to clean up behavior:\n%s' % str(e))
if not self._switching:
rospy.loginfo('Behavior execution finished with result %s.', str(result))
rospy.loginfo('\033[92m--- Behavior Engine ready! ---\033[0m')
self._running = False
self.be = None
def _behavior_execution(self, msg):
if self._running:
Logger.loginfo('--> Initiating behavior switch...')
self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['received']))
else:
Logger.loginfo('--> Starting new behavior...')
be = self._prepare_behavior(msg)
if be is None:
Logger.logerr('Dropped behavior start request because preparation failed.')
if self._running:
self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['failed']))
else:
rospy.loginfo('\033[92m--- Behavior Engine ready! ---\033[0m')
return
if self._running:
if self._switching:
Logger.logwarn('Already switching, dropped new start request.')
return
self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['start']))
if not self._is_switchable(be):
Logger.logerr('Dropped behavior start request because switching is not possible.')
self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['not_switchable']))
return
self._switching = True
active_state = self.be.get_current_state()
rospy.loginfo("Current state %s is kept active.", active_state.name)
try:
be.prepare_for_switch(active_state)
self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['prepared']))
except Exception as e:
Logger.logerr('Failed to prepare behavior switch:\n%s' % str(e))
self._switching = False
self._pub.publish(self.feedback_topic, CommandFeedback(command="switch", args=['failed']))
return
rospy.loginfo('Preempting current behavior version...')
self.be.preempt_for_switch()
rate = rospy.Rate(10)
while self._running:
rate.sleep()
self._switching = False
self._running = True
self.be = be
result = ""
try:
rospy.loginfo('Behavior ready, execution starts now.')
rospy.loginfo('[%s : %s]', be.name, msg.behavior_checksum)
self.be.confirm()
args = [self.be.requested_state_path] if self.be.requested_state_path is not None else []
self._pub.publish(self.status_topic, BEStatus(behavior_id=self.be.id, code=BEStatus.STARTED, args=args))
result = self.be.execute()
if self._switching:
self._pub.publish(self.status_topic, BEStatus(behavior_id=self.be.id, code=BEStatus.SWITCHING))
else:
self._pub.publish(self.status_topic, BEStatus(behavior_id=self.be.id, code=BEStatus.FINISHED, args=[str(result)]))
except Exception as e:
self._pub.publish(self.status_topic, BEStatus(behavior_id=msg.behavior_checksum, code=BEStatus.FAILED))
Logger.logerr('Behavior execution failed!\n%s' % str(e))
import traceback
Logger.loginfo(traceback.format_exc())
result = "failed"
try:
self._cleanup_behavior(msg.behavior_checksum)
except Exception as e:
rospy.logerr('Failed to clean up behavior:\n%s' % str(e))
self.be = None
if not self._switching:
rospy.loginfo('Behavior execution finished with result %s.', str(result))
rospy.loginfo('\033[92m--- Behavior Engine ready! ---\033[0m')
self._running = False