Source Code for Module smach_ros.monitor_state

 1   
 2  import roslib; roslib.load_manifest('smach_ros') 
 3  import rospy 
 4   
 5  import threading 
 6  import traceback 
 7   
 8  import smach 
 9   
10  __all__ = ['MonitorState'] 
11   
12 -class MonitorState(smach.State): 
13      """ 
14      A state that will check a given ROS topic with a condition function. 
15      """ 
16 -    def __init__(self, topic, msg_type, cond_cb, max_checks=-1,input_keys = [],output_keys=[]): 
17          """State constructor 
18          @type topic string 
19          @param topic the topic to monitor 
20   
21          @type msg_type a ROS message type 
22          @param msg_type determines the type of the monitored topic 
23   
24          @type max_checks int 
25          @param max_checks the number of messages to receive and evaluate. If cond_cb returns False for any 
26                 of them, the state will finish with outcome 'invalid'. If cond_cb returns True for  
27                 all of them, the outcome will be 'valid' 
28           
29          """ 
30          smach.State.__init__( 
31              self, 
32              outcomes=['valid','invalid','preempted'], 
33              input_keys = input_keys, 
34              output_keys = output_keys) 
35   
36          self._topic = topic 
37          self._msg_type = msg_type 
38          self._cond_cb = cond_cb 
39          self._max_checks = max_checks 
40          self._n_checks = 0 
41   
42          self._trigger_event = threading.Event() 
43   
44 -    def execute(self, ud): 
45          # If prempted before even getting a chance, give up. 
46          if self.preempt_requested(): 
47              self.service_preempt() 
48              return 'preempted' 
49   
50          self._n_checks = 0 
51          self._trigger_event.clear() 
52   
53          self._sub = rospy.Subscriber(self._topic, self._msg_type, self._cb, callback_args=ud) 
54   
55          self._trigger_event.wait() 
56          self._sub.unregister() 
57   
58          if self.preempt_requested(): 
59              self.service_preempt() 
60              return 'preempted' 
61   
62          if self._max_checks > 0 and self._n_checks >= self._max_checks: 
63              return 'valid' 
64   
65          return 'invalid' 
66   
67 -    def _cb(self,msg,ud) : 
68          try: 
69              if self._cond_cb(ud, msg): 
70                  self._n_checks +=1 
71              else: 
72                  self._trigger_event.set() 
73          except Exception as e: 
74              rospy.logerr("Error thrown while executing condition callback %s: %s" % (str(self._cond_cb), e)) 
75              self._trigger_event.set() 
76               
77          if (self._max_checks > 0 and self._n_checks >= self._max_checks): 
78              self._trigger_event.set() 
79   
80 -    def request_preempt(self): 
81          smach.State.request_preempt(self) 
82          self._trigger_event.set() 
83