Concurrence race conditions on request_preempt

[krixkrix]

I believe there are race condition problems with smach containers in general, here demonstrated for Concurrence state.

Scenario:

1. A preempt request is called on the concurrence state right when the children have completed, but the concurrence state itself is still executing.
2. The concurrence state will then (sometimes) claim that the preempt was serviced, though none of its children serviced the preempt. This means that the preempt request is sort of lost.

Below a script that demonstrates the problem, it does not require any ROS code running. The script just creates this state machine.

• Sequence(top)
  • Concurrence(Cc)
    • Delay1(0.3 seconds) (for demonstration purposes: the only child of Concurrence)
  • Delay2(0.5 seconds)

When the script is preempted after approx. 0.3 seconds, I would expect either Delay1 or Delay2 to return 'preempted'.

Test script:

#!/usr/bin/env python

from time import sleep
from smach import State, Concurrence, Sequence
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, userdata):
        # 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():
    """test demonstrating race condition

    Creates a state machine:

    - Sequence(top)
      - Concurrence(Cc)
        - Delay1(0.3 seconds) (for demonstration purposes: the only child of Concurrence)
      - Delay2(0.5 seconds)

    When preempting the state machine after ~0.3 seconds, the machine
    is expected to return 'preempted'
    """

    def outcome_cb(outcome_map):
        if 'preempted' in outcome_map.values():
            return 'preempted'
        return 'succeeded'

    cc = Concurrence(
        outcomes=['succeeded', 'preempted'],
        default_outcome='succeeded',
        outcome_cb=outcome_cb)
    with cc:
        Concurrence.add('Delay1', DelayState(delay=0.3))

    top = Sequence(outcomes=['succeeded', 'preempted'],
                   connector_outcome='succeeded')

    with top:
        Sequence.add('Cc', cc)
        Sequence.add('Delay2', DelayState(delay=0.5))

    # Execute state machine and try cancel after various milliseconds of delay
    for msDelay in range(290, 330, 2):
        print ('Cancel after delay {}'.format(msDelay))
        t = Timer(msDelay/1000.0, top.request_preempt)
        t.start()
        if top.execute() != 'preempted':
            print ('===== TEST FAILED, delay: %sms =====', msDelay)
            return

    print ('===== TEST OK =====')


test_concurrence_preempt()

The above scenario is one of several cases, where a preempt request is kind of lost. I believe similar problems exist for Sequence container as well.

I will happily work on a pull request, but since this is a quite generic problem for Smach as I see it, it might require some discussion on the right approach.

My setup:

• Ubuntu 16.04,
• ROS kinetic
• ros-kinetic-smach 2.0.1-0xenial-20170608-133042-0800