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):

53 """ 54 Constructor. 55 @param hz: hz rate to determine sleeping 56 @type hz: int 57 """ 58 # #1403 59 self.last_time = rospy.rostime.get_rostime() 60 self.sleep_dur = rospy.rostime.Duration(0, int(1e9/hz))

61

62 - def sleep(self):

63 """ 64 Attempt sleep at the specified rate. sleep() takes into 65 account the time elapsed since the last successful 66 sleep(). 67 68 @raise ROSInterruptException: if ROS time is set backwards or if 69 ROS shutdown occurs before sleep completes 70 """ 71 curr_time = rospy.rostime.get_rostime() 72 # detect time jumping backwards 73 if self.last_time > curr_time: 74 self.last_time = curr_time 75 76 # calculate sleep interval 77 elapsed = curr_time - self.last_time 78 sleep(self.sleep_dur - elapsed) 79 self.last_time = self.last_time + self.sleep_dur 80 81 # detect time jumping forwards, as well as loops that are 82 # inherently too slow 83 if curr_time - self.last_time > self.sleep_dur * 2: 84 self.last_time = curr_time

85 86 # TODO: may want more specific exceptions for sleep

87 -def sleep(duration):

88 """ 89 sleep for the specified duration in ROS time. If duration 90 is negative, sleep immediately returns. 91 92 @param duration: seconds (or rospy.Duration) to sleep 93 @type duration: float or Duration 94 @raise ROSInterruptException: if ROS time is set backwards or if 95 ROS shutdown occurs before sleep completes 96 """ 97 if rospy.rostime.is_wallclock(): 98 if isinstance(duration, genpy.Duration): 99 duration = duration.to_sec() 100 if duration < 0: 101 return 102 else: 103 rospy.rostime.wallsleep(duration) 104 else: 105 initial_rostime = rospy.rostime.get_rostime() 106 if not isinstance(duration, genpy.Duration): 107 duration = genpy.Duration.from_sec(duration) 108 109 rostime_cond = rospy.rostime.get_rostime_cond() 110 111 # #3123 112 if initial_rostime == genpy.Time(0): 113 # break loop if time is initialized or node is shutdown 114 while initial_rostime == genpy.Time(0) and \ 115 not rospy.core.is_shutdown(): 116 with rostime_cond: 117 rostime_cond.wait(0.3) 118 initial_rostime = rospy.rostime.get_rostime() 119 120 sleep_t = initial_rostime + duration 121 122 # break loop if sleep_t is reached, time moves backwards, or 123 # node is shutdown 124 while rospy.rostime.get_rostime() < sleep_t and \ 125 rospy.rostime.get_rostime() >= initial_rostime and \ 126 not rospy.core.is_shutdown(): 127 with rostime_cond: 128 rostime_cond.wait(0.5) 129 130 if rospy.rostime.get_rostime() < initial_rostime: 131 raise rospy.exceptions.ROSInterruptException("ROS time moved backwards") 132 if rospy.core.is_shutdown(): 133 raise rospy.exceptions.ROSInterruptException("ROS shutdown request")

134

135 -class TimerEvent(object):

136 """ 137 Constructor. 138 @param last_expected: in a perfect world, this is when the previous callback should have happened 139 @type last_expected: rospy.Time 140 @param last_real: when the callback actually happened 141 @type last_real: rospy.Time 142 @param current_expected: in a perfect world, this is when the current callback should have been called 143 @type current_expected: rospy.Time 144 @param last_duration: contains the duration of the last callback (end time minus start time) in seconds. 145 Note that this is always in wall-clock time. 146 @type last_duration: float 147 """

148 - def __init__(self, last_expected, last_real, current_expected, current_real, last_duration):

149 self.last_expected = last_expected 150 self.last_real = last_real 151 self.current_expected = current_expected 152 self.current_real = current_real 153 self.last_duration = last_duration

154

155 -class Timer(threading.Thread):

156 """ 157 Convenience class for calling a callback at a specified rate 158 """ 159

160 - def __init__(self, period, callback, oneshot=False):

161 """ 162 Constructor. 163 @param period: desired period between callbacks 164 @type period: rospy.Time 165 @param callback: callback to be called 166 @type callback: function taking rospy.TimerEvent 167 @param oneshot: if True, fire only once, otherwise fire continuously until shutdown is called [default: False] 168 @type oneshot: bool 169 """ 170 threading.Thread.__init__(self) 171 self._period = period 172 self._callback = callback 173 self._oneshot = oneshot 174 self._shutdown = False 175 self.setDaemon(True) 176 self.start()

177

178 - def shutdown(self):

179 """ 180 Stop firing callbacks. 181 """ 182 self._shutdown = True

183

184 - def run(self):

185 r = Rate(1.0 / self._period.to_sec()) 186 current_expected = rospy.rostime.get_rostime() + self._period 187 last_expected, last_real, last_duration = None, None, None 188 while not rospy.core.is_shutdown() and not self._shutdown: 189 r.sleep() 190 if self._shutdown: 191 break 192 current_real = rospy.rostime.get_rostime() 193 start = time.time() 194 self._callback(TimerEvent(last_expected, last_real, current_expected, current_real, last_duration)) 195 if self._oneshot: 196 break 197 last_duration = time.time() - start 198 last_expected, last_real = current_expected, current_real 199 current_expected += self._period

200

Source Code for Module rospy.timer