preemptable_state.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import rospy
00003 
00004 from flexbe_core.core.loopback_state import LoopbackState
00005 
00006 from flexbe_core.proxy import ProxyPublisher, ProxySubscriberCached
00007 from flexbe_msgs.msg import CommandFeedback
00008 from std_msgs.msg import Empty
00009 
00010 
00011 class PreemptableState(LoopbackState):
00012     """
00013     A state that can be preempted.
00014     If preempted, the state will not be executed anymore and return the outcome preempted.
00015     """
00016     
00017     _preempted_name = 'preempted'
00018     preempt = False
00019     switching = False
00020 
00021     def __init__(self, *args, **kwargs):
00022         # add preempted outcome
00023         if len(args) > 0 and type(args[0]) is list:
00024             # need this ugly check for list type, because first argument in CBState is the callback
00025             args[0].append(self._preempted_name)
00026         else:
00027             outcomes = kwargs.get('outcomes', [])
00028             outcomes.append(self._preempted_name)
00029             kwargs['outcomes'] = outcomes
00030             
00031         super(PreemptableState, self).__init__(*args, **kwargs)
00032         self.__execute = self.execute
00033         self.execute = self._preemptable_execute
00034 
00035         self._feedback_topic = 'flexbe/command_feedback'
00036         self._preempt_topic = 'flexbe/command/preempt'
00037 
00038         self._pub = ProxyPublisher()
00039         self._sub = ProxySubscriberCached()
00040 
00041         PreemptableState.preempt = False
00042 
00043 
00044     def _preemptable_execute(self, *args, **kwargs):
00045         preempting = False
00046         if self._is_controlled and self._sub.has_msg(self._preempt_topic):
00047             self._sub.remove_last_msg(self._preempt_topic)
00048             self._pub.publish(self._feedback_topic, CommandFeedback(command="preempt"))
00049             preempting = True
00050             PreemptableState.preempt = True
00051             rospy.loginfo("--> Behavior will be preempted")
00052 
00053         if PreemptableState.preempt:
00054             preempting = True
00055             rospy.loginfo("Behavior will be preempted")
00056             
00057         if preempting:
00058             self.service_preempt()
00059             self._force_transition = True
00060             return self._preempted_name
00061 
00062         return self.__execute(*args, **kwargs)
00063 
00064 
00065     def _notify_skipped(self):
00066         # make sure we dont miss a preempt even if not being executed
00067         if self._is_controlled and self._sub.has_msg(self._preempt_topic):
00068             self._sub.remove_last_msg(self._preempt_topic)
00069             self._pub.publish(self._feedback_topic, CommandFeedback(command="preempt"))
00070             PreemptableState.preempt = True
00071 
00072 
00073     def _enable_ros_control(self):
00074         super(PreemptableState, self)._enable_ros_control()
00075         self._pub.createPublisher(self._feedback_topic, CommandFeedback)
00076         self._sub.subscribe(self._preempt_topic, Empty)
00077         PreemptableState.preempt = False
00078 
00079     def _disable_ros_control(self):
00080         super(PreemptableState, self)._disable_ros_control()
00081         self._sub.unsubscribe_topic(self._preempt_topic)


flexbe_core
Author(s): Philipp Schillinger
autogenerated on Thu Jun 6 2019 19:32:27