Esempio n. 1
0
    def set_contained_state(label,
                            state,
                            loop_outcomes=[],
                            break_outcomes=[],
                            final_outcome_map={}):
        """Set the contained state
        
        @type label: string
        @param label: The label of the state being added.
        
        @type state: L{smach.State}
        @param state: An instance of a class implementing the L{smach.State} interface.

        @param loop_outcomes: List of contained state outcomes that should cause
        the iterator to continue. If this is empty, all outcomes that are not
        in the break_outcomes list will cause the iterator to continue to
        iterate. NOTE: loop_outcomes will be overriden by break_outcomes if both
        parameters are used.

        @param break_outcomes: List of contained state outcomes that should
        cause the iterator to break. When the contained state emits an outcome
        in this list, the container will terminate and return either that
        outcome or the outcome it is mapped to in final_outcome_map. NOTE:
        loop_outcomes will be overriden by break_outcomes if both
        parameters are used.

        @param final_outcome_map: A map from contained state outcomes to container
        outcomes. On termination of the iterator (either from finishing or from
        a break) this map will be used to translate contained state outcomes to
        container outcomes.
        Unspecified contained state outcomes will fall through as
        container outcomes.
        """
        # Get currently opened container
        self = Iterator._currently_opened_container()

        self._state_label = label
        self._state = state

        # Get potential state outcomes
        state_outcomes = state.get_registered_outcomes()

        # Check for loop and break outcomes
        if loop_outcomes and break_outcomes:
            smach.logwarn(
                'Both loop_outcomes and break_outcomes were specified when constructing SMACH iterator container.'
            )

        if break_outcomes:
            self._break_outcomes = break_outcomes
            for outcome in state_outcomes:
                if outcome not in break_outcomes:
                    self._loop_outcomes.append(outcome)
        else:
            self._loop_outcomes = loop_outcomes
            for outcome in state_outcomes:
                if outcome not in loop_outcomes:
                    self._break_outcomes.append(outcome)

        self._final_outcome_map = final_outcome_map
Esempio n. 2
0
    def set_contained_state(
            label,
            state,
            loop_outcomes = [],
            break_outcomes = [],
            final_outcome_map = {}):
        """Set the contained state
        
        @type label: string
        @param label: The label of the state being added.
        
        @type state: L{smach.State}
        @param state: An instance of a class implementing the L{smach.State} interface.

        @param loop_outcomes: List of contained state outcomes that should cause
        the iterator to continue. If this is empty, all outcomes that are not
        in the break_outcomes list will cause the iterator to continue to
        iterate. NOTE: loop_outcomes will be overriden by break_outcomes if both
        parameters are used.

        @param break_outcomes: List of contained state outcomes that should
        cause the iterator to break. When the contained state emits an outcome
        in this list, the container will terminate and return either that
        outcome or the outcome it is mapped to in final_outcome_map. NOTE:
        loop_outcomes will be overriden by break_outcomes if both
        parameters are used.

        @param final_outcome_map: A map from contained state outcomes to container
        outcomes. On termination of the iterator (either from finishing or from
        a break) this map will be used to translate contained state outcomes to
        container outcomes.
        Unspecified contained state outcomes will fall through as
        container outcomes.
        """
        # Get currently opened container
        self = Iterator._currently_opened_container()

        self._state_label = label
        self._state = state

        # Get potential state outcomes
        state_outcomes = state.get_registered_outcomes()

        # Check for loop and break outcomes
        if loop_outcomes and break_outcomes:
            smach.logwarn('Both loop_outcomes and break_outcomes were specified when constructing SMACH iterator container.')

        if break_outcomes:
            self._break_outcomes = break_outcomes
            for outcome in state_outcomes:
                if outcome not in break_outcomes:
                    self._loop_outcomes.append(outcome)
        else:
            self._loop_outcomes = loop_outcomes
            for outcome in state_outcomes:
                if outcome not in loop_outcomes:
                    self._break_outcomes.append(outcome)

        self._final_outcome_map = final_outcome_map
Esempio n. 3
0
 def _copy_output_keys(self, ud, parent_ud):
     if parent_ud is not None:
         output_keys = self.get_registered_output_keys()
         for ok in output_keys:
             try:
                 parent_ud[ok] = ud[ok]
             except KeyError:
                 smach.logwarn("Attempting to copy output key '%s', but this key does not exist." % ok)
Esempio n. 4
0
 def _copy_input_keys(self, parent_ud, ud):
     if parent_ud is not None:
         input_keys = self.get_registered_input_keys()
         for ik in input_keys:
             try:
                 ud[ik] = parent_ud[ik]
             except KeyError:
                 smach.logwarn("Attempting to copy input key '%s', but this key does not exist." % ik)
Esempio n. 5
0
    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
Esempio n. 6
0
    def set_initial_state(self, initial_states, userdata):
        if initial_states > 0:
            if initial_states < len(self._states):
                smach.logwarn("Attempting to set initial states in Concurrence"
                              " container, but Concurrence children are always"
                              " all executed initially, ignoring call.")

        # Set local userdata
        self.userdata.update(userdata)
Esempio n. 7
0
    def set_initial_state(self, initial_states, userdata):
        if initial_states > 0:
            if initial_states < len(self._states):
                smach.logwarn("Attempting to set initial states in Concurrence"
                              " container, but Concurrence children are always"
                              " all executed initially, ignoring call.")

        # Set local userdata
        self.userdata.update(userdata)
Esempio n. 8
0
 def _copy_input_keys(self, parent_ud, ud):
     if parent_ud is not None:
         input_keys = self.get_registered_input_keys()
         for ik in input_keys:
             try:
                 ud[ik] = parent_ud[ik]
             except KeyError:
                 smach.logwarn(
                     "Attempting to copy input key '%s', but this key does not exist."
                     % ik)
Esempio n. 9
0
 def _copy_output_keys(self, ud, parent_ud):
     if parent_ud is not None:
         output_keys = self.get_registered_output_keys()
         for ok in output_keys:
             try:
                 parent_ud[ok] = ud[ok]
             except KeyError:
                 smach.logwarn(
                     "Attempting to copy output key '%s', but this key does not exist."
                     % ok)
    def set_initial_state(self, initial_states, userdata=smach.UserData()):
        smach.logdebug("Setting initial state to "+str(initial_states))

        if len(initial_states) > 1:
            smach.logwarn("Attempting to set initial state to include more than one state, but the StateMachine container can only have one initial state.")

        # Set the initial state label
        if len(initial_states) > 0:
            self._initial_state_label = initial_states[0]
        # Set local userdata
        self.userdata.update(userdata)
Esempio n. 11
0
    def set_initial_state(self, initial_states, userdata=smach.UserData()):
        smach.logdebug("Setting initial state to "+str(initial_states))

        if len(initial_states) > 1:
            smach.logwarn("Attempting to set initial state to include more than one state, but the StateMachine container can only have one initial state.")

        # Set the initial state label
        if len(initial_states) > 0:
            self._initial_state_label = initial_states[0]
        # Set local userdata
        self.userdata.update(userdata)
Esempio n. 12
0
    def set_initial_state(self, initial_states, userdata):
        # Check initial state
        if len(initial_states) > 1:
            smach.logwarn("Attempting to set initial state to include more than one state, but Iterator container can only have one initial state." % (self._state_label))

        if len(initial_states) > 0:
            if initial_states[0] != self._state_label:
                smach.logwarn("Attempting to set state '%s' as initial state in Iterator container. The only available state is '%s'." % (initial_states[0], self._state_label))
                raise KeyError()

        # Set local userdata
        self.userdata.update(userdata)
Esempio n. 13
0
 def request_preempt(self):
     """Propagate preempt to currently active state.
     
     This will attempt to preempt the currently active state.
     """
     with self._state_transitioning_lock:
         smach.logwarn("Request preemption on state machine")
         # Allways Set this container's preempted flag
         self._preempt_requested = True
         # Only propagate preempt if the current state is defined
         if self._current_state is not None:
             self._preempt_current_state()
Esempio n. 14
0
    def set_initial_state(self, initial_states, userdata=None):
        """Set initial active states of a container.
        
        @type initial_states: list of string
        @param initial_states: A description of the initial active state of this
        container.
        
        @type userdata: L{UserData}
        @param userdata: Initial userdata for this container.
        """
        with self._lock:
            if userdata is None:
                userdata = smach.UserData()

            smach.logdebug("Setting initial states to " + str(initial_states))

            if len(initial_states) > 1:
                smach.logwarn(
                    "Attempting to set initial state to include more than"
                    " one state, but the BehaviorTreeContainer container can only"
                    " have one initial state. Taking the first one."
                )

            # Set the initial state label
            if len(initial_states) > 0:
                initial_state_label = initial_states[0]
                # TODO We only support the root as initial state for now
                # for proper support we'll want to walk the tree and set initial idx
                # for every container on the path to the task with the specified
                # label.

                self._rebuild_unique_task_names()
                try:
                    self._initial_task = self._task_for_label[initial_state_label]
                except KeyError:
                    smach.logwarn("Unknown initial state '%s' requested", initial_state_label)
            # Set local userdata
            self.userdata.update(userdata)
Esempio n. 15
0
    def set_initial_state(self, initial_states, userdata=None):
        """Set initial active states of a container.
        
        @type initial_states: list of string
        @param initial_states: A description of the initial active state of this
        container.
        
        @type userdata: L{UserData}
        @param userdata: Initial userdata for this container.
        """
        with self._lock:
            if userdata is None:
                userdata = smach.UserData()

            smach.logdebug("Setting initial states to " + str(initial_states))

            if len(initial_states) > 1:
                smach.logwarn("Attempting to set initial state to include more than"
                              " one state, but the BehaviorTreeContainer container can only"
                              " have one initial state. Taking the first one.")

            # Set the initial state label
            if len(initial_states) > 0:
                initial_state_label = initial_states[0]
                # TODO We only support the root as initial state for now
                # for proper support we'll want to walk the tree and set initial idx
                # for every container on the path to the task with the specified
                # label.
                
                self._rebuild_unique_task_names()
                try:
                    self._initial_task = self._task_for_label[initial_state_label]
                except KeyError:
                    smach.logwarn("Unknown initial state '%s' requested", initial_state_label)
            # Set local userdata
            self.userdata.update(userdata)
Esempio n. 16
0
    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
Esempio n. 17
0
    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
Esempio n. 18
0
    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.logwarn("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.logwarn("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
                    smach.logwarn("Preemption flag on preempted state '%s' was reset. Container can reset and continue..." % (self._preempted_label))
                    smach.logwarn("This could be errornous because state machine should preempt and we still continue. Most likely we will just preempt execution")
                    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
                smach.logwarn("State Machine preempted after the last state was running. Preempting this state '%s' before executing..." %self._current_label)
                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]))
            self._current_outcome = outcome
        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 not self._shutdown_requested 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))

            # 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() or self._shutdown_requested:
                # 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