Can't preempt state machine more than once

[msadowski]

Hi!

I just came across the following issue. In my state machine I have a bunch of SimpleActionStates that control the behaviour of the robot.

When one of the states is preempted the robot needs to perform a correcting action, that ideally could also be preempted by the user. For simplicity here is the state transition:

A ---preempt---->B---preempt----->C 
                 |
                 |---success---->D

where:
A - SimpleActionState
B - SimpleActionState
C - State
D - State

In my tests I've found out that when I execute the preempt in the state machine, I can only execute it once. Once we are in B, we cannot execute the preempt anymore (I can confirm that the action servers */cancel topics receive the cancel signal only once for the first preempt).

Debugging the code I believe the issue comes from the _preempt_current_state function:

    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()))

The first time the above is executed the _preempted_state is None and thus the self._preempted_state != self._current_state check goes through.

However, the second time it's executed both self._preempted_state and the self._current_state point to the same object, that is different from the _current_state we've seen in the previous preempt.

I assume the issue comes from the assignment:

            self._preempted_state = self._current_state

Where self._preempted_state starts being a reference to the self._current_state.

Anyone has any thoughts about it? This point could be highly relevant to the discussion in #52 .

[jack-frampton-dx]

@msadowski This issue works similar to states within a concurrent state machine. Albeit slightly differently, and more alike to the issue here that you posted: #52.

I have not looked too deeply into the source code, but I have found a potential fix / workaround in local code.

Issue: State preempts early if previously preempted.

You cannot resolve this issue by simply resetting the _preempt_requested flag to False (either explicitly, through recall_preempt() or service_preempt()) within the execute() function. It prevents the state from being preempted again.

Resetting the _preempt_requested flag to False can be done "cleanly" (allowing the state to be preempted again) if the preempt flag is reset outside the container requiring the reset.

My workaround was to recall_preempt() on the appropriate states within the custom concurrent state machine class before returning its own outcome (via concurrence_outcome_cb).

I hope this helps someone who also comes across this "early preempt" issue.

Common cmd line output: [smach_ros]: Preempt requested on state machine before executing the next state. (so someone can find this)

EDIT: Another workaround is self.preempt_recall() specifically just before you return an outcome in the state itself. Rather than at the start / anywhere else in the execute() function. Weird. Example:

if self._preempt_requested:
                self.recall_preempt()
                return outcomes.PREEMPTED.value

(Latter method works in deployment but during some unit tests it causes the concurrent state machine to raise an unspecified exception, former passes both)