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