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
167
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
215 """
216 Stop firing callbacks.
217 """
218 self._shutdown = True
219
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