def __setattr__(self, name, value):
     if name[0] == '_' or not self.__dict__.has_key('_Remapper__initialized'):
         return object.__setattr__(self, name, value)
     if name not in self._output:
         smach.logerr("Writing to SMACH userdata key '%s' but the only keys that were declared as output from this state were: %s." % (name, self._output))
         return None
     setattr(self._ud, self._remap(name), value)

 def __getitem__(self, key):
     if key not in self._states:
         smach.logerr(
             "Attempting to get state '%s' from StateMachine container. The only available states are: %s"
             % (key, self._states.keys())
         )
         raise KeyError()
     return self._states[key]

 def __setitem__(self, key, item):
     if key not in self._output:
         smach.logerr(
             "Writing to SMACH userdata key '%s' but the only keys that were declared as output from this state were: %s."
             % (key, self._output)
         )
         return
     self._ud.__setitem__(self._remap(key), item)

    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 extract(self, keys, remapping):
     ud = UserData()
     reverse_remapping = dict([(v,k) for (k,v) in remapping.iteritems()])
     if len(reverse_remapping) != len(remapping):
         smach.logerr("SMACH userdata remapping is not one-to-one: " + str(remapping))
     for k in keys:
         rmk = k
         if k in reverse_remapping:
             rmk = reverse_remapping[k]
         ud[rmk] = copy.copy(self[k])
     return ud

 def call_transition_cbs(self):
     """Calls the registered transition callbacks.
     Callback functions are called with two arguments in addition to any
     user-supplied arguments:
      - userdata
      - a list of active states
      """
     try:
         for (cb,args) in self._transition_cbs:
             cb(self.userdata, self.get_active_states(), *args)
     except:
         smach.logerr("Could not execute transition callback: "+traceback.format_exc())

    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 call_termination_cbs(self, terminal_states, outcome):
     """Calls the registered termination callbacks.
     Callback functions are called with three arguments in addition to any
     user-supplied arguments:
      - userdata
      - a list of terminal states
      - the outcome of this container
     """
     try:
         for (cb,args) in self._termination_cbs:
             cb(self.userdata, terminal_states, outcome, *args)
     except:
         smach.logerr("Could not execute termination callback: "+traceback.format_exc())

    def __getattr__(self, name):
        """Overload getattr to be thread safe."""
        if name[0] == '_':
            return object.__getattr__(self, name)
        if not name in self._locks.keys():
            self._locks[name] = threading.Lock()

        try:
            with self._locks[name]:
                temp = self._data[name]
        except:
            smach.logerr("Userdata key '%s' not available. Available keys are: %s" % (name, self._data.keys()))
            raise KeyError()

        return temp

    def close(self):
        """Close the container."""
        # Make sure this container is the currently open container
        if len(Container._construction_stack) > 0:
            if self != Container._construction_stack[-1]:
                raise smach.InvalidStateError('Attempting to close a container that is not currently open.')

        # Pop this container off the construction stack
        Container._construction_stack.pop()
        Container._construction_lock.release()

        # Check consistency of container, post-construction
        try:
            self.check_consistency()
        except (smach.InvalidStateError, smach.InvalidTransitionError):
            smach.logerr("Container consistency check failed.")

    def _preempt_current_state(self):
        """Preempt the current state (might not be executing yet).
        This also resets the preempt flag on a state that had previously received the preempt, but not serviced it."""
        if self._preempted_state != self._current_state:
            if self._preempted_state is not None:
                # Reset the previously preempted state (that has now terminated)
                self._preempted_state.recall_preempt()

            # Store the label of the currently active state
            self._preempted_state = self._current_state
            self._preempted_label = self._current_label

            # Request the currently active state to preempt
            try:
                self._preempted_state.request_preempt()
            except:
                smach.logerr("Failed to preempt contained state '%s': %s" % (self._preempted_label, traceback.format_exc()))

    def add(label, state, transitions = None, remapping = None):
        """Add a state to the sequence.
        Each state added will receive an additional transition from it to the
        state which is added after it. The transition will follow the outcome
        specified at construction of this container.
        
        @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. If one of these transitions follows the connector outcome
        specified in the constructor, the provided transition will override
        the automatically generated connector transition.
        """
        # Get currently opened container
        self = Sequence._currently_opened_container()

        if transitions is None:
            transitions = {}

        # Perform sequence linking
        if self._last_added_seq_label is not None:
            #print self._transitions[self._last_added_seq_label]

            last_label = self._last_added_seq_label
            # Check if the connector outcome has been overriden
            if self._connector_outcome not in self._transitions[last_label]\
                    or self._transitions[last_label][self._connector_outcome] is None:
                self._transitions[last_label][self._connector_outcome] = label
            try:
                self.check_state_spec(last_label, self._states[last_label], self._transitions[last_label])
            except:
                smach.logerr("Attempting to construct smach state sequence failed.")

            #print self._transitions[self._last_added_seq_label]

        # Store the last added state label
        self._last_added_seq_label = label

        return smach.StateMachine.add(label, state, transitions, remapping)

 def _execute_state(self, state, force_exit=False):
     result = None
     try:
         ud = smach.Remapper(
                     self.userdata,
                     state.get_registered_input_keys(),
                     state.get_registered_output_keys(),
                     self._remappings[state.name])
         if force_exit:
             state.on_exit(ud)
         else:
             result = state.execute(ud)
         #print 'execute %s --> %s' % (state.name, self._returned_outcomes[state.name])
     except smach.InvalidUserCodeError as ex:
         smach.logerr("State '%s' failed to execute." % state.name)
         raise ex
     except:
         raise smach.InvalidUserCodeError("Could not execute state '%s' of type '%s': " %
                                          (state.name, state)
                                          + traceback.format_exc())
     return result

    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 __exit__(self, exc_type, exc_val, exc_tb):
     if exc_type is None:
         return self.close()
     else:
         if exc_type != smach.InvalidStateError and exc_type != smach.InvalidTransitionError:
             smach.logerr("Error raised during SMACH container construction: \n" + "\n".join(traceback.format_exception(exc_type, exc_val, exc_tb)))

 def __delattr__(self, name):
     smach.logerr("Attempting to delete '%s' but this member is read-only." % name)
     raise TypeError()

    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
#.........这里部分代码省略.........

 def __init__(self, message):
     smach.logerr(self.__class__.__name__ + ": " + message)
     Exception.__init__(self, message)

    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(
#.........这里部分代码省略.........

 def __init__(self,message):
     smach.logerr("InvalidUserCodeError: "+message)
     self.message = message