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 current_real: when the current callback is actually being called
177 (rospy.Time.now() as of immediately before calling the callback)
178 @type current_real: rospy.Time
179 @param last_duration: contains the duration of the last callback (end time minus start time) in seconds.
180 Note that this is always in wall-clock time.
181 @type last_duration: float
182 """
183 - def __init__(self, last_expected, last_real, current_expected, current_real, last_duration):
184 self.last_expected = last_expected
185 self.last_real = last_real
186 self.current_expected = current_expected
187 self.current_real = current_real
188 self.last_duration = last_duration
189
190 -class Timer(threading.Thread):
191 """
192 Convenience class for calling a callback at a specified rate
193 """
194
195 - def __init__(self, period, callback, oneshot=False, reset=False):
196 """
197 Constructor.
198 @param period: desired period between callbacks
199 @type period: rospy.Duration
200 @param callback: callback to be called
201 @type callback: function taking rospy.TimerEvent
202 @param oneshot: if True, fire only once, otherwise fire continuously until shutdown is called [default: False]
203 @type oneshot: bool
204 @param reset: if True, timer is reset when rostime moved backward. [default: False]
205 @type reset: bool
206 """
207 super(Timer, self).__init__()
208 self._period = period
209 self._callback = callback
210 self._oneshot = oneshot
211 self._reset = reset
212 self._shutdown = False
213 self.daemon = True
214 self.start()
215
217 """
218 Stop firing callbacks.
219 """
220 self._shutdown = True
221
223 r = Rate(1.0 / self._period.to_sec(), reset=self._reset)
224 current_expected = rospy.rostime.get_rostime() + self._period
225 last_expected, last_real, last_duration = None, None, None
226 while not rospy.core.is_shutdown() and not self._shutdown:
227 try:
228 r.sleep()
229 except rospy.exceptions.ROSInterruptException as e:
230 if rospy.core.is_shutdown():
231 break
232 raise
233 if self._shutdown:
234 break
235 current_real = rospy.rostime.get_rostime()
236 start = time.time()
237 self._callback(TimerEvent(last_expected, last_real, current_expected, current_real, last_duration))
238 if self._oneshot:
239 break
240 last_duration = time.time() - start
241 last_expected, last_real = current_expected, current_real
242 current_expected += self._period
243