class PreemptableState(LoopbackState): """ A state that can be preempted. If preempted, the state will not be executed anymore and return the outcome preempted. """ _preempted_name = "preempted" preempt = False switching = False def __init__(self, *args, **kwargs): # add preempted outcome if len(args) > 0 and type(args[0]) is list: # need this ugly check for list type, because first argument in CBState is the callback args[0].append(self._preempted_name) else: outcomes = kwargs.get("outcomes", []) outcomes.append(self._preempted_name) kwargs["outcomes"] = outcomes super(PreemptableState, self).__init__(*args, **kwargs) self.__execute = self.execute self.execute = self._preemptable_execute self._feedback_topic = "/flexbe/command_feedback" self._preempt_topic = "/flexbe/command/preempt" self._pub = ProxyPublisher() self._sub = ProxySubscriberCached() 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 rospy.loginfo("--> Behavior will be preempted") if PreemptableState.preempt: PreemptableState.preempt = False 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 _enable_ros_control(self): super(PreemptableState, self)._enable_ros_control() self._pub.createPublisher(self._feedback_topic, CommandFeedback) self._sub.subscribe(self._preempt_topic, Empty) PreemptableState.preempt = False def _disable_ros_control(self): super(PreemptableState, self)._disable_ros_control() self._sub.unsubscribe_topic(self._preempt_topic)
class WaitForNaoQiSpeechState(EventState): """ Implements a state that waits for a NaoQI-Speech-Command. -- strings_to_rec string[] List of speech commands to listen for -- outcomes string[] List of outcomes corresponding to the speech commands <= done Indicates completion. """ recognized = None _outcomes = None def __init__(self, strings_to_rec, outcomes, topic='/pepper_robot/speechrec/context'): """ Constructor """ super(WaitForNaoQiSpeechState, self).__init__(outcomes=outcomes) self._topic = topic self.outcomes = outcomes self._target_strings = strings_to_rec self._sub = ProxySubscriberCached() self.recognized = None def _speech_callback(self, msg): Logger.loginfo('speechrec callback:%s' % msg.data.lower()) for result in self._target_strings: Logger.loginfo('\tspeechrec check:%s' % result.lower()) if msg.data.lower() == result.lower(): Logger.loginfo('\t\tspeechrec MATCH!') self.recognized = result def execute(self, userdata): """Execute this state""" if self.recognized is not None: try: index = self._target_strings.index(self.recognized) return self.outcomes[index] except Exception as e: Logger.loginfo(str(e)) def on_enter(self, userdata): self.recognized = None self._sub.subscribe(self._topic, String, self._speech_callback) def on_stop(self): self._sub.unsubscribe_topic(self._topic) def on_exit(self, userdata): self._sub.unsubscribe_topic(self._topic)
class ManuallyTransitionableState(MonitoringState): """ A state for that a desired outcome can be declared. If any outcome is declared, this outcome is forced. """ def __init__(self, *args, **kwargs): super(ManuallyTransitionableState, self).__init__(*args, **kwargs) self._force_transition = False self.__execute = self.execute self.execute = self._manually_transitionable_execute self._feedback_topic = '/flexbe/command_feedback' self._transition_topic = '/flexbe/command/transition' self._pub = ProxyPublisher() self._sub = ProxySubscriberCached() 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 _enable_ros_control(self): super(ManuallyTransitionableState, self)._enable_ros_control() self._pub.createPublisher(self._feedback_topic, CommandFeedback) self._sub.subscribe(self._transition_topic, OutcomeRequest) self._sub.enable_buffer(self._transition_topic) def _disable_ros_control(self): super(ManuallyTransitionableState, self)._disable_ros_control() self._sub.unsubscribe_topic(self._transition_topic)
class WaitForRosNavgoalState(EventState): """ Implements a state that waits for a Navgoal on a RosTopic. <# topic string The ROS topic to listen on #> navgoal PoseStamped The navgoal. <= done Indicates completion. """ def __init__(self, topic='/hololens/navgoal'): """ Constructor """ super(WaitForRosNavgoalState, self).__init__(outcomes=['done'], output_keys=['navgoal']) self._sub = ProxySubscriberCached() self._topic = topic self._navgoal = None def execute(self, userdata): """Execute this state""" if self._navgoal is not None: userdata.navgoal = self._navgoal return 'done' def _navgoal_callback(self, msg): # prevent re-using latched message # (this is probably only happening when using rostopic pub from the console, but whatever) try: if self.last_id == msg.header.stamp: return except AttributeError: pass self.last_id = msg.header.stamp Logger.loginfo( 'navgoal callback on topic %s:%s' % (self._topic, str(msg.pose.position).replace('\n', ' '))) self._navgoal = msg def on_enter(self, userdata): self._navgoal = None self._sub.subscribe(self._topic, PoseStamped, self._navgoal_callback) def on_stop(self): self._sub.unsubscribe_topic(self._topic) def on_exit(self, userdata): self._sub.unsubscribe_topic(self._topic)
class ManuallyTransitionableState(MonitoringState): """ A state for that a desired outcome can be declared. If any outcome is declared, this outcome is forced. """ def __init__(self, *args, **kwargs): super(ManuallyTransitionableState, self).__init__(*args, **kwargs) self._force_transition = False self.__execute = self.execute self.execute = self._manually_transitionable_execute self._feedback_topic = '/flexbe/command_feedback' self._transition_topic = '/flexbe/command/transition' self._pub = ProxyPublisher() self._sub = ProxySubscriberCached() 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 _enable_ros_control(self): super(ManuallyTransitionableState, self)._enable_ros_control() self._pub.createPublisher(self._feedback_topic, CommandFeedback) self._sub.subscribe(self._transition_topic, OutcomeRequest) self._sub.enable_buffer(self._transition_topic) def _disable_ros_control(self): super(ManuallyTransitionableState, self)._disable_ros_control() self._sub.unsubscribe_topic(self._transition_topic)
class WaitForOpenDoorState(EventState): """ Implements a state that waits for the way to be clear. <= done Indicates completion. """ LASERS_TO_IGNORE = 5 ERR_THRESHOLD = 0.15 old_values = None def __init__(self): """ Constructor """ super(WaitForOpenDoorState, self).__init__(outcomes=['done']) self._topic = '/laser/srd_front/scan' self._sub = ProxySubscriberCached({self._topic: LaserScan}) def execute(self, userdata): """Execute this state""" while self._sub.has_buffered(self._topic): msg = self._sub.get_from_buffer(self._topic) values = msg.ranges if self.old_values is None: self.old_values = values else: err = 0.0 for i in range(self.LASERS_TO_IGNORE, len(values)-1-self.LASERS_TO_IGNORE): err += self.old_values[i] - values[i] err /= len(values) if err < 0 and abs(err) > self.ERR_THRESHOLD: Logger.loginfo('THRESHOLD REACHED! (%.3f) - Door should be open!' % err) return 'done' self.old_values = values def on_enter(self, userdata): self._sub.enable_buffer(self._topic) Logger.loginfo('Waiting for open door...') def on_stop(self): self._sub.unsubscribe_topic(self._topic) def on_exit(self): self._sub.unsubscribe_topic(self._topic)
class LockableState(ManuallyTransitionableState): """ A state that can be locked. When locked, no transition can be done regardless of the resulting outcome. However, if any outcome would be triggered, the outcome will be stored and the state won't be executed anymore until it is unlocked and the stored outcome is set. """ def __init__(self, *args, **kwargs): super(LockableState, self).__init__(*args, **kwargs) self._locked = False self._stored_outcome = None self.__execute = self.execute self.execute = self._lockable_execute self._feedback_topic = 'flexbe/command_feedback' self._lock_topic = 'flexbe/command/lock' self._unlock_topic = 'flexbe/command/unlock' self._pub = ProxyPublisher() self._sub = ProxySubscriberCached() def _lockable_execute(self, *args, **kwargs): if self._is_controlled and self._sub.has_msg(self._lock_topic): msg = self._sub.get_last_msg(self._lock_topic) self._sub.remove_last_msg(self._lock_topic) self._execute_lock(msg.data) if self._is_controlled and self._sub.has_msg(self._unlock_topic): msg = self._sub.get_last_msg(self._unlock_topic) self._sub.remove_last_msg(self._unlock_topic) self._execute_unlock(msg.data) if self._locked: if self._stored_outcome is None or self._stored_outcome == 'None': self._stored_outcome = self.__execute(*args, **kwargs) return None if not self._locked and not self._stored_outcome is None and not self._stored_outcome == 'None': if self._parent.transition_allowed(self.name, self._stored_outcome): outcome = self._stored_outcome self._stored_outcome = None return outcome else: return None outcome = self.__execute(*args, **kwargs) if outcome is None or outcome == 'None': return None if not self._parent is None and not self._parent.transition_allowed(self.name, outcome): self._stored_outcome = outcome return None return outcome def _execute_lock(self, target): found_target = False if target == self._get_path(): found_target = True self._locked = True else: found_target = self._parent.lock(target) self._pub.publish(self._feedback_topic, CommandFeedback(command="lock", args=[target, target if found_target else ""])) if not found_target: rospy.logwarn("--> Wanted to lock %s, but could not find it in current path %s.", target, self._get_path()) else: rospy.loginfo("--> Locking in state %s", target) 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 _enable_ros_control(self): super(LockableState, self)._enable_ros_control() self._pub.createPublisher(self._feedback_topic, CommandFeedback) self._sub.subscribe(self._lock_topic, String) self._sub.subscribe(self._unlock_topic, String) def _disable_ros_control(self): super(LockableState, self)._disable_ros_control() self._sub.unsubscribe_topic(self._lock_topic) self._sub.unsubscribe_topic(self._unlock_topic) def is_locked(self): return self._locked
class MonitoringState(smach.State): """ A state to monitor a custom set of conditions. For each conditions, an outcome is added or mapped which will be returned if the condition is not met. """ def __init__(self, *args, **kwargs): super(MonitoringState, self).__init__(*args, **kwargs) outcomes = kwargs.get('outcomes', []) self._outcome_list = list(outcomes) self._outcome_list.remove('loopback') self.name = None self._parent = None self._is_controlled = False self._force_monitoring = False self._monitored_keys = dict() self._sent_keys = list() self._current_status = None self.__execute = self.execute self.execute = self._monitoring_execute self._diagnostics_topic = 'diagnostics_agg' self._sub = ProxySubscriberCached() def _monitoring_execute(self, *args, **kwargs): new_status = None had_warning = False if (self._force_monitoring or self._is_controlled) and self._sub.has_buffered( self._diagnostics_topic): new_status = "" diag_msg = self._sub.get_from_buffer(self._diagnostics_topic) for status in diag_msg.status: if not status.name in self._monitored_keys.keys(): continue if status.level == DiagnosticStatus.WARN: had_warning = True if not status.name + "_warn" in self._sent_keys: self._sent_keys.append(status.name + "_warn") Logger.logwarn("%s: %s" % (status.name, status.message)) if status.level == DiagnosticStatus.ERROR: if not status.name + "_err" in self._sent_keys: self._sent_keys.append(status.name + "_err") Logger.logerr("%s: %s" % (status.name, status.message)) new_status = status.name if new_status == "": self._current_status = None new_status = None if not had_warning: self._sent_keys = list() if new_status is None or self._current_status == new_status: return self.__execute(*args, **kwargs) self._current_status = new_status return self._monitored_keys[self._current_status] def monitor(self, key, outcome=None): oc = outcome if not outcome is None else key self._monitored_keys[key] = oc if not oc in self._outcomes: self.register_outcomes([oc]) self._outcome_list.append(oc) def force_monitoring(self): self._force_monitoring = True if not self._is_controlled: self._sub.subscribe(self._diagnostics_topic, DiagnosticArray) self._sub.enable_buffer(self._diagnostics_topic) def _enable_ros_control(self): self._is_controlled = True self._sub.subscribe(self._diagnostics_topic, DiagnosticArray) self._sub.enable_buffer(self._diagnostics_topic) def _disable_ros_control(self): self._is_controlled = False self._sub.unsubscribe_topic(self._diagnostics_topic) def _get_path(self): return self._parent._get_path() + "/" + self.name
class OperatableStateMachine(PreemptableStateMachine): """ A state machine that can be operated. It synchronizes its current state with the mirror and supports some control mechanisms. """ autonomy_level = 3 silent_mode = False def __init__(self, *args, **kwargs): super(OperatableStateMachine, self).__init__(*args, **kwargs) self._message = None self._rate = rospy.Rate(10) self.id = None self.autonomy = None self._autonomy = {} self._ordered_states = [] self._inner_sync_request = False self._pub = ProxyPublisher() self._sub = ProxySubscriberCached() def execute(self, parent_ud=smach.UserData()): # First time in this state machine if self._current_state is None: # Spew some info smach.loginfo( "State machine starting in initial state '%s' with userdata: \n\t%s" % (self._current_label, list(self.userdata.keys()))) # Set initial state self._set_current_state(self._initial_state_label) # Initialize preempt state self._preempted_label = None self._preempted_state = None # Call start callbacks self.call_start_cbs() # Copy input keys self._copy_input_keys(parent_ud, self.userdata) container_outcome = self._loopback_name if self.id is not None: # only top-level statemachine should loop for outcome while container_outcome == self._loopback_name and not smach.is_shutdown( ): # Update the state machine container_outcome = self._async_execute(parent_ud) else: container_outcome = self._async_execute(parent_ud) if smach.is_shutdown(): container_outcome = 'preempted' return container_outcome def _async_execute(self, parent_ud=smach.UserData()): """Run the state machine on entry to this state. This will set the "closed" flag and spin up the execute thread. Once this flag has been set, it will prevent more states from being added to the state machine. """ # This will prevent preempts from getting propagated to non-existent children with self._state_transitioning_lock: # Check state consistency try: self.check_consistency() except (smach.InvalidStateError, smach.InvalidTransitionError): smach.logerr("Container consistency check failed.") return None # Set running flag self._is_running = True # Initialize container outcome container_outcome = None # Step through state machine if self._is_running and not smach.is_shutdown(): # Update the state machine container_outcome = self._update_once() if self._current_state is not None: try: self._current_state._rate.sleep() except ROSInterruptException: rospy.logwarn('Interrupted rate sleep.') if container_outcome is not None and container_outcome != self._loopback_name: # Copy output keys self._copy_output_keys(self.userdata, parent_ud) else: container_outcome = self._loopback_name # provide explicit sync as back-up functionality # should be used only if there is no other choice # since it requires additional 8 byte + header update bandwith and time to restart mirror if self._inner_sync_request and self._get_deep_state() is not None: self._inner_sync_request = False if self.id is None: self._parent._inner_sync_request = True else: 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) # We're no longer running self._is_running = False return container_outcome @staticmethod def add(label, state, transitions=None, autonomy=None, remapping=None): """ Add a state to the opened state machine. @type label: string @param label: The label of the state being added. @param state: An instance of a class implementing the L{State} interface. @param transitions: A dictionary mapping state outcomes to other state labels or container outcomes. @param autonomy: A dictionary mapping state outcomes to their required autonomy level @param remapping: A dictionary mapping local userdata keys to userdata keys in the container. """ self = StateMachine._currently_opened_container() # add loopback transition to loopback states if isinstance(state, LoopbackState): transitions[LoopbackState._loopback_name] = label autonomy[LoopbackState._loopback_name] = -1 if isinstance(state, OperatableStateMachine): transitions[OperatableStateMachine._loopback_name] = label autonomy[OperatableStateMachine._loopback_name] = -1 self._ordered_states.append(state) state.name = label state.transitions = transitions state.autonomy = autonomy state._parent = self StateMachine.add(label, state, transitions, remapping) self._autonomy[label] = autonomy def replace(self, new_state): old_state = self._states[new_state.name] new_state.transitions = old_state.transitions new_state.autonomy = old_state.autonomy new_state._parent = old_state._parent self._ordered_states[self._ordered_states.index(old_state)] = new_state self._states[new_state.name] = new_state def destroy(self): self._notify_stop() self._disable_ros_control() self._sub.unsubscribe_topic('flexbe/command/autonomy') self._sub.unsubscribe_topic('flexbe/command/sync') self._sub.unsubscribe_topic('flexbe/request_mirror_structure') StateLogger.shutdown() def confirm(self, name, id): """ Confirms the state machine and triggers the creation of the structural message. It is mandatory to call this function at the top-level state machine between building it and starting its execution. @type name: string @param name: The name of this state machine to identify it. """ self.name = name self.id = id self._pub.createPublisher( 'flexbe/mirror/sync', BehaviorSync, _latch=True ) # Update mirror with currently active state (high bandwidth mode) self._pub.createPublisher('flexbe/mirror/preempt', Empty, _latch=True) # Preempts the mirror self._pub.createPublisher( 'flexbe/mirror/structure', ContainerStructure) # Sends the current structure to the mirror self._pub.createPublisher('flexbe/log', BehaviorLog) # Topic for logs to the GUI self._pub.createPublisher( 'flexbe/command_feedback', CommandFeedback ) # Gives feedback about executed commands to the GUI self._sub.subscribe('flexbe/command/autonomy', UInt8, self._set_autonomy_level) self._sub.subscribe('flexbe/command/sync', Empty, self._sync_callback) self._sub.subscribe('flexbe/command/attach', UInt8, self._attach_callback) self._sub.subscribe('flexbe/request_mirror_structure', Int32, self._mirror_structure_callback) StateLogger.initialize(name) if OperatableStateMachine.autonomy_level != 255: self._enable_ros_control() rospy.sleep(0.5) # no clean way to wait for publisher to be ready... self._notify_start() def _set_autonomy_level(self, msg): """ Sets the current autonomy level. """ if OperatableStateMachine.autonomy_level != msg.data: rospy.loginfo('--> 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 _sync_callback(self, msg): rospy.loginfo("--> Synchronization requested...") msg = BehaviorSync() msg.behavior_id = self.id while self._get_deep_state() is None: rospy.sleep(0.1) 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 _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 _mirror_structure_callback(self, msg): rospy.loginfo("--> Creating behavior structure for mirror...") msg = self._build_msg('') msg.behavior_id = self.id self._pub.publish('flexbe/mirror/structure', msg) rospy.loginfo("<-- Sent behavior structure for mirror.") def _transition_allowed(self, label, outcome): return self._autonomy[label][ outcome] < OperatableStateMachine.autonomy_level def _build_msg(self, prefix, msg=None): """ Adds this state machine to the initial structure message. @type prefix: string @param prefix: A path consisting of the container hierarchy containing this state. @type msg: ContainerStructure @param msg: The message that will finally contain the structure message. """ # set children children = [] for state in self._ordered_states: children.append(str(state.name)) # set name name = prefix + (self.name if self.id is None else '') if msg is None: # top-level state machine (has no transitions) self._message = ContainerStructure() outcomes = list(self._outcomes) transitions = None autonomy = None else: # lower-level state machine self._message = msg outcomes = list(self.transitions) # set transitions and autonomy transitions = [] autonomy = [] for i in range(len(self.transitions)): outcome = outcomes[i] if outcome == 'preempted': # set preempt transition transitions.append('preempted') autonomy.append(-1) else: transitions.append(str(self.transitions[outcome])) autonomy.append(self.autonomy[outcome]) # add to message self._message.containers.append( Container(name, children, outcomes, transitions, autonomy)) # build message for children for state in self._ordered_states: state._build_msg(name + '/', self._message) # top-level state machine returns the message if msg is None: return self._message def _notify_start(self): for state in self._ordered_states: if isinstance(state, LoopbackState): state.on_start() if isinstance(state, OperatableStateMachine): state._notify_start() def _enable_ros_control(self): self._is_controlled = True for state in self._ordered_states: if isinstance(state, LoopbackState): state._enable_ros_control() if isinstance(state, OperatableStateMachine): state._enable_ros_control() def _notify_stop(self): for state in self._ordered_states: if isinstance(state, LoopbackState): state.on_stop() state._disable_ros_control() if isinstance(state, OperatableStateMachine): state._notify_stop() def _disable_ros_control(self): self._is_controlled = False for state in self._ordered_states: if isinstance(state, LoopbackState): state._disable_ros_control() if isinstance(state, OperatableStateMachine): state._disable_ros_control() def on_exit(self, userdata): if self._current_state is not None: self._current_state.on_exit(userdata) self._current_state = None
class MonitoringState(smach.State): """ A state to monitor a custom set of conditions. For each conditions, an outcome is added or mapped which will be returned if the condition is not met. """ def __init__(self, *args, **kwargs): super(MonitoringState, self).__init__(*args, **kwargs) outcomes = kwargs.get('outcomes', []) self._outcome_list = list(outcomes) self._outcome_list.remove('loopback') self.name = None self._parent = None self._is_controlled = False self._force_monitoring = False self._monitored_keys = dict() self._sent_keys = list() self._current_status = None self.__execute = self.execute self.execute = self._monitoring_execute self._diagnostics_topic = 'diagnostics_agg' self._sub = ProxySubscriberCached() def _monitoring_execute(self, *args, **kwargs): new_status = None had_warning = False if (self._force_monitoring or self._is_controlled) and self._sub.has_buffered(self._diagnostics_topic): new_status = "" diag_msg = self._sub.get_from_buffer(self._diagnostics_topic) for status in diag_msg.status: if not status.name in self._monitored_keys.keys(): continue if status.level == DiagnosticStatus.WARN: had_warning = True if not status.name + "_warn" in self._sent_keys: self._sent_keys.append(status.name + "_warn") Logger.logwarn("%s: %s" % (status.name, status.message)) if status.level == DiagnosticStatus.ERROR: if not status.name + "_err" in self._sent_keys: self._sent_keys.append(status.name + "_err") Logger.logerr("%s: %s" % (status.name, status.message)) new_status = status.name if new_status == "": self._current_status = None new_status = None if not had_warning: self._sent_keys = list() if new_status is None or self._current_status == new_status: return self.__execute(*args, **kwargs) self._current_status = new_status return self._monitored_keys[self._current_status] def monitor(self, key, outcome = None): oc = outcome if not outcome is None else key self._monitored_keys[key] = oc if not oc in self._outcomes: self.register_outcomes([oc]) self._outcome_list.append(oc) def force_monitoring(self): self._force_monitoring = True if not self._is_controlled: self._sub.subscribe(self._diagnostics_topic, DiagnosticArray) self._sub.enable_buffer(self._diagnostics_topic) def _enable_ros_control(self): self._is_controlled = True self._sub.subscribe(self._diagnostics_topic, DiagnosticArray) self._sub.enable_buffer(self._diagnostics_topic) def _disable_ros_control(self): self._is_controlled = False self._sub.unsubscribe_topic(self._diagnostics_topic) def _get_path(self): return self._parent._get_path() + "/" + self.name
class EventState(OperatableState): """ A state that allows implementing certain events. """ def __init__(self, *args, **kwargs): super(EventState, self).__init__(*args, **kwargs) self._entering = True self._skipped = False self.__execute = self.execute self.execute = self._event_execute self._paused = False self._last_active_container = None self._feedback_topic = 'flexbe/command_feedback' self._repeat_topic = 'flexbe/command/repeat' self._pause_topic = 'flexbe/command/pause' self._pub = ProxyPublisher() self._sub = ProxySubscriberCached() 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 _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 _enable_ros_control(self): super(EventState, self)._enable_ros_control() self._pub.createPublisher(self._feedback_topic, CommandFeedback) self._sub.subscribe(self._repeat_topic, Empty) self._sub.subscribe(self._pause_topic, Bool) def _disable_ros_control(self): super(EventState, self)._disable_ros_control() self._sub.unsubscribe_topic(self._repeat_topic) self._sub.unsubscribe_topic(self._pause_topic) self._last_active_container = None if self._paused: PriorityContainer.active_container = None # Events # (just implement the ones you need) def on_start(self): """ Will be executed once when the behavior starts. """ pass def on_stop(self): """ Will be executed once when the behavior stops or is preempted. """ pass def on_pause(self): """ Will be executed each time this state is paused. """ pass def on_resume(self, userdata): """ Will be executed each time this state is resumed. """ pass def on_enter(self, userdata): """ Will be executed each time the state is entered from any other state (but not from itself). """ pass def on_exit(self, userdata): """ Will be executed each time the state will be left to any other state (but not to itself). """ pass
class EventState(OperatableState): """ A state that allows implementing certain events. """ def __init__(self, *args, **kwargs): super(EventState, self).__init__(*args, **kwargs) self._entering = True self._skipped = False self.__execute = self.execute self.execute = self._event_execute self._paused = False self._last_active_container = None self._feedback_topic = "flexbe/command_feedback" self._repeat_topic = "flexbe/command/repeat" self._pause_topic = "flexbe/command/pause" self._pub = ProxyPublisher() self._sub = ProxySubscriberCached() 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 PriorityContainer.active_container = "" 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: 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 or repeat: self._entering = True self.on_exit(*args, **kwargs) return execute_outcome 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 _enable_ros_control(self): super(EventState, self)._enable_ros_control() self._pub.createPublisher(self._feedback_topic, CommandFeedback) self._sub.subscribe(self._repeat_topic, Empty) self._sub.subscribe(self._pause_topic, Bool) def _disable_ros_control(self): super(EventState, self)._disable_ros_control() self._sub.unsubscribe_topic(self._repeat_topic) self._sub.unsubscribe_topic(self._pause_topic) self._last_active_container = None if self._paused: PriorityContainer.active_container = None # Events # (just implement the ones you need) def on_start(self): """ Will be executed once when the behavior starts. """ pass def on_stop(self): """ Will be executed once when the behavior stops or is preempted. """ pass def on_pause(self): """ Will be executed each time this state is paused. """ pass def on_resume(self, userdata): """ Will be executed each time this state is resumed. """ pass def on_enter(self, userdata): """ Will be executed each time the state is entered from any other state (but not from itself). """ pass def on_exit(self, userdata): """ Will be executed each time the state will be left to any other state (but not to itself). """ pass
class PreemptableState(LoopbackState): """ A state that can be preempted. If preempted, the state will not be executed anymore and return the outcome preempted. """ _preempted_name = 'preempted' preempt = False switching = False def __init__(self, *args, **kwargs): # add preempted outcome if len(args) > 0 and type(args[0]) is list: # need this ugly check for list type, because first argument in CBState is the callback args[0].append(self._preempted_name) else: outcomes = kwargs.get('outcomes', []) outcomes.append(self._preempted_name) kwargs['outcomes'] = outcomes super(PreemptableState, self).__init__(*args, **kwargs) self.__execute = self.execute self.execute = self._preemptable_execute self._feedback_topic = 'flexbe/command_feedback' self._preempt_topic = 'flexbe/command/preempt' self._pub = ProxyPublisher() self._sub = ProxySubscriberCached() PreemptableState.preempt = False 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 _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 _enable_ros_control(self): super(PreemptableState, self)._enable_ros_control() self._pub.createPublisher(self._feedback_topic, CommandFeedback) self._sub.subscribe(self._preempt_topic, Empty) PreemptableState.preempt = False def _disable_ros_control(self): super(PreemptableState, self)._disable_ros_control() self._sub.unsubscribe_topic(self._preempt_topic)
class OperatableStateMachine(PreemptableStateMachine): """ A state machine that can be operated. It synchronizes its current state with the mirror and supports some control mechanisms. """ autonomy_level = 3 silent_mode = False def __init__(self, *args, **kwargs): super(OperatableStateMachine, self).__init__(*args, **kwargs) self._message = None self.id = None self.autonomy = None self._autonomy = {} self._ordered_states = [] self._pub = ProxyPublisher() self._sub = ProxySubscriberCached() @staticmethod def add(label, state, transitions = None, autonomy = None, remapping = None): """ Add a state to the opened state machine. @type label: string @param label: The label of the state being added. @param state: An instance of a class implementing the L{State} interface. @param transitions: A dictionary mapping state outcomes to other state labels or container outcomes. @param autonomy: A dictionary mapping state outcomes to their required autonomy level @param remapping: A dictionary mapping local userdata keys to userdata keys in the container. """ self = StateMachine._currently_opened_container() # add loopback transition to loopback states if isinstance(state, LoopbackState): transitions[LoopbackState._loopback_name] = label autonomy[LoopbackState._loopback_name] = -1 self._ordered_states.append(state) state.name = label state.transitions = transitions state.autonomy = autonomy state._parent = self StateMachine.add(label, state, transitions, remapping) self._autonomy[label] = autonomy def replace(self, new_state): old_state = self._states[new_state.name] new_state.transitions = old_state.transitions new_state.autonomy = old_state.autonomy new_state._parent = old_state._parent self._ordered_states[self._ordered_states.index(old_state)] = new_state self._states[new_state.name] = new_state def destroy(self): self._notify_stop() self._disable_ros_control() self._sub.unsubscribe_topic('/flexbe/command/autonomy') self._sub.unsubscribe_topic('/flexbe/command/sync') self._sub.unsubscribe_topic('/flexbe/request_mirror_structure') StateLogger.shutdown() def confirm(self, name, id): """ Confirms the state machine and triggers the creation of the structural message. It is mandatory to call this function at the top-level state machine between building it and starting its execution. @type name: string @param name: The name of this state machine to identify it. """ self.name = name self.id = id self._pub.createPublisher('/flexbe/mirror/sync', BehaviorSync, _latch = True) # Update mirror with currently active state (high bandwidth mode) self._pub.createPublisher('/flexbe/mirror/preempt', Empty, _latch = True) # Preempts the mirror self._pub.createPublisher('/flexbe/mirror/structure', ContainerStructure) # Sends the current structure to the mirror self._pub.createPublisher('/flexbe/log', BehaviorLog) # Topic for logs to the GUI self._pub.createPublisher('/flexbe/command_feedback', CommandFeedback) # Gives feedback about executed commands to the GUI self._sub.subscribe('/flexbe/command/autonomy', UInt8, self._set_autonomy_level) self._sub.subscribe('/flexbe/command/sync', Empty, self._sync_callback) self._sub.subscribe('/flexbe/request_mirror_structure', Int32, self._mirror_structure_callback) StateLogger.initialize(name) if OperatableStateMachine.autonomy_level != 255: self._enable_ros_control() rospy.sleep(0.5) # no clean way to wait for publisher to be ready... self._notify_start() def _set_autonomy_level(self, msg): """ Sets the current autonomy level. """ if OperatableStateMachine.autonomy_level != msg.data: rospy.loginfo('--> 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 _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 _mirror_structure_callback(self, msg): rospy.loginfo("--> Creating behavior structure for mirror...") msg = self._build_msg('') msg.behavior_id = self.id self._pub.publish('/flexbe/mirror/structure', msg) rospy.loginfo("<-- Sent behavior structure for mirror.") def _transition_allowed(self, label, outcome): return self._autonomy[label][outcome] < OperatableStateMachine.autonomy_level def _build_msg(self, prefix, msg = None): """ Adds this state machine to the initial structure message. @type prefix: string @param prefix: A path consisting of the container hierarchy containing this state. @type msg: ContainerStructure @param msg: The message that will finally contain the structure message. """ # set children children = [] for state in self._ordered_states: children.append(str(state.name)) # set name name = prefix + self.name if msg is None: # top-level state machine (has no transitions) self._message = ContainerStructure() outcomes = list(self._outcomes) transitions = None autonomy = None else: # lower-level state machine self._message = msg outcomes = list(self.transitions) # set transitions and autonomy transitions = [] autonomy = [] for i in range(len(self.transitions)): outcome = outcomes[i] if outcome == 'preempted': # set preempt transition transitions.append('preempted') autonomy.append(-1) else: transitions.append(str(self.transitions[outcome])) autonomy.append(self.autonomy[outcome]) # add to message self._message.containers.append(Container(name, children, outcomes, transitions, autonomy)) # build message for children for state in self._ordered_states: state._build_msg(name+'/', self._message) # top-level state machine returns the message if msg is None: return self._message def _notify_start(self): for state in self._ordered_states: if isinstance(state, LoopbackState): state.on_start() if isinstance(state, OperatableStateMachine): state._notify_start() def _enable_ros_control(self): for state in self._ordered_states: if isinstance(state, LoopbackState): state._enable_ros_control() if isinstance(state, OperatableStateMachine): state._enable_ros_control() def _notify_stop(self): for state in self._ordered_states: if isinstance(state, LoopbackState): state.on_stop() state._disable_ros_control() if isinstance(state, OperatableStateMachine): state._notify_stop() def _disable_ros_control(self): for state in self._ordered_states: if isinstance(state, LoopbackState): state._disable_ros_control() if isinstance(state, OperatableStateMachine): state._disable_ros_control()
class CheckForPersonState(EventState): """ Checks whether or not the robot sees a Person -- name String Name of the Person to search for -- timeout float Timeout in seconds <= found The Person was found <= not_found The Person was not found <= failed Execution failed """ def __init__(self, name, timeout=2.0): """Constructor""" super(CheckForPersonState, self).__init__(outcomes=['found', 'not_found', 'failed']) self._name = name self._mutex = Lock() self._people = [] self._timeout = timeout self._start_time = None self._callback_received = False self._transform_topic = '/clf_perception_vision/people/raw/transform' self._service_name = 'pepper_face_identification' self._service_proxy = ProxyServiceCaller( {self._service_name: DoIKnowThatPerson}) self._transform_listener = ProxySubscriberCached( {self._transform_topic: ExtendedPeople}) def people_callback(self, data): self._mutex.acquire() self._callback_received = True self._people = [] for p in data.persons: self._people.append(p) self._transform_listener.unsubscribe_topic(self._transform_topic) self._mutex.release() def execute(self, userdata): """wait for transform callback, check all transforms for requested person""" elapsed = rospy.get_rostime() - self._start_time if elapsed.to_sec() > self._timeout: return 'not_found' self._mutex.acquire() if self._callback_received: known = False for p in self._people: request = DoIKnowThatPersonRequest() request.transform_id = p.transformid response = self._service_proxy.call(self._service_name, request) if response.name == self.name: known = True self._mutex.release() if known: return 'found' else: return 'not_found' self._mutex.release() def on_enter(self, userdata): """Subscribe to transforms, start timeout-timer""" self._start_time = rospy.get_rostime() self._transform_listener.subscribe(self._transform_topic, ExtendedPeople, self.people_callback) def on_exit(self, userdata): self._transform_listener.unsubscribe_topic(self._transform_topic) def on_stop(self): self._transform_listener.unsubscribe_topic(self._transform_topic)