preemptable_state_machine.py

Go to the documentation of this file.

#!/usr/bin/env python
from flexbe_core.core.preemptable_state import PreemptableState
from flexbe_core.proxy import ProxySubscriberCached
 
from std_msgs.msg import Empty
 
from flexbe_core.core.lockable_state_machine import LockableStateMachine
 
 
class PreemptableStateMachine(LockableStateMachine):
    """
    A state machine that can be preempted.
    If preempted, the state machine will return the outcome preempted.
    """
 
    _preempted_name = 'preempted'
 
    def __init__(self, *args, **kwargs):
        super(PreemptableStateMachine, self).__init__(*args, **kwargs)
        # always listen to preempt so that the behavior can be stopped even if unsupervised
        self._preempt_topic = 'flexbe/command/preempt'
        self._sub = ProxySubscriberCached({self._preempt_topic: Empty})
        self._sub.set_callback(self._preempt_topic, self._preempt_cb)
 
    def _preempt_cb(self, msg):
        if not self._is_controlled:
            PreemptableState.preempt = True
 
    @staticmethod
    def add(label, state, transitions=None, remapping=None):
        transitions[PreemptableState._preempted_name] = PreemptableStateMachine._preempted_name
        LockableStateMachine.add(label, state, transitions, remapping)
 
    @property
    def _valid_targets(self):
        return super(PreemptableStateMachine, self)._valid_targets + [PreemptableStateMachine._preempted_name]