Source Code for Module rospy.timer

  1  # Software License Agreement (BSD License) 
  2  # 
  3  # Copyright (c) 2010, Willow Garage, Inc. 
  4  # All rights reserved. 
  5  # 
  6  # Redistribution and use in source and binary forms, with or without 
  7  # modification, are permitted provided that the following conditions 
  8  # are met: 
  9  # 
 10  #  * Redistributions of source code must retain the above copyright 
 11  #    notice, this list of conditions and the following disclaimer. 
 12  #  * Redistributions in binary form must reproduce the above 
 13  #    copyright notice, this list of conditions and the following 
 14  #    disclaimer in the documentation and/or other materials provided 
 15  #    with the distribution. 
 16  #  * Neither the name of Willow Garage, Inc. nor the names of its 
 17  #    contributors may be used to endorse or promote products derived 
 18  #    from this software without specific prior written permission. 
 19  # 
 20  # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 
 21  # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 
 22  # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 
 23  # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 
 24  # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 
 25  # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 
 26  # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
 27  # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 
 28  # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 
 29  # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 
 30  # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
 31  # POSSIBILITY OF SUCH DAMAGE. 
 32  # 
 33  # Revision $Id: __init__.py 12069 2010-11-09 20:31:55Z kwc $ 
 34   
 35  import threading 
 36  import time 
 37   
 38  import roslib.rostime 
 39   
 40  import rospy.core 
 41  import rospy.rostime 
 42   
 43  # author: tfield (Timers) 
 44  # author: kwc (Rate, sleep) 
 45   
 46 -class Rate(object): 
 47      """ 
 48      Convenience class for sleeping in a loop at a specified rate 
 49      """ 
 50       
 51 -    def __init__(self, hz): 
 52          """ 
 53          Constructor. 
 54          @param hz: hz rate to determine sleeping 
 55          @type  hz: int 
 56          """ 
 57          # #1403 
 58          self.last_time = rospy.rostime.get_rostime() 
 59          self.sleep_dur = rospy.rostime.Duration(0, int(1e9/hz)) 
 60   
 61 -    def sleep(self): 
 62          """ 
 63          Attempt sleep at the specified rate. sleep() takes into 
 64          account the time elapsed since the last successful 
 65          sleep(). 
 66           
 67          @raise ROSInterruptException: if ROS time is set backwards or if 
 68          ROS shutdown occurs before sleep completes 
 69          """ 
 70          curr_time = rospy.rostime.get_rostime() 
 71          # detect time jumping backwards 
 72          if self.last_time > curr_time: 
 73              self.last_time = curr_time 
 74   
 75          # calculate sleep interval 
 76          elapsed = curr_time - self.last_time 
 77          sleep(self.sleep_dur - elapsed) 
 78          self.last_time = self.last_time + self.sleep_dur 
 79   
 80          # detect time jumping forwards, as well as loops that are 
 81          # inherently too slow 
 82          if curr_time - self.last_time > self.sleep_dur * 2: 
 83              self.last_time = curr_time 
 84   
 85  # TODO: may want more specific exceptions for sleep 
 86 -def sleep(duration): 
 87      """ 
 88      sleep for the specified duration in ROS time. If duration 
 89      is negative, sleep immediately returns. 
 90       
 91      @param duration: seconds (or rospy.Duration) to sleep 
 92      @type  duration: float or Duration 
 93      @raise ROSInterruptException: if ROS time is set backwards or if 
 94      ROS shutdown occurs before sleep completes 
 95      """ 
 96      if rospy.rostime.is_wallclock(): 
 97          if isinstance(duration, roslib.rostime.Duration): 
 98              duration = duration.to_sec() 
 99          if duration < 0: 
100              return 
101          else: 
102              rospy.rostime.wallsleep(duration) 
103      else: 
104          initial_rostime = rospy.rostime.get_rostime() 
105          if not isinstance(duration, roslib.rostime.Duration): 
106              duration = rospy.rostime.Duration.from_sec(duration) 
107   
108          rostime_cond = rospy.rostime.get_rostime_cond() 
109   
110          # #3123 
111          if initial_rostime == roslib.rostime.Time(0): 
112              # break loop if time is initialized or node is shutdown 
113              while initial_rostime == roslib.rostime.Time(0) and \ 
114                        not rospy.core.is_shutdown(): 
115                  with rostime_cond: 
116                      rostime_cond.wait(0.3) 
117                  initial_rostime = rospy.rostime.get_rostime() 
118   
119          sleep_t = initial_rostime + duration 
120   
121          # break loop if sleep_t is reached, time moves backwards, or 
122          # node is shutdown 
123          while rospy.rostime.get_rostime() < sleep_t and \ 
124                rospy.rostime.get_rostime() >= initial_rostime and \ 
125                    not rospy.core.is_shutdown(): 
126              with rostime_cond: 
127                  rostime_cond.wait(0.5) 
128   
129          if rospy.rostime.get_rostime() < initial_rostime: 
130              raise rospy.exceptions.ROSInterruptException("ROS time moved backwards") 
131          if rospy.core.is_shutdown(): 
132              raise rospy.exceptions.ROSInterruptException("ROS shutdown request") 
133   
134 -class TimerEvent(object): 
135      """ 
136      Constructor. 
137      @param last_expected: in a perfect world, this is when the previous callback should have happened 
138      @type  last_expected: rospy.Time 
139      @param last_real: when the callback actually happened 
140      @type  last_real: rospy.Time 
141      @param current_expected: in a perfect world, this is when the current callback should have been called 
142      @type  current_expected: rospy.Time 
143      @param last_duration: contains the duration of the last callback (end time minus start time) in seconds. 
144                            Note that this is always in wall-clock time. 
145      @type  last_duration: float 
146      """ 
147 -    def __init__(self, last_expected, last_real, current_expected, current_real, last_duration): 
148          self.last_expected    = last_expected 
149          self.last_real        = last_real 
150          self.current_expected = current_expected 
151          self.current_real     = current_real 
152          self.last_duration    = last_duration 
153   
154 -class Timer(threading.Thread): 
155      """ 
156      Convenience class for calling a callback at a specified rate 
157      """ 
158   
159 -    def __init__(self, period, callback, oneshot=False): 
160          """ 
161          Constructor. 
162          @param period: desired period between callbacks 
163          @type  period: rospy.Time 
164          @param callback: callback to be called 
165          @type  callback: function taking rospy.TimerEvent 
166          @param oneshot: if True, fire only once, otherwise fire continuously until shutdown is called [default: False] 
167          @type  oneshot: bool 
168          """ 
169          threading.Thread.__init__(self) 
170          self._period   = period 
171          self._callback = callback 
172          self._oneshot  = oneshot 
173          self._shutdown = False 
174          self.setDaemon(True) 
175          self.start() 
176   
177 -    def shutdown(self): 
178          """ 
179          Stop firing callbacks. 
180          """ 
181          self._shutdown = True 
182           
183 -    def run(self): 
184          r = Rate(1.0 / self._period.to_sec()) 
185          current_expected = rospy.rostime.get_rostime() + self._period 
186          last_expected, last_real, last_duration = None, None, None 
187          while not rospy.core.is_shutdown() and not self._shutdown: 
188              r.sleep() 
189              current_real = rospy.rostime.get_rostime() 
190              start = time.time() 
191              self._callback(TimerEvent(last_expected, last_real, current_expected, current_real, last_duration)) 
192              if self._oneshot: 
193                  break 
194              last_duration = time.time() - start 
195              last_expected, last_real = current_expected, current_real 
196              current_expected += self._period 
197