preemptable_state.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 import rospy
3 
4 from flexbe_core.core.loopback_state import LoopbackState
5 
6 from flexbe_core.proxy import ProxyPublisher, ProxySubscriberCached
7 from flexbe_msgs.msg import CommandFeedback
8 from std_msgs.msg import Empty
9 
10 
12  """
13  A state that can be preempted.
14  If preempted, the state will not be executed anymore and return the outcome preempted.
15  """
16 
17  _preempted_name = 'preempted'
18  preempt = False
19  switching = False
20 
21  def __init__(self, *args, **kwargs):
22  # add preempted outcome
23  if len(args) > 0 and type(args[0]) is list:
24  # need this ugly check for list type, because first argument in CBState is the callback
25  args[0].append(self._preempted_name)
26  else:
27  outcomes = kwargs.get('outcomes', [])
28  outcomes.append(self._preempted_name)
29  kwargs['outcomes'] = outcomes
30 
31  super(PreemptableState, self).__init__(*args, **kwargs)
32  self.__execute = self.execute
34 
35  self._feedback_topic = 'flexbe/command_feedback'
36  self._preempt_topic = 'flexbe/command/preempt'
37 
38  self._pub = ProxyPublisher()
39  self._sub = ProxySubscriberCached()
40 
41  PreemptableState.preempt = False
42 
43 
44  def _preemptable_execute(self, *args, **kwargs):
45  preempting = False
46  if self._is_controlled and self._sub.has_msg(self._preempt_topic):
47  self._sub.remove_last_msg(self._preempt_topic)
48  self._pub.publish(self._feedback_topic, CommandFeedback(command="preempt"))
49  preempting = True
50  PreemptableState.preempt = True
51  rospy.loginfo("--> Behavior will be preempted")
52 
53  if PreemptableState.preempt:
54  preempting = True
55  rospy.loginfo("Behavior will be preempted")
56 
57  if preempting:
58  self.service_preempt()
59  self._force_transition = True
60  return self._preempted_name
61 
62  return self.__execute(*args, **kwargs)
63 
64 
65  def _notify_skipped(self):
66  # make sure we dont miss a preempt even if not being executed
67  if self._is_controlled and self._sub.has_msg(self._preempt_topic):
68  self._sub.remove_last_msg(self._preempt_topic)
69  self._pub.publish(self._feedback_topic, CommandFeedback(command="preempt"))
70  PreemptableState.preempt = True
71 
72 
74  super(PreemptableState, self)._enable_ros_control()
75  self._pub.createPublisher(self._feedback_topic, CommandFeedback)
76  self._sub.subscribe(self._preempt_topic, Empty)
77  PreemptableState.preempt = False
78 
80  super(PreemptableState, self)._disable_ros_control()
81  self._sub.unsubscribe_topic(self._preempt_topic)


flexbe_core
Author(s): Philipp Schillinger
autogenerated on Wed Jun 5 2019 21:51:59