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      """A state that will check a given ROS topic with a condition function.  
14      """ 
15 -    def __init__(self, topic, msg_type, cond_cb, max_checks=-1): 
16          smach.State.__init__(self,outcomes=['valid','invalid','preempted']) 
17   
18          self._topic = topic 
19          self._msg_type = msg_type 
20          self._cond_cb = cond_cb 
21          self._max_checks = max_checks 
22          self._n_checks = 0 
23   
24          self._trigger_cond = threading.Condition() 
25   
26 -    def execute(self, ud): 
27          self._n_checks = 0 
28   
29          self._sub = rospy.Subscriber(self._topic, self._msg_type, self._cb, callback_args=[ud]) 
30   
31          self._trigger_cond.acquire() 
32          self._trigger_cond.wait() 
33          self._trigger_cond.release() 
34   
35          self._sub.unregister() 
36   
37          if self.preempt_requested(): 
38              self.service_preempt() 
39              return 'preempted' 
40   
41          if self._max_checks > 0 and self._n_checks >= self._max_checks: 
42              return 'valid' 
43   
44          return 'invalid' 
45   
46 -    def _cb(self, msg, ud): 
47          self._n_checks += 1 
48          try: 
49              if (self._max_checks > 0 and self._n_checks >= self._max_checks) or not self._cond_cb(ud, msg): 
50                  self._trigger_cond.acquire() 
51                  self._trigger_cond.notify() 
52                  self._trigger_cond.release() 
53          except Exception as e: 
54              rospy.logerr("Error thrown while executing condition callback %s: %s" % (str(self._cond_cb), e)) 
55              self._trigger_cond.acquire() 
56              self._trigger_cond.notify() 
57              self._trigger_cond.release() 
58   
59 -    def request_preempt(self): 
60          smach.State.request_preempt(self) 
61          self._trigger_cond.acquire() 
62          self._trigger_cond.notify() 
63          self._trigger_cond.release() 
64