lockable_state.py

Go to the documentation of this file.

#!/usr/bin/env python
from flexbe_core.logger import Logger
 
from flexbe_msgs.msg import CommandFeedback
from std_msgs.msg import String
 
from flexbe_core.core.manually_transitionable_state import ManuallyTransitionableState
 
 
class LockableState(ManuallyTransitionableState):
    """
    A state that can be locked.
    When locked, no transition can be done regardless of the resulting outcome.
    However, if any outcome would be triggered, the outcome will be stored
    and the state won't be executed anymore until it is unlocked and the stored outcome is set.
    """
 
    def __init__(self, *args, **kwargs):
        super(LockableState, self).__init__(*args, **kwargs)
        self.__execute = self.execute
        self.execute = self._lockable_execute
 
        self._locked = False
        self._stored_outcome = None
 
        self._feedback_topic = 'flexbe/command_feedback'
        self._lock_topic = 'flexbe/command/lock'
        self._unlock_topic = 'flexbe/command/unlock'
 
    def _lockable_execute(self, *args, **kwargs):
        if self._is_controlled and self._sub.has_msg(self._lock_topic):
            msg = self._sub.get_last_msg(self._lock_topic)
            self._sub.remove_last_msg(self._lock_topic)
            self._execute_lock(msg.data)
 
        if self._is_controlled and self._sub.has_msg(self._unlock_topic):
            msg = self._sub.get_last_msg(self._unlock_topic)
            self._sub.remove_last_msg(self._unlock_topic)
            self._execute_unlock(msg.data)
 
        # locked, so execute until we want to transition
        if self._locked:
            if self._stored_outcome is None or self._stored_outcome == 'None':
                self._stored_outcome = self.__execute(*args, **kwargs)
            return None
 
        # not locked, but there still is a transition we want to trigger
        if not self._locked and self._stored_outcome is not None and not self._stored_outcome == 'None':
            if self.parent.transition_allowed(self.name, self._stored_outcome):
                outcome = self._stored_outcome
                self._stored_outcome = None
                return outcome
            else:
                return None
 
        outcome = self.__execute(*args, **kwargs)
 
        if outcome is None or outcome == 'None':
            return None
 
        # not locked, but still, a parent could be locked so we need to ensure that we do not cause an outcome there
        if self.parent is not None and not self.parent.transition_allowed(self.name, outcome):
            self._stored_outcome = outcome
            return None
 
        return outcome
 
    def _execute_lock(self, target):
        if target == self.path or target == '':
            target = self.path
            found_target = True
            self._locked = True
        else:
            found_target = self._parent.lock(target)
        # provide feedback about lock
        self._pub.publish(self._feedback_topic, CommandFeedback(command="lock",
                                                                args=[target, target if found_target else ""]))
        if not found_target:
            Logger.logwarn("Wanted to lock %s, but could not find it in current path %s." % (target, self.path))
        else:
            Logger.localinfo("--> Locking in state %s" % target)
 
    def _execute_unlock(self, target):
        if target == self.path or (self._locked and target == ''):
            target = self.path
            found_target = True
            self._locked = False
        else:
            found_target = self._parent.unlock(target)
        # provide feedback about unlock
        self._pub.publish(self._feedback_topic, CommandFeedback(command="unlock",
                                                                args=[target, target if found_target else ""]))
        if not found_target:
            Logger.logwarn("Wanted to unlock %s, but could not find it in current path %s." % (target, self.path))
        else:
            Logger.localinfo("--> Unlocking in state %s" % target)
 
    def _enable_ros_control(self):
        super(LockableState, self)._enable_ros_control()
        self._pub.createPublisher(self._feedback_topic, CommandFeedback)
        self._sub.subscribe(self._lock_topic, String)
        self._sub.subscribe(self._unlock_topic, String)
 
    def _disable_ros_control(self):
        super(LockableState, self)._disable_ros_control()
        self._sub.unsubscribe_topic(self._lock_topic)
        self._sub.unsubscribe_topic(self._unlock_topic)
 
    def is_locked(self):
        return self._locked