def execute(self, userdata): smach.loginfo(self.__class__.__name__ + " must be subclassed. It does nothing.") time.sleep(SLEEP_TIME) # why do we need to sleep? self.counter += 1 # shouldn't this be incremented after? if self.outcomes is None: self.outcomes = self.get_registered_outcomes() return self.outcomes[self.counter%len(self.outcomes)]
def execute(self, userdata): smach.loginfo(self.__class__.__name__ + " must be subclassed. It does nothing.") time.sleep(SLEEP_TIME) self.counter += 1 if self.outcomes is None: self.outcomes = self.get_registered_outcomes() return self.outcomes[self.counter%len(self.outcomes)]
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 execute(self, userdata): # Print input keys to terminal for input_key in self._input_keys: smach.loginfo('userdata.{}: {}'.format(input_key, userdata[input_key])) return 'succeeded'
def execute(self, userdata): smach.loginfo(self.__class__.__name__ + " must be subclassed. It does nothing.") time.sleep(SLEEP_TIME) self.counter += 1 if self.outcomes is None: self.outcomes = self.get_registered_outcomes() return self.outcomes[self.counter % len(self.outcomes)]
def execute(self, parent_ud): self._is_running = True # Copy input keys self._copy_input_keys(parent_ud, self.userdata) self.call_start_cbs() # Iterate over items outcome = self._exhausted_outcome if hasattr(self._items, '__call__'): it = self._items().__iter__() else: it = self._items.__iter__() while not smach.is_shutdown(): try: item = next(it) except: outcome = self._exhausted_outcome break smach.loginfo("Iterating %s of %s" % (str(item), str(self._items))) self.userdata[self._items_label] = item # Enter the contained state try: outcome = self._state.execute(self.userdata) except smach.InvalidUserCodeError as ex: smach.logerr("Could not execute Iterator state '%s'" % self._state_label) raise ex except: raise smach.InvalidUserCodeError( "Could not execute iterator state '%s' of type '%s': " % (self._state_label, self._state) + traceback.format_exc()) # Check if we should stop preemptively if self.preempt_requested(): self.service_preempt() outcome = 'preempted' break if outcome in self._break_outcomes\ or (len(self._loop_outcomes) > 0 and outcome not in self._loop_outcomes): break self.call_transition_cbs() # Remap the outcome if necessary if outcome in self._final_outcome_map: outcome = self._final_outcome_map[outcome] # Copy output keys self._copy_output_keys(self.userdata, parent_ud) self._is_running = False self.call_termination_cbs(self._state_label, outcome) return outcome
def _state_runner(self, label): """Runs the states in parallel threads.""" # Wait until all threads are ready to start before beginnging self._ready_event.wait() self.call_transition_cbs() # Execute child state try: self._child_outcomes[label] = self._states[label].execute( smach.Remapper( self.userdata, self._states[label].get_registered_input_keys(), self._states[label].get_registered_output_keys(), self._remappings[label])) except PreemptException: self._child_outcomes[label] = self._default_outcome except: self._user_code_exception = True with self._done_cond: self._done_cond.notify_all() raise smach.InvalidStateError( ("Could not execute child state '%s': " % label) + traceback.format_exc()) # Make sure the child returned an outcome if self._child_outcomes[label] is None: raise smach.InvalidStateError( "Concurrent state '%s' returned no outcome on termination." % label) else: smach.loginfo( "Concurrent state '%s' returned outcome '%s' on termination." % (label, self._child_outcomes[label])) self._done_cond.acquire() self.request_preempt() self._done_cond.notify_all() self._done_cond.release() # Check if all of the states have completed with self._done_cond: # initialize preemption flag preempt_others = False # Call transition cb's self.call_transition_cbs() # Call child termination cb if it's defined if self._child_termination_cb: try: preempt_others = self._child_termination_cb( self._child_outcomes) except: raise smach.InvalidUserCodeError( "Could not execute child termination callback: " + traceback.format_exc()) # Notify the container to terminate (and preempt other states if neceesary) if preempt_others or all( [o is not None for o in self._child_outcomes.values()]): self._done_cond.notify_all()
def 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 preempt state self._preempted_label = None self._preempted_state = None # Set initial state self._set_current_state(self._initial_state_label) # Copy input keys self._copy_input_keys(parent_ud, self.userdata) # Spew some info smach.loginfo("State machine starting in initial state '%s' with userdata: \n\t%s" % (self._current_label, list(self.userdata.keys()))) if self._preempt_requested: smach.logwarn("Preempt on State machine requested before even executing initial state. This could be a bug. Did last execution not service preemption?") # Call start callbacks self.call_start_cbs() # Initialize container outcome container_outcome = None # Step through state machine while container_outcome is None and self._is_running and not smach.is_shutdown(): # Update the state machine container_outcome = self._update_once() # Copy output keys self._copy_output_keys(self.userdata, parent_ud) # We're no longer running self._is_running = False if self._preempt_requested: self.service_preempt() return container_outcome
def execute(self, parent_ud): self._is_running = True # Copy input keys self._copy_input_keys(parent_ud, self.userdata) self.call_start_cbs() # Iterate over items outcome = self._exhausted_outcome if hasattr(self._items,'__call__'): it = self._items().__iter__() else: it = self._items.__iter__() while not smach.is_shutdown(): try: item = next(it) except: outcome = self._exhausted_outcome break smach.loginfo("Iterating %s of %s" % (str(item), str(self._items))) self.userdata[self._items_label] = item # Enter the contained state try: outcome = self._state.execute(self.userdata) except smach.InvalidUserCodeError as ex: smach.logerr("Could not execute Iterator state '%s'" % self._state_label) raise ex except: raise smach.InvalidUserCodeError("Could not execute iterator state '%s' of type '%s': " % ( self._state_label, self._state) + traceback.format_exc()) # Check if we should stop preemptively if self._preempt_requested\ or outcome in self._break_outcomes\ or (len(self._loop_outcomes) > 0 and outcome not in self._loop_outcomes): self._preempt_requested = False break self.call_transition_cbs() # Remap the outcome if necessary if outcome in self._final_outcome_map: outcome = self._final_outcome_map[outcome] # Copy output keys self._copy_output_keys(self.userdata, parent_ud) self._is_running = False self.call_termination_cbs(self._state_label,outcome) return outcome
def setup_state_machine(self): smach.loginfo("Initializing state machine") rospy.sleep(1) with self.state_machine: smach.StateMachine.add('START', Start(self.davinciArm), transitions={'success':'MOVE_TO_RETRACTION_STAGING_AREA'}, remapping ={'retractionStagingPose':'sm_data1', 'graspPoint':'sm_data2', 'dropOffStagingPose': 'sm_data3', 'dropOffPose': 'sm_data4'} ) smach.StateMachine.add('MOVE_TO_RETRACTION_STAGING_AREA', MoveToRetractionStagingArea(self.davinciArm), transitions={'success':'MOVE_TO_GRASP_POINT', 'failure': 'HOME_POSITION'}, remapping = {'retractionStagingPose':'sm_data1'}) smach.StateMachine.add('HOME_POSITION', HomePosition(self.davinciArm), transitions={'success':'START', 'failure': 'HOME_POSITION'}) smach.StateMachine.add('MOVE_TO_GRASP_POINT', MoveToGraspPoint(self.davinciArm), transitions={'success':'GRASP_BLOCK'}, remapping = {'graspPoint': 'sm_data2'}) smach.StateMachine.add('GRASP_BLOCK', GraspBlock(self.davinciArm), transitions={'success':'RETURN_TO_RETRACTION_STAGING_AREA_WITH_BLOCK', 'failure': 'RELEASE_GRIPPERS_NO_BLOCK'}, remapping = {'graspPoint': 'sm_data2'}) smach.StateMachine.add('CHECK_GRASP', CheckGrasp(self.davinciArm), transitions={'success':'MOVE_TO_DROP_OFF_STAGING_AREA', 'failure': 'RELEASE_GRIPPERS_NO_BLOCK'}, remapping = {'graspPoint': 'sm_data2'}) smach.StateMachine.add('RELEASE_GRIPPERS_NO_BLOCK', ReleaseGrippersNoBlock(self.davinciArm), transitions={'success':'MOVE_TO_RETRACTION_STAGING_AREA', 'failure': 'RELEASE_GRIPPERS_NO_BLOCK'}) smach.StateMachine.add('RETURN_TO_RETRACTION_STAGING_AREA_WITH_BLOCK', ReturnToRetractionStagingAreaWithBlock(self.davinciArm), transitions={'success':'CHECK_GRASP', 'failure': 'ABORT'}, remapping = {'retractionStagingPose':'sm_data1'}) smach.StateMachine.add('MOVE_TO_DROP_OFF_STAGING_AREA', MoveToDropOffStagingArea(self.davinciArm), transitions={'success':'MOVE_TO_DROP_OFF_POINT', 'failure': 'ABORT'}, remapping = {'dropOffStagingPose': 'sm_data3'}) smach.StateMachine.add('MOVE_TO_DROP_OFF_POINT', MoveToDropOffPoint(self.davinciArm), transitions={'success':'RELEASE_GRIPPERS', 'failure': 'ABORT'}, remapping = {'dropOffPose':'sm_data4'}) smach.StateMachine.add('RELEASE_GRIPPERS', ReleaseGripper(self.davinciArm), transitions={'success':'CHECK_DROP_OFF', 'failure': 'RELEASE_GRIPPERS'}, remapping = {'dropOffPose':'sm_data4'}) smach.StateMachine.add('CHECK_DROP_OFF', CheckDropOff(self.davinciArm), transitions={'stillLooping': 'HOME_POSITION', 'success':'SUCCESS', 'failure': 'ABORT'}) smach.StateMachine.add('ABORT', Abort(self.davinciArm), transitions={'failure': 'FAILURE'})
def 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 preempt state self._preempted_label = None self._preempted_state = None # Set initial state self._set_current_state(self._initial_state_label) # Copy input keys self._copy_input_keys(parent_ud, self.userdata) # Spew some info smach.loginfo( "State machine starting in initial state '%s' with userdata: \n\t%s" % (self._current_label, str(self.userdata.keys())) ) # Call start callbacks self.call_start_cbs() # Initialize container outcome container_outcome = None # Step through state machine while container_outcome is None and self._is_running and not smach.is_shutdown(): # Update the state machine container_outcome = self._update_once() # Copy output keys self._copy_output_keys(self.userdata, parent_ud) # We're no longer running self._is_running = False return container_outcome
def _state_runner(self, label): """Runs the states in parallel threads.""" # Wait until all threads are ready to start before beginnging self._ready_event.wait() self.call_transition_cbs() # Execute child state try: self._child_outcomes[label] = self._states[label].execute( smach.Remapper( self.userdata, self._states[label].get_registered_input_keys(), self._states[label].get_registered_output_keys(), self._remappings[label], ) ) except: self._user_code_exception = True with self._done_cond: self._done_cond.notify_all() raise smach.InvalidStateError(("Could not execute child state '%s': " % label) + traceback.format_exc()) # Make sure the child returned an outcome if self._child_outcomes[label] is None: raise smach.InvalidStateError("Concurrent state '%s' returned no outcome on termination." % label) else: smach.loginfo( "Concurrent state '%s' returned outcome '%s' on termination." % (label, self._child_outcomes[label]) ) # Check if all of the states have completed with self._done_cond: # initialize preemption flag preempt_others = False # Call transition cb's self.call_transition_cbs() # Call child termination cb if it's defined if self._child_termination_cb: try: preempt_others = self._child_termination_cb(self._child_outcomes) except: raise smach.InvalidUserCodeError( "Could not execute child termination callback: " + traceback.format_exc() ) # Notify the container to terminate (and preempt other states if neceesary) if preempt_others or all([o is not None for o in self._child_outcomes.values()]): self._done_cond.notify_all()
def _set_current_state(self, state_label): if type(state_label) == list(): state_label = state_label[0] if state_label is not None: # Store the current label and states smach.loginfo(state_label) self._current_label = state_label self._current_state = self._states[state_label] self._current_transitions = self._transitions[state_label] self._current_outcome = None else: # Store the current label and states self._current_label = None self._current_state = None self._current_transitions = None self._current_outcome = None
def _set_current_state(self, state_label): if state_label is not None: smach.loginfo(self._states.keys()) # Store the current label and states self._current_label = state_label self._current_state = self._states[state_label] self._current_outcome = None #if type(self._current_state) == smach.StateMachineRL: #self.state_label = self._current_state.state_label # else: # self.state_label = None else: # Store the current label and states self._current_label = None self._current_state = None self._current_outcome = None
def execute(self, userdata): for input_key in self._input_keys: smach.loginfo('Userdata input key \'{}\' BEFORE callback execution: {}'.format(input_key, userdata[input_key])) # Call callbacks for (cb, ik, ok) in zip(self._cbs, self._cb_input_keys, self._cb_output_keys): # Call callback with limited userdata try: cb_outcome = cb(self, smach.Remapper(userdata,ik,ok,{})) except: cb_outcome = cb(smach.Remapper(userdata,ik,ok,{})) for input_key in self._input_keys: smach.loginfo('Userdata input key \'{}\' AFTER callback execution: {}'.format(input_key, userdata[input_key])) return 'succeeded'
def add_learner(self, label, container): self.container = container rl_states = list() self.find_keys(rl_states, self._states) rl_states.append((label, self)) smach.loginfo(rl_states) for (l, s) in rl_states: learner.add_state_actions(l, s._states.keys()) if s.state_label != None: self.multi_used_sm = True # temp = s.state_label # s.state_label = list() # s.state_label.append(temp) #if type(s.state_label) == list(): #s.state_label.append(l) #s.state_label = l for (l, s) in rl_states: smach.loginfo(s.state_label) #learner.add_state_actions(label, keys) self.state_label = label self.initial_state = label
def add_learner(self, label, container): self.container = container rl_states = list() self.find_keys(rl_states, self._states) rl_states.append((label,self)) smach.loginfo(rl_states) for (l,s) in rl_states: learner.add_state_actions(l, s._states.keys()) if s.state_label != None: self.multi_used_sm = True # temp = s.state_label # s.state_label = list() # s.state_label.append(temp) #if type(s.state_label) == list(): #s.state_label.append(l) #s.state_label = l for (l,s) in rl_states: smach.loginfo(s.state_label) #learner.add_state_actions(label, keys) self.state_label = label self.initial_state = label
def main(): # print "Started main" # smach.loginfo("Starting main...") # # traj = get_full_traj() # traj = (1, 1) # print "Loaded traj" # dummy_left_arm = None # right_arm = MasterClass(traj[1], dummy_left_arm) # davinciArmLeft = RavenArm(raven_constants.Arm.Left) # davinciArmLeft.start() # left_arm = Follower(traj[0], davinciArmLeft) # print "Follower started" # print "Master created" # right_arm.run() print "Started main" smach.loginfo("Starting main...") traj = get_full_traj() # traj = (1, 1) print "Loaded traj" master = MasterClass(traj) print "Master created" master.run()
def request_preempt(self): smach.loginfo("Preempt requested on iterator") smach.State.request_preempt(self)
def _update_once(self): """Method that updates the state machine once. This checks if the current state is ready to transition, if so, it requests the outcome of the current state, and then extracts the next state label from the current state's transition dictionary, and then transitions to the next state. """ outcome = None transition_target = None last_state_label = self._current_label # Make sure the state exists if self._current_label not in self._states: raise smach.InvalidStateError("State '%s' does not exist. Available states are: %s" % (self._current_label, list(self._states.keys()))) # Check if a preempt was requested before or while the last state was running if self.preempt_requested(): smach.loginfo("Preempt requested on state machine before executing the next state.") # We were preempted if self._preempted_state is not None: # We were preempted while the last state was running if self._preempted_state.preempt_requested(): smach.loginfo("Last state '%s' did not service preempt. Preempting next state '%s' before executing..." % (self._preempted_label, self._current_label)) # The flag was not reset, so we need to keep preempting # (this will reset the current preempt) self._preempt_current_state() else: # The flag was reset, so the container can reset self._preempt_requested = False self._preempted_state = None else: # We were preempted after the last state was running # So we should preempt this state before we execute it self._preempt_current_state() # Execute the state try: self._state_transitioning_lock.release() outcome = self._current_state.execute( smach.Remapper( self.userdata, self._current_state.get_registered_input_keys(), self._current_state.get_registered_output_keys(), self._remappings[self._current_label])) except smach.InvalidUserCodeError as ex: smach.logerr("State '%s' failed to execute." % self._current_label) raise ex except: raise smach.InvalidUserCodeError("Could not execute state '%s' of type '%s': " % (self._current_label, self._current_state) + traceback.format_exc()) finally: self._state_transitioning_lock.acquire() # Check if outcome was a potential outcome for this type of state if outcome not in self._current_state.get_registered_outcomes(): raise smach.InvalidTransitionError( "Attempted to return outcome '%s' from state '%s' of" " type '%s' which only has registered outcomes: %s" % (outcome, self._current_label, self._current_state, self._current_state.get_registered_outcomes())) # Check if this outcome is actually mapped to any target if outcome not in self._current_transitions: raise smach.InvalidTransitionError("Outcome '%s' of state '%s' is not bound to any transition target. Bound transitions include: %s" % (str(outcome), str(self._current_label), str(self._current_transitions))) # Set the transition target transition_target = self._current_transitions[outcome] # Check if the transition target is a state in this state machine, or an outcome of this state machine if (isinstance(transition_target, list) and set(transition_target).intersection(self._states)) or (isinstance(transition_target, str) and transition_target in self._states): # Set the new state self._set_current_state(transition_target) # Spew some info smach.loginfo("State machine transitioning '%s':'%s'-->'%s'" % (last_state_label, outcome, transition_target)) if type(self._states[transition_target] == smach.StateMachineRL): self._states[transition_target].state_label = transition_target if 'reward' in self.userdata: reward = self.userdata['reward'] self.publisher.update_rl(last_state_label, -reward, last_state_label, self.num) # Call transition callbacks self.call_transition_cbs() else: # This is a terminal state self.publisher.update_rl('END', 0, 'END', self.num) if self._preempt_requested and self._preempted_state is not None: if not self._current_state.preempt_requested(): self.service_preempt() if transition_target not in self.get_registered_outcomes(): # This is a container outcome that will fall through transition_target = outcome if transition_target in self.get_registered_outcomes(): # The transition target is an outcome of the state machine self._set_current_state(None) # Spew some info smach.loginfo("State machine terminating '%s':'%s':'%s'" % (last_state_label, outcome, transition_target)) # Call termination callbacks self.call_termination_cbs([last_state_label],transition_target) return transition_target else: raise smach.InvalidTransitionError("Outcome '%s' of state '%s' with transition target '%s' is neither a registered state nor a registered container outcome." % (outcome, self._current_label, transition_target)) return None
def 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 preempt state self._preempted_label = None self._preempted_state = None # Set initial state self._set_current_state(self._initial_state_label) # Copy input keys self._copy_input_keys(parent_ud, self.userdata) # Spew some info smach.loginfo( "State machine starting in initial state '%s' with userdata: \n\t%s" % (self._current_label, list(self.userdata.keys()))) # Call start callbacks self.call_start_cbs() # Initialize container outcome container_outcome = None # Step through state machine while container_outcome is None and self._is_running and not smach.is_shutdown( ): # Update the state machine try: container_outcome = self._update_once() except Exception as ex: # Save the coverage information if 'SSCOV_FILE' in os.environ: covbasename = os.environ['SSCOV_FILE'] else: covbasename = '.scov' # Determine if suffixes are required if StateMachine.cov_instance_counter == 1: covsuffix = '' else: covsuffix = '_' + str(self.cov_id) with open(covbasename + covsuffix, 'w') as covfile: pickle.dump(self.cov_states, covfile) with open('.ERRORS_' + covbasename + covsuffix, 'w') as ef: ef.write(str(ex)) raise # Copy output keys self._copy_output_keys(self.userdata, parent_ud) # We're no longer running self._is_running = False # Save the coverage information if 'SSCOV_FILE' in os.environ: covbasename = os.environ['SSCOV_FILE'] else: covbasename = '.scov' # Determine if suffixes are required if StateMachine.cov_instance_counter == 1: covsuffix = '' else: covsuffix = '_' + str(self.cov_id) with open(covbasename + covsuffix, 'w') as covfile: pickle.dump(self.cov_states, covfile) return container_outcome
def 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 preempt state self._preempted_label = None self._preempted_state = None # Set initial state self._set_current_state(self._initial_state_label) # Copy input keys self._copy_input_keys(parent_ud, self.userdata) # Spew some info smach.loginfo( "State machine starting in initial state '%s' with userdata: \n\t%s" % (self._current_label, list(self.userdata.keys()))) # Call start callbacks self.call_start_cbs() # Initialize container outcome container_outcome = None ## Copy the datamodel's value into the userData for data in self._datamodel: self.userdata[data] = self._datamodel[data] ## Do the <onentry> if (self._onEntry is not None): try: self._onEntry.execute(self.userdata) except Exception as ex: rospy.logerr('%s::onEntry::execute() raised | %s' % (self.__class__.__name__, str(ex))) return "preempt" # Step through state machine while container_outcome is None and self._is_running and not smach.is_shutdown( ): # Update the state machine if (self._pause): self._tree_view_pause_state(self._current_label) cpt = 30 while (self._pause): if (cpt >= 30): cpt = 0 rospy.logwarn( "[SSM] : Smart State Machine is paused") rospy.logwarn("[SSM] : Next state executed is : " + self._current_label) cpt = cpt + 1 rospy.sleep(0.1) if (self._preempt_pause): self._tree_view_disable_state(self._current_label) return "preempt" container_outcome = self._update_once() ## Do the <onexit> if (self._onExit is not None): try: container_outcome = self._onExit.execute( self.userdata, container_outcome) except Exception as ex: rospy.logerr('%s::onExit::execute() raised | %s' % (self.__class__.__name__, str(ex))) return "preempt" # Copy output keys self._copy_output_keys(self.userdata, parent_ud) # We're no longer running self._is_running = False return container_outcome
def setup_state_machine(self, traj, left_arm): smach.loginfo("Initializing state machine") rospy.sleep(1) print "Setup SM " with self.state_machine: # ------ Phase 1 ----- smach.StateMachine.add('START', Start(self.davinciArmRight), transitions={'success':'DETECT_CIRCLE'}) smach.StateMachine.add('DETECT_CIRCLE', DetectCircle(self.davinciArmRight), transitions={'success':'WARP'}) smach.StateMachine.add('WARP', Warp(self.davinciArmRight, traj, left_arm), transitions={'success':'GRASP_GAUZE'}, remapping ={'new_traj_R':'sm_data1'}) smach.StateMachine.add('GRASP_GAUZE', GraspGauze(self.davinciArmRight, traj, left_arm), transitions={'success':'CHECK_GRASP'}, remapping ={'new_traj_R':'sm_data1'}) smach.StateMachine.add('CHECK_GRASP', CheckGrasp(self.davinciArmRight), transitions={'success':'EXECUTE_NOTCH_CUT'}) # smach.StateMachine.add('CHECK_GRASP', # CheckGrasp(self.davinciArmRight), # transitions={'success':'EXECUTE_NOTCH_CUT', 'failure': 'WARP_GRASP'},) # smach.StateMachine.add('CHECK_GRASP', # CheckGrasp(self.davinciArmRight), # transitions={'success':'EXECUTE_NOTCH_CUT', 'failure': 'WARP_GRASP'},) # smach.StateMachine.add('WARP_GRASP', # WarpGrasp(self.davinciArmRight, traj, left_arm), # transitions={'success':'EXECUTE_REGRASP'}) # smach.StateMachine.add('EXECUTE_REGRASP', # ExecuteRegrasp(self.davinciArmRight), # transitions={'success':'CHECK_GRASP'}) smach.StateMachine.add('EXECUTE_NOTCH_CUT', ExecuteNotchCut(self.davinciArmRight, traj, left_arm), transitions={'success':'CHECK_NOTCH'}, remapping ={'new_traj_R':'sm_data1'}) smach.StateMachine.add('CHECK_NOTCH', CheckNotch(self.davinciArmRight), transitions={'success':'EXECUTE_PARTIAL_TRAJ_2', 'failure':'EXECUTE_NOTCH_CUT'}) # ------ Phase 2 ----- smach.StateMachine.add('EXECUTE_PARTIAL_TRAJ_2', ExecutePartialTraj2(self.davinciArmRight, traj, left_arm), transitions={'complete':'TRANSFER'}, remapping ={'new_traj_R':'sm_data1'}) smach.StateMachine.add('TRANSFER', PhaseTransfer(self.davinciArmRight, traj, left_arm), transitions={'success':'EXECUTE_PARTIAL_TRAJ_3'}, remapping ={'new_traj_R':'sm_data1'}) # smach.StateMachine.add('EXECUTE_PARTIAL_TRAJ_2', # ExecutePartialTraj2(self.davinciArmRight, traj, left_arm), # transitions={'looping':'CHECK_POSITION_2', 'complete':'EXECUTE_PARTIAL_TRAJ_3'}) # smach.StateMachine.add('CHECK_POSITION_2', # CheckPostion2(self.davinciArmRight), # transitions={'success':'EXECUTE_PARTIAL_TRAJ_2', 'above': 'WARP_ABOVE_2', 'below': 'WARP_BELOW_2'}) # smach.StateMachine.add('WARP_ABOVE_2', # WarpAbove2(self.davinciArmRight), # transitions={'success':'MOVE_ABOVE_2'}) # smach.StateMachine.add('MOVE_ABOVE_2', # MoveAbove2(self.davinciArmRight), # transitions={'success':'CHECK_POSITION_2'}) # smach.StateMachine.add('WARP_BELOW_2', # WarpBelow2(self.davinciArmRight), # transitions={'success':'MOVE_BELOW_2'}) # smach.StateMachine.add('MOVE_BELOW_2', # MoveBelow2(self.davinciArmRight), # transitions={'success':'CHECK_POSITION_2'}) # ------ Phase 3 ----- smach.StateMachine.add('EXECUTE_PARTIAL_TRAJ_3', ExecutePartialTraj3(self.davinciArmRight, traj, left_arm), transitions={'abridged_state_machine':'SUCCESS'}, remapping ={'new_traj_R':'sm_data1'})
def execute(self, parent_ud=smach.UserData()): """Overridden execute method. This starts all the threads. """ # Clear the ready event self._ready_event.clear() # Reset child outcomes self._child_outcomes = {} # Copy input keys self._copy_input_keys(parent_ud, self.userdata) # Spew some info smach.loginfo("Concurrence starting with userdata: \n\t%s" % (str(list(self.userdata.keys())))) # Call start callbacks self.call_start_cbs() # Create all the threads for (label, state) in ((k, self._states[k]) for k in self._states): # Initialize child outcomes self._child_outcomes[label] = None self._threads[label] = threading.Thread( name="concurrent_split:" + label, target=self._state_runner, args=(label,) ) # Launch threads for thread in self._threads.values(): thread.start() # Wait for done notification self._done_cond.acquire() # Notify all threads ready to go self._ready_event.set() # Wait for a done notification from a thread self._done_cond.wait() self._done_cond.release() # Preempt any running states smach.logdebug("SMACH Concurrence preempting running states.") for label in self._states: if self._child_outcomes[label] == None: self._states[label].request_preempt() # Wait for all states to terminate while not smach.is_shutdown(): if all([not t.isAlive() for t in self._threads.values()]): break self._done_cond.acquire() self._done_cond.wait(0.1) self._done_cond.release() # Check for user code exception if self._user_code_exception: self._user_code_exception = False raise smach.InvalidStateError("A concurrent state raised an exception during execution.") # Check for preempt if self.preempt_requested(): # initialized serviced flag children_preempts_serviced = True # Service this preempt if for (label, state) in ((k, self._states[k]) for k in self._states): if state.preempt_requested(): # Reset the flag children_preempts_serviced = False # Complain smach.logwarn("State '%s' in concurrence did not service preempt." % label) # Recall the preempt if it hasn't been serviced state.recall_preempt() if children_preempts_serviced: smach.loginfo("Concurrence serviced preempt.") self.service_preempt() # Spew some debyg info smach.loginfo("Concurrent Outcomes: " + str(self._child_outcomes)) # Initialize the outcome outcome = self._default_outcome # Determine the outcome from the outcome map smach.logdebug("SMACH Concurrence determining contained state outcomes.") for (container_outcome, outcomes) in ((k, self._outcome_map[k]) for k in self._outcome_map): if all([self._child_outcomes[label] == outcomes[label] for label in outcomes]): smach.logdebug("Terminating concurrent split with mapped outcome.") outcome = container_outcome # Check outcome callback if self._outcome_cb: try: cb_outcome = self._outcome_cb(copy.copy(self._child_outcomes)) if cb_outcome: if cb_outcome == str(cb_outcome): outcome = cb_outcome else: smach.logerr( "Outcome callback returned a non-string '%s', using default outcome '%s'" % (str(cb_outcome), self._default_outcome) ) else: smach.logwarn("Outcome callback returned None, using outcome '%s'" % outcome) except: raise smach.InvalidUserCodeError( ("Could not execute outcome callback '%s': " % self._outcome_cb) + traceback.format_exc() ) # Cleanup self._threads = {} self._child_outcomes = {} # Call termination callbacks self.call_termination_cbs(list(self._states.keys()), outcome) # Copy output keys self._copy_output_keys(self.userdata, parent_ud) return outcome
def 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. """ #smach.loginfo(learner.q_table) smach.loginfo(learner.q_table_additive_states) # Initialize preempt state self._preempted_label = None self._preempted_state = None current_label = self.state_label # Set initial state. TODO:Do this using RL. #val = random.randrange(0,len(self._states),1) label_transition = learner.get_action(self.state_label) smach.loginfo('CONTAINER LABEL: ' + str(self.state_label) + ' ACTION LABEL: ' + label_transition) #smach.loginfo(val) #label = self._labels[val] self._set_current_state(label_transition) #self._current_state.state_label = label_transition next_label = label_transition smach.loginfo('CONTAINER LABEL AFTER TRANSITION: ' + str(label_transition)) self._states[label_transition].state_label = label_transition #If the next-state is an RL-State, there is no reward until that state is executed. Add the current state to be updated #by the next-states reward. if next_label in learner.q_table.keys(): learner.queue_state_update(current_label, label_transition) #smach.loginfo(self._states['START_NAV'].state_label) # Copy input keys self._copy_input_keys(parent_ud, self.userdata) execution_outcome = self._current_state.execute((smach.Remapper( self.userdata, self._states[label_transition].get_registered_input_keys(), self._states[label_transition].get_registered_output_keys(), self._remappings[label_transition]))) if 'reward' in self.userdata: reward = self.userdata['reward'] self.publisher.update_rl(current_label, -reward, label_transition, self.container.num) #learner.update(next_label, label_transition, current_label, -reward) #learner.update_queued_states(next_label, -reward) # Spew some info #smach.loginfo("RL Stuff '%s' with userdata: \n\t%s" % # (self._current_label, list(self.userdata.keys()))) # Copy output keys self._copy_output_keys(self.userdata, parent_ud) # We're no longer running self._is_running = False found = False for (container_outcome, outcomes) in ((k, self._outcome_map[k]) for k in self._outcome_map): if execution_outcome in outcomes: found = True break if found == False: smach.loginfo("RL BAD OUTCOME %s", execution_outcome) smach.loginfo('CONTAINER OUTCOME: ' + container_outcome + ' EXECUTION OUTCOME: ' + execution_outcome) return container_outcome
def 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. """ #smach.loginfo(learner.q_table) smach.loginfo(learner.q_table_additive_states) # Initialize preempt state self._preempted_label = None self._preempted_state = None current_label = self.state_label # Set initial state. TODO:Do this using RL. #val = random.randrange(0,len(self._states),1) label_transition = learner.get_action(self.state_label) smach.loginfo('CONTAINER LABEL: ' + str(self.state_label) + ' ACTION LABEL: ' + label_transition) #smach.loginfo(val) #label = self._labels[val] self._set_current_state(label_transition) #self._current_state.state_label = label_transition next_label = label_transition smach.loginfo('CONTAINER LABEL AFTER TRANSITION: ' + str(label_transition)) self._states[label_transition].state_label = label_transition #If the next-state is an RL-State, there is no reward until that state is executed. Add the current state to be updated #by the next-states reward. if next_label in learner.q_table.keys(): learner.queue_state_update(current_label, label_transition) #smach.loginfo(self._states['START_NAV'].state_label) # Copy input keys self._copy_input_keys(parent_ud, self.userdata) execution_outcome = self._current_state.execute((smach.Remapper( self.userdata, self._states[label_transition].get_registered_input_keys(), self._states[label_transition].get_registered_output_keys(), self._remappings[label_transition]))) if 'reward' in self.userdata: reward = self.userdata['reward'] self.publisher.update_rl(current_label, -reward, label_transition, self.container.num) #learner.update(next_label, label_transition, current_label, -reward) #learner.update_queued_states(next_label, -reward) # Spew some info #smach.loginfo("RL Stuff '%s' with userdata: \n\t%s" % # (self._current_label, list(self.userdata.keys()))) # Copy output keys self._copy_output_keys(self.userdata, parent_ud) # We're no longer running self._is_running = False found = False for (container_outcome, outcomes) in ((k,self._outcome_map[k]) for k in self._outcome_map): if execution_outcome in outcomes: found = True break if found == False: smach.loginfo("RL BAD OUTCOME %s", execution_outcome) smach.loginfo('CONTAINER OUTCOME: ' + container_outcome + ' EXECUTION OUTCOME: ' + execution_outcome) return container_outcome
def 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 preempt state self._preempted_label = None self._preempted_state = None # Set initial state self._set_current_state(self._initial_state_label) # Copy input keys self._copy_input_keys(parent_ud, self.userdata) # Spew some info smach.loginfo("State machine starting in initial state '%s' with userdata: \n\t%s" % (self._current_label, list(self.userdata.keys()))) # Call start callbacks self.call_start_cbs() # Initialize container outcome container_outcome = None # Step through state machine while container_outcome is None and self._is_running and not smach.is_shutdown(): # Update the state machine try: container_outcome = self._update_once() except Exception as ex: # Save the coverage information if 'SSCOV_FILE' in os.environ: covbasename = os.environ['SSCOV_FILE'] else: covbasename = '.scov' # Determine if suffixes are required if StateMachine.cov_instance_counter == 1: covsuffix = '' else: covsuffix = '_' + str(self.cov_id) with open(covbasename + covsuffix, 'w') as covfile: pickle.dump(self.cov_states, covfile) with open('.ERRORS_' + covbasename + covsuffix, 'w') as ef: ef.write(str(ex)) raise # Copy output keys self._copy_output_keys(self.userdata, parent_ud) # We're no longer running self._is_running = False # Save the coverage information if 'SSCOV_FILE' in os.environ: covbasename = os.environ['SSCOV_FILE'] else: covbasename = '.scov' # Determine if suffixes are required if StateMachine.cov_instance_counter == 1: covsuffix = '' else: covsuffix = '_' + str(self.cov_id) with open(covbasename + covsuffix, 'w') as covfile: pickle.dump(self.cov_states, covfile) return container_outcome
def execute(self, parent_ud=smach.UserData()): """Overridden execute method. This starts all the threads. """ # Clear the ready event self._ready_event.clear() # Reset child outcomes self._child_outcomes = {} # Copy input keys self._copy_input_keys(parent_ud, self.userdata) ## Copy the datamodel's value into the userData for data in self._datamodel: if (self._datamodel[data] != ""): self.userdata[data] = self._datamodel[data] ## Do the <onentry> if (self._onEntry is not None): try: self._onEntry.execute(self.userdata) except Exception as ex: rospy.logerr('%s::onEntry::execute() raised | %s' % (self.__class__.__name__, str(ex))) return "preempt" # Spew some info smach.loginfo("Concurrence starting with userdata: \n\t%s" % (str(list(self.userdata.keys())))) # Call start callbacks self.call_start_cbs() # Create all the threads for (label, state) in ((k, self._states[k]) for k in self._states): # Initialize child outcomes self._child_outcomes[label] = None self._threads[label] = threading.Thread(name='concurrent_split:' + label, target=self._state_runner, args=(label, )) # Launch threads for thread in self._threads.values(): thread.start() # Wait for done notification self._done_cond.acquire() # Notify all threads ready to go self._ready_event.set() # Wait for a done notification from a thread self._done_cond.wait() self._done_cond.release() # Preempt any running states smach.logdebug("SMACH Concurrence preempting running states.") for label in self._states: if self._child_outcomes[label] == None: self._states[label].request_preempt() # Wait for all states to terminate while not smach.is_shutdown(): if all([not t.isAlive() for t in self._threads.values()]): break self._done_cond.acquire() self._done_cond.wait(0.1) self._done_cond.release() # Check for user code exception if self._user_code_exception: self._user_code_exception = False raise smach.InvalidStateError( "A concurrent state raised an exception during execution.") # Check for preempt if self.preempt_requested(): # initialized serviced flag children_preempts_serviced = True # Service this preempt if for (label, state) in ((k, self._states[k]) for k in self._states): if state.preempt_requested(): # Reset the flag children_preempts_serviced = False # Complain smach.logwarn( "State '%s' in concurrence did not service preempt." % label) # Recall the preempt if it hasn't been serviced state.recall_preempt() if children_preempts_serviced: smach.loginfo("Concurrence serviced preempt.") self.service_preempt() # Spew some debyg info smach.loginfo("Concurrent Outcomes: " + str(self._child_outcomes)) # Initialize the outcome outcome = self._default_outcome # Determine the outcome from the outcome map smach.logdebug( "SMACH Concurrence determining contained state outcomes.") for (container_outcome, outcomes) in ((k, self._outcome_map[k]) for k in self._outcome_map): if all([ self._child_outcomes[label] == outcomes[label] for label in outcomes ]): smach.logdebug( "Terminating concurrent split with mapped outcome.") outcome = container_outcome # Check outcome callback if self._outcome_cb: try: cb_outcome = self._outcome_cb(copy.copy(self._child_outcomes)) if cb_outcome: if cb_outcome == str(cb_outcome): outcome = cb_outcome else: smach.logerr( "Outcome callback returned a non-string '%s', using default outcome '%s'" % (str(cb_outcome), self._default_outcome)) else: smach.logwarn( "Outcome callback returned None, using outcome '%s'" % outcome) except: raise smach.InvalidUserCodeError( ("Could not execute outcome callback '%s': " % self._outcome_cb) + traceback.format_exc()) # Cleanup self._threads = {} self._child_outcomes = {} # Call termination callbacks self.call_termination_cbs(list(self._states.keys()), outcome) ## Do the <onexit> if (self._onExit is not None): try: outcome = self._onExit.execute(self.userdata, outcome) except Exception as ex: rospy.logerr('%s::onExit::execute() raised | %s' % (self.__class__.__name__, str(ex))) return "preempt" # Copy output keys self._copy_output_keys(self.userdata, parent_ud) return outcome
def execute(self, userdata): smach.loginfo('Executing state {}'.format(self._name)) smach.loginfo('Returning {}'.format(self._outcome)) return self._outcome
def main(): smach.loginfo("Starting main...") master = MasterClass() master.run()
def setup_state_machine(self): smach.loginfo("Initializing state machine") rospy.sleep(1) with self.state_machine: smach.StateMachine.add( 'START', Start(self.davinciArmLeft, self.davinciArmRight), transitions={'success': 'IDENTIFY_GRASP_POINT'}) smach.StateMachine.add('IDENTIFY_GRASP_POINT', IdentifyGraspPoint(self.davinciArmLeft, self.davinciArmRight), transitions={ 'success': 'PLAN_TRAJ_TO_GRASP_POINT_RIGHT', 'failure': 'IDENTIFY_GRASP_POINT', 'complete': 'SUCCESS' }, remapping={ 'graspPoint': 'sm_data1', 'counter': 'sm_data2', 'maxDebris': 'sm_data3' }) smach.StateMachine.add('PLAN_TRAJ_TO_GRASP_POINT_RIGHT', PlanTrajToGraspPointRight( self.davinciArmLeft, self.davinciArmRight), transitions={ 'success': 'MOVE_TO_PRE_GRASP_POINT', 'failure': 'PLAN_TRAJ_TO_GRASP_POINT_RIGHT' }) smach.StateMachine.add('MOVE_TO_PRE_GRASP_POINT', MoveToPreGraspPoint(self.davinciArmLeft, self.davinciArmRight), transitions={ 'success': 'MOVE_TO_GRASP_POINT', 'failure': 'HOME_POSITION_RIGHT' }, remapping={'graspPoint': 'sm_data1'}) smach.StateMachine.add('MOVE_TO_GRASP_POINT', MoveToGraspPoint(self.davinciArmLeft, self.davinciArmRight), transitions={ 'success': 'GRASP_GAK', 'failure': 'HOME_POSITION_RIGHT' }, remapping={'graspPoint': 'sm_data1'}) smach.StateMachine.add('HOME_POSITION_RIGHT', HomePositionRight(self.davinciArmLeft, self.davinciArmRight), transitions={ 'success': 'IDENTIFY_GRASP_POINT', 'failure': 'HOME_POSITION_RIGHT' }) smach.StateMachine.add('GRASP_GAK', GraspGak(self.davinciArmLeft, self.davinciArmRight), transitions={'success': 'RETRACT_GAK'}) smach.StateMachine.add('RETRACT_GAK', RetractGak(self.davinciArmLeft, self.davinciArmRight), transitions={'success': 'CHECK_GRASP'}) smach.StateMachine.add('CHECK_GRASP', CheckGrasp(self.davinciArmLeft, self.davinciArmRight), transitions={ 'success': 'IDENTIFY_CUT_POINT', 'failure': 'RELEASE_GRIPPERS_NO_GAK' }) smach.StateMachine.add('RELEASE_GRIPPERS_NO_GAK', ReleaseGrippersNoGak( self.davinciArmLeft, self.davinciArmRight), transitions={ 'success': 'HOME_POSITION_RIGHT', 'failure': 'RELEASE_GRIPPERS_NO_GAK' }) smach.StateMachine.add('IDENTIFY_CUT_POINT', IdentifyCutPoint(self.davinciArmLeft, self.davinciArmRight), transitions={ 'success': 'PLAN_TRAJ_TO_PRE_CUT_POINT_LEFT', 'failure': 'IDENTIFY_CUT_POINT' }, remapping={'cutPoint': 'sm_data4'}) smach.StateMachine.add('PLAN_TRAJ_TO_PRE_CUT_POINT_LEFT', PlanTrajToPreCutPointLeft( self.davinciArmLeft, self.davinciArmRight), transitions={ 'success': 'MOVE_TO_PRE_CUT_POINT', 'failure': 'PLAN_TRAJ_TO_PRE_CUT_POINT_LEFT' }) smach.StateMachine.add('MOVE_TO_PRE_CUT_POINT', MoveToPreCutPoint(self.davinciArmLeft, self.davinciArmRight), transitions={ 'success': 'CUTTING_ACTION', 'failure': 'HOME_POSITION_LEFT' }, remapping={'cutPoint': 'sm_data4'}) smach.StateMachine.add('CUTTING_ACTION', CuttingAction(self.davinciArmLeft, self.davinciArmRight), transitions={'success': 'CHECK_CUT'}, remapping={'cutPoint': 'sm_data4'}) smach.StateMachine.add('CHECK_CUT', CheckCut(self.davinciArmLeft, self.davinciArmRight), transitions={ 'success': 'CLEANING', 'failure': 'IDENTIFY_CUT_POINT' }) smach.StateMachine.add('CLEANING', Cleaning(self.davinciArmLeft, self.davinciArmRight), transitions={ 'success': 'SUCCESS', 'loop': 'IDENTIFY_GRASP_POINT' }, remapping={ 'counter': 'sm_data2', 'maxDebris': 'sm_data3' }) smach.StateMachine.add('HOME_POSITION_LEFT', HomePositionLeft(self.davinciArmLeft, self.davinciArmRight), transitions={ 'success': 'IDENTIFY_CUT_POINT', 'failure': 'ABORT' }) smach.StateMachine.add('ABORT', Abort(self.davinciArmLeft, self.davinciArmRight), transitions={'failure': 'FAILURE'})
def _update_once(self): """Method that updates the state machine once. This checks if the current state is ready to transition, if so, it requests the outcome of the current state, and then extracts the next state label from the current state's transition dictionary, and then transitions to the next state. """ outcome = None transition_target = None last_state_label = self._current_label # Make sure the state exists if self._current_label not in self._states: raise smach.InvalidStateError( "State '%s' does not exist. Available states are: %s" % (self._current_label, list(self._states.keys()))) # Check if a preempt was requested before or while the last state was running if self.preempt_requested(): smach.loginfo( "Preempt requested on state machine before executing the next state." ) # We were preempted if self._preempted_state is not None: # We were preempted while the last state was running if self._preempted_state.preempt_requested(): smach.loginfo( "Last state '%s' did not service preempt. Preempting next state '%s' before executing..." % (self._preempted_label, self._current_label)) # The flag was not reset, so we need to keep preempting # (this will reset the current preempt) self._preempt_current_state() else: # The flag was reset, so the container can reset self._preempt_requested = False self._preempted_state = None else: # We were preempted after the last state was running # So we sho if(self._preempt_requested):uld preempt this state before we execute it self._preempt_current_state() # Execute the state self._tree_view_enable_state(self._current_label) try: self._state_transitioning_lock.release() outcome = self._current_state.execute( smach.Remapper( self.userdata, self._current_state.get_registered_input_keys(), self._current_state.get_registered_output_keys(), self._remappings[self._current_label])) except smach.InvalidUserCodeError as ex: smach.logerr("State '%s' failed to execute." % self._current_label) raise ex except: raise smach.InvalidUserCodeError( "Could not execute state '%s' of type '%s': " % (self._current_label, self._current_state) + traceback.format_exc()) finally: self._state_transitioning_lock.acquire() self._tree_view_disable_state(self._current_label) # Check if outcome was a potential outcome for this type of state if outcome not in self._current_state.get_registered_outcomes(): raise smach.InvalidTransitionError( "Attempted to return outcome '%s' preempt_requestedfrom state '%s' of" " type '%s' which only has registered outcomes: %s" % (outcome, self._current_label, self._current_state, self._current_state.get_registered_outcomes())) # Check if this outcome is actually mapped to any target if outcome not in self._current_transitions: raise smach.InvalidTransitionError( "Outcome '%s' of state '%s' is not bound to any transition target. Bound transitions include: %s" % (str(outcome), str( self._current_label), str(self._current_transitions))) # Set the transition target transition_target = self._current_transitions[outcome] # Check if the transition target is a state in this state machine, or an outcome of this state machine if transition_target in self._states: # Set the new state self._set_current_state(transition_target) # Spew some info smach.loginfo("State machine transitioning '%s':'%s'-->'%s'" % (last_state_label, outcome, transition_target)) # Call transition callbacks self.call_transition_cbs() else: # This is a terminal state if self._preempt_requested and self._preempted_state is not None: if not self._current_state.preempt_requested(): self.service_preempt() if transition_target not in self.get_registered_outcomes(): # This is a container outcome that will fall through transition_target = outcome if transition_target in self.get_registered_outcomes(): # The transition target is an outcome of the state machine self._set_current_state(None) # Spew some info smach.loginfo("State machine terminating '%s':'%s':'%s'" % (last_state_label, outcome, transition_target)) # Call termination callbacks self.call_termination_cbs([last_state_label], transition_target) return transition_target else: raise smach.InvalidTransitionError( "Outcome '%s' of state '%s' with transition target '%s' is neither a registered state nor a registered container outcome." % (outcome, self._current_label, transition_target)) return None