Package rospy :: Module timer

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  # for Time, Duration 
 39  import genpy 
 40   
 41  import rospy.core 
 42  import rospy.rostime 
 43   
 44  # author: tfield (Timers) 
 45  # author: kwc (Rate, sleep) 
 46   
47 -class Rate(object):
48 """ 49 Convenience class for sleeping in a loop at a specified rate 50 """ 51
52 - def __init__(self, hz, reset=False):
53 """ 54 Constructor. 55 @param hz: hz rate to determine sleeping 56 @type hz: float 57 @param reset: if True, timer is reset when rostime moved backward. [default: False] 58 @type reset: bool 59 """ 60 # #1403 61 self.last_time = rospy.rostime.get_rostime() 62 self.sleep_dur = rospy.rostime.Duration(0, int(1e9/hz)) 63 self._reset = reset
64
65 - def _remaining(self, curr_time):
66 """ 67 Calculate the time remaining for rate to sleep. 68 @param curr_time: current time 69 @type curr_time: L{Time} 70 @return: time remaining 71 @rtype: L{Time} 72 """ 73 # detect time jumping backwards 74 if self.last_time > curr_time: 75 self.last_time = curr_time 76 77 # calculate remaining time 78 elapsed = curr_time - self.last_time 79 return self.sleep_dur - elapsed
80
81 - def remaining(self):
82 """ 83 Return the time remaining for rate to sleep. 84 @return: time remaining 85 @rtype: L{Time} 86 """ 87 curr_time = rospy.rostime.get_rostime() 88 return self._remaining(curr_time)
89
90 - def sleep(self):
91 """ 92 Attempt sleep at the specified rate. sleep() takes into 93 account the time elapsed since the last successful 94 sleep(). 95 96 @raise ROSInterruptException: if ROS shutdown occurs before 97 sleep completes 98 @raise ROSTimeMovedBackwardsException: if ROS time is set 99 backwards 100 """ 101 curr_time = rospy.rostime.get_rostime() 102 try: 103 sleep(self._remaining(curr_time)) 104 except rospy.exceptions.ROSTimeMovedBackwardsException: 105 if not self._reset: 106 raise 107 self.last_time = rospy.rostime.get_rostime() 108 return 109 self.last_time = self.last_time + self.sleep_dur 110 111 # detect time jumping forwards, as well as loops that are 112 # inherently too slow 113 if curr_time - self.last_time > self.sleep_dur * 2: 114 self.last_time = curr_time
115
116 -def sleep(duration):
117 """ 118 sleep for the specified duration in ROS time. If duration 119 is negative, sleep immediately returns. 120 121 @param duration: seconds (or rospy.Duration) to sleep 122 @type duration: float or Duration 123 @raise ROSInterruptException: if ROS shutdown occurs before sleep 124 completes 125 @raise ROSTimeMovedBackwardsException: if ROS time is set 126 backwards 127 """ 128 if rospy.rostime.is_wallclock(): 129 if isinstance(duration, genpy.Duration): 130 duration = duration.to_sec() 131 if duration < 0: 132 return 133 else: 134 rospy.rostime.wallsleep(duration) 135 else: 136 initial_rostime = rospy.rostime.get_rostime() 137 if not isinstance(duration, genpy.Duration): 138 duration = genpy.Duration.from_sec(duration) 139 140 rostime_cond = rospy.rostime.get_rostime_cond() 141 142 # #3123 143 if initial_rostime == genpy.Time(0): 144 # break loop if time is initialized or node is shutdown 145 while initial_rostime == genpy.Time(0) and \ 146 not rospy.core.is_shutdown(): 147 with rostime_cond: 148 rostime_cond.wait(0.3) 149 initial_rostime = rospy.rostime.get_rostime() 150 151 sleep_t = initial_rostime + duration 152 153 # break loop if sleep_t is reached, time moves backwards, or 154 # node is shutdown 155 while rospy.rostime.get_rostime() < sleep_t and \ 156 rospy.rostime.get_rostime() >= initial_rostime and \ 157 not rospy.core.is_shutdown(): 158 with rostime_cond: 159 rostime_cond.wait(0.5) 160 161 if rospy.rostime.get_rostime() < initial_rostime: 162 time_jump = (initial_rostime - rospy.rostime.get_rostime()).to_sec() 163 rospy.core.logerr("ROS time moved backwards: %ss", time_jump) 164 raise rospy.exceptions.ROSTimeMovedBackwardsException(time_jump) 165 if rospy.core.is_shutdown(): 166 raise rospy.exceptions.ROSInterruptException("ROS shutdown request")
167
168 -class TimerEvent(object):
169 """ 170 Constructor. 171 @param last_expected: in a perfect world, this is when the previous callback should have happened 172 @type last_expected: rospy.Time 173 @param last_real: when the callback actually happened 174 @type last_real: rospy.Time 175 @param current_expected: in a perfect world, this is when the current callback should have been called 176 @type current_expected: rospy.Time 177 @param last_duration: contains the duration of the last callback (end time minus start time) in seconds. 178 Note that this is always in wall-clock time. 179 @type last_duration: float 180 """
181 - def __init__(self, last_expected, last_real, current_expected, current_real, last_duration):
182 self.last_expected = last_expected 183 self.last_real = last_real 184 self.current_expected = current_expected 185 self.current_real = current_real 186 self.last_duration = last_duration
187
188 -class Timer(threading.Thread):
189 """ 190 Convenience class for calling a callback at a specified rate 191 """ 192
193 - def __init__(self, period, callback, oneshot=False, reset=False):
194 """ 195 Constructor. 196 @param period: desired period between callbacks 197 @type period: rospy.Duration 198 @param callback: callback to be called 199 @type callback: function taking rospy.TimerEvent 200 @param oneshot: if True, fire only once, otherwise fire continuously until shutdown is called [default: False] 201 @type oneshot: bool 202 @param reset: if True, timer is reset when rostime moved backward. [default: False] 203 @type reset: bool 204 """ 205 super(Timer, self).__init__() 206 self._period = period 207 self._callback = callback 208 self._oneshot = oneshot 209 self._reset = reset 210 self._shutdown = False 211 self.setDaemon(True) 212 self.start()
213
214 - def shutdown(self):
215 """ 216 Stop firing callbacks. 217 """ 218 self._shutdown = True
219
220 - def run(self):
221 r = Rate(1.0 / self._period.to_sec(), reset=self._reset) 222 current_expected = rospy.rostime.get_rostime() + self._period 223 last_expected, last_real, last_duration = None, None, None 224 while not rospy.core.is_shutdown() and not self._shutdown: 225 try: 226 r.sleep() 227 except rospy.exceptions.ROSInterruptException as e: 228 if rospy.core.is_shutdown(): 229 break 230 raise 231 if self._shutdown: 232 break 233 current_real = rospy.rostime.get_rostime() 234 start = time.time() 235 self._callback(TimerEvent(last_expected, last_real, current_expected, current_real, last_duration)) 236 if self._oneshot: 237 break 238 last_duration = time.time() - start 239 last_expected, last_real = current_expected, current_real 240 current_expected += self._period
241