Source Code for Module smach_ros.condition_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__ = ['ConditionState'] 
11   
12 -class ConditionState(smach.State): 
13      """A state that will check a condition function a number of times. 
14       
15      If max_checks > 1, it will block while the condition is false and once it 
16      has checked max_checks times, it will return false. 
17      """ 
18 -    def __init__(self, 
19              cond_cb, 
20              input_keys = [], 
21              poll_rate = rospy.Duration(0.05), 
22              timeout = None, 
23              max_checks = 1): 
24          smach.State.__init__(self,outcomes = ['true', 'false','preempted'], input_keys = input_keys) 
25   
26          self._cond_cb = cond_cb 
27          if hasattr(cond_cb,'get_registered_input_keys') and hasattr(cond_cb,'get_registered_output_keys'): 
28              self._cond_cb_input_keys = cond_cb.get_registered_input_keys() 
29              self._cond_cb_output_keys = cond_cb.get_registered_output_keys() 
30              self.register_input_keys(self._cond_cb_input_keys) 
31              self.register_output_keys(self._cond_cb_output_keys) 
32          self._poll_rate = poll_rate 
33          self._timeout = timeout 
34          self._max_checks = max_checks 
35   
36 -    def execute(self, ud): 
37          start_time = rospy.Time.now() 
38          n_checks = 0 
39   
40          while self._max_checks == -1 or n_checks <= self._max_checks: 
41              # Check for timeout 
42              if self._timeout and rospy.Time.now() - start_time > self._timeout: 
43                  break 
44              # Check for preemption 
45              if self.preempt_requested(): 
46                  self.service_preempt() 
47                  return 'preempted' 
48              # Call the condition 
49              try: 
50                  if self._cond_cb(ud): 
51                      return 'true' 
52              except: 
53                  raise smach.InvalidUserCodeError("Error thrown while executing condition callback %s: " % str(self._cond_cb) +traceback.format_exc()) 
54              n_checks += 1 
55              rospy.sleep(self._poll_rate) 
56           
57          return 'false' 
58