Package smach_ros :: Module monitor_state

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