Go to the documentation of this file.00001
00002 import rospy
00003
00004 from flexbe_core.core.manually_transitionable_state import ManuallyTransitionableState
00005
00006 from flexbe_core.proxy import ProxyPublisher, ProxySubscriberCached
00007 from flexbe_msgs.msg import CommandFeedback
00008 from std_msgs.msg import String
00009
00010
00011 class LockableState(ManuallyTransitionableState):
00012 """
00013 A state that can be locked.
00014 When locked, no transition can be done regardless of the resulting outcome.
00015 However, if any outcome would be triggered, the outcome will be stored
00016 and the state won't be executed anymore until it is unlocked and the stored outcome is set.
00017 """
00018
00019 def __init__(self, *args, **kwargs):
00020 super(LockableState, self).__init__(*args, **kwargs)
00021
00022 self._locked = False
00023 self._stored_outcome = None
00024
00025 self.__execute = self.execute
00026 self.execute = self._lockable_execute
00027
00028 self._feedback_topic = 'flexbe/command_feedback'
00029 self._lock_topic = 'flexbe/command/lock'
00030 self._unlock_topic = 'flexbe/command/unlock'
00031
00032 self._pub = ProxyPublisher()
00033 self._sub = ProxySubscriberCached()
00034
00035
00036 def _lockable_execute(self, *args, **kwargs):
00037 if self._is_controlled and self._sub.has_msg(self._lock_topic):
00038 msg = self._sub.get_last_msg(self._lock_topic)
00039 self._sub.remove_last_msg(self._lock_topic)
00040 self._execute_lock(msg.data)
00041
00042 if self._is_controlled and self._sub.has_msg(self._unlock_topic):
00043 msg = self._sub.get_last_msg(self._unlock_topic)
00044 self._sub.remove_last_msg(self._unlock_topic)
00045 self._execute_unlock(msg.data)
00046
00047 if self._locked:
00048 if self._stored_outcome is None or self._stored_outcome == 'None':
00049 self._stored_outcome = self.__execute(*args, **kwargs)
00050 return None
00051
00052 if not self._locked and not self._stored_outcome is None and not self._stored_outcome == 'None':
00053 if self._parent.transition_allowed(self.name, self._stored_outcome):
00054 outcome = self._stored_outcome
00055 self._stored_outcome = None
00056 return outcome
00057 else:
00058 return None
00059
00060 outcome = self.__execute(*args, **kwargs)
00061
00062 if outcome is None or outcome == 'None':
00063 return None
00064
00065 if not self._parent is None and not self._parent.transition_allowed(self.name, outcome):
00066 self._stored_outcome = outcome
00067 return None
00068
00069 return outcome
00070
00071
00072 def _execute_lock(self, target):
00073 if target == self._get_path() or target == '':
00074 target = self._get_path()
00075 found_target = True
00076 self._locked = True
00077 else:
00078 found_target = self._parent.lock(target)
00079
00080 self._pub.publish(self._feedback_topic, CommandFeedback(command="lock", args=[target, target if found_target else ""]))
00081 if not found_target:
00082 rospy.logwarn("--> Wanted to lock %s, but could not find it in current path %s.", target, self._get_path())
00083 else:
00084 rospy.loginfo("--> Locking in state %s", target)
00085
00086
00087 def _execute_unlock(self, target):
00088 if target == self._get_path() or (self._locked and target == ''):
00089 target = self._get_path()
00090 found_target = True
00091 self._locked = False
00092 else:
00093 found_target = self._parent.unlock(target)
00094
00095 self._pub.publish(self._feedback_topic, CommandFeedback(command="unlock", args=[target, target if found_target else ""]))
00096 if not found_target:
00097 rospy.logwarn("--> Wanted to unlock %s, but could not find it in current path %s.", target, self._get_path())
00098 else:
00099 rospy.loginfo("--> Unlocking in state %s", target)
00100
00101
00102 def _enable_ros_control(self):
00103 super(LockableState, self)._enable_ros_control()
00104 self._pub.createPublisher(self._feedback_topic, CommandFeedback)
00105 self._sub.subscribe(self._lock_topic, String)
00106 self._sub.subscribe(self._unlock_topic, String)
00107
00108 def _disable_ros_control(self):
00109 super(LockableState, self)._disable_ros_control()
00110 self._sub.unsubscribe_topic(self._lock_topic)
00111 self._sub.unsubscribe_topic(self._unlock_topic)
00112
00113
00114 def is_locked(self):
00115 return self._locked