1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35 import threading
36 import time
37
38
39 import genpy
40
41 import rospy.core
42 import rospy.rostime
43
44
45
46
48 """
49 Convenience class for sleeping in a loop at a specified rate
50 """
51
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
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
74 if self.last_time > curr_time:
75 self.last_time = curr_time
76
77
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
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
112
113 if curr_time - self.last_time > self.sleep_dur * 2:
114 self.last_time = curr_time
115
166
168 """
169 Constructor.
170 @param last_expected: in a perfect world, this is when the previous callback should have happened
171 @type last_expected: rospy.Time
172 @param last_real: when the callback actually happened
173 @type last_real: rospy.Time
174 @param current_expected: in a perfect world, this is when the current callback should have been called
175 @type current_expected: rospy.Time
176 @param last_duration: contains the duration of the last callback (end time minus start time) in seconds.
177 Note that this is always in wall-clock time.
178 @type last_duration: float
179 """
180 - def __init__(self, last_expected, last_real, current_expected, current_real, last_duration):
181 self.last_expected = last_expected
182 self.last_real = last_real
183 self.current_expected = current_expected
184 self.current_real = current_real
185 self.last_duration = last_duration
186
187 -class Timer(threading.Thread):
188 """
189 Convenience class for calling a callback at a specified rate
190 """
191
192 - def __init__(self, period, callback, oneshot=False, reset=False):
193 """
194 Constructor.
195 @param period: desired period between callbacks
196 @type period: rospy.Duration
197 @param callback: callback to be called
198 @type callback: function taking rospy.TimerEvent
199 @param oneshot: if True, fire only once, otherwise fire continuously until shutdown is called [default: False]
200 @type oneshot: bool
201 @param reset: if True, timer is reset when rostime moved backward. [default: False]
202 @type reset: bool
203 """
204 super(Timer, self).__init__()
205 self._period = period
206 self._callback = callback
207 self._oneshot = oneshot
208 self._reset = reset
209 self._shutdown = False
210 self.daemon = True
211 self.start()
212
214 """
215 Stop firing callbacks.
216 """
217 self._shutdown = True
218
220 r = Rate(1.0 / self._period.to_sec(), reset=self._reset)
221 current_expected = rospy.rostime.get_rostime() + self._period
222 last_expected, last_real, last_duration = None, None, None
223 while not rospy.core.is_shutdown() and not self._shutdown:
224 try:
225 r.sleep()
226 except rospy.exceptions.ROSInterruptException as e:
227 if rospy.core.is_shutdown():
228 break
229 raise
230 if self._shutdown:
231 break
232 current_real = rospy.rostime.get_rostime()
233 start = time.time()
234 self._callback(TimerEvent(last_expected, last_real, current_expected, current_real, last_duration))
235 if self._oneshot:
236 break
237 last_duration = time.time() - start
238 last_expected, last_real = current_expected, current_real
239 current_expected += self._period
240