Preempting a concurrence container might not have 'preempted' as outcome

[MCFurry]

Might be related to https://github.com/ros/executive_smach/issues/53

But we discovered this in a bigger state-machine where a concurrent monitoring state is trying to preempt another concurrence container. We expected this container then to finalize with outcome 'preempted'. However, if at that moment the concurrency container already was terminating and one of the states had an outcome defined already, the resulting outcome is not 'preempted' which made our state machine end up in an undesired state.

Inspired by https://github.com/ros/executive_smach/issues/53 I created a small script as well to demonstrate this:

#!/usr/bin/env python

from time import sleep
from smach import State, Concurrence
from threading import Timer

class DelayState(State):
    """Delay state for testing purposes"""

    def __init__(self, delay):
        State.__init__(self, outcomes=['succeeded', 'preempted'])
        self.delay = delay

    def execute(self, _):
        # A better delay state should be able to preempt during its sleep state
        sleep(self.delay)
        if self.preempt_requested():
            self.service_preempt()
            return 'preempted'
        return 'succeeded'

def test_concurrence_preempt():

    cc1 = Concurrence(
        outcomes=['succeeded', 'preempted'],
        default_outcome='succeeded',
        child_termination_cb=lambda *_: True,  # Always evaluate outcome_map
        outcome_map={'preempted': {'State1': 'preempted',
                                   'State2': 'preempted'}})
    with cc1:
        Concurrence.add('State1', DelayState(0.1))
        Concurrence.add('State2', DelayState(1.0))

    t = Timer(0.3, cc1.request_preempt)
    t.start()
    print(f' Resulting state: {cc1.execute()}')

if __name__ == '__main__':
    test_concurrence_preempt()

In this simple example State1 is already done when the concurrence container is asked to preempt and thus the resulting outcome is succeeded although preempted would be expected.

In code the case where the concurrence is asked to preempt after all states are terminated is caught already: https://github.com/ros/executive_smach/blob/d4226ba66363f4da92e7ff1052c383bb3a5adc77/smach/src/smach/concurrence.py#L259 but the resulting outcome is not changed.

I'll sketch-up a PR as well with a potential fix!