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
61
62 - def _remaining(self, curr_time):
63 """
64 Calculate the time remaining for rate to sleep.
65 @param curr_time: current time
66 @type curr_time: L{Time}
67 @return: time remaining
68 @rtype: L{Time}
69 """
70
71 if self.last_time > curr_time:
72 self.last_time = curr_time
73
74
75 elapsed = curr_time - self.last_time
76 return self.sleep_dur - elapsed
77
78 - def remaining(self):
79 """
80 Return the time remaining for rate to sleep.
81 @return: time remaining
82 @rtype: L{Time}
83 """
84 curr_time = rospy.rostime.get_rostime()
85 return self._remaining(curr_time)
86
88 """
89 Attempt sleep at the specified rate. sleep() takes into
90 account the time elapsed since the last successful
91 sleep().
92
93 @raise ROSInterruptException: if ROS shutdown occurs before
94 sleep completes
95 @raise ROSTimeMovedBackwardsException: if ROS time is set
96 backwards
97 """
98 curr_time = rospy.rostime.get_rostime()
99 sleep(self._remaining(curr_time))
100 self.last_time = self.last_time + self.sleep_dur
101
102
103
104 if curr_time - self.last_time > self.sleep_dur * 2:
105 self.last_time = curr_time
106
158
160 """
161 Constructor.
162 @param last_expected: in a perfect world, this is when the previous callback should have happened
163 @type last_expected: rospy.Time
164 @param last_real: when the callback actually happened
165 @type last_real: rospy.Time
166 @param current_expected: in a perfect world, this is when the current callback should have been called
167 @type current_expected: rospy.Time
168 @param last_duration: contains the duration of the last callback (end time minus start time) in seconds.
169 Note that this is always in wall-clock time.
170 @type last_duration: float
171 """
172 - def __init__(self, last_expected, last_real, current_expected, current_real, last_duration):
173 self.last_expected = last_expected
174 self.last_real = last_real
175 self.current_expected = current_expected
176 self.current_real = current_real
177 self.last_duration = last_duration
178
179 -class Timer(threading.Thread):
180 """
181 Convenience class for calling a callback at a specified rate
182 """
183
184 - def __init__(self, period, callback, oneshot=False):
185 """
186 Constructor.
187 @param period: desired period between callbacks
188 @type period: rospy.Duration
189 @param callback: callback to be called
190 @type callback: function taking rospy.TimerEvent
191 @param oneshot: if True, fire only once, otherwise fire continuously until shutdown is called [default: False]
192 @type oneshot: bool
193 """
194 super(Timer, self).__init__()
195 self._period = period
196 self._callback = callback
197 self._oneshot = oneshot
198 self._shutdown = False
199 self.setDaemon(True)
200 self.start()
201
203 """
204 Stop firing callbacks.
205 """
206 self._shutdown = True
207
209 r = Rate(1.0 / self._period.to_sec())
210 current_expected = rospy.rostime.get_rostime() + self._period
211 last_expected, last_real, last_duration = None, None, None
212 while not rospy.core.is_shutdown() and not self._shutdown:
213 try:
214 r.sleep()
215 except rospy.exceptions.ROSInterruptException as e:
216 if rospy.core.is_shutdown():
217 break
218 raise
219 if self._shutdown:
220 break
221 current_real = rospy.rostime.get_rostime()
222 start = time.time()
223 self._callback(TimerEvent(last_expected, last_real, current_expected, current_real, last_duration))
224 if self._oneshot:
225 break
226 last_duration = time.time() - start
227 last_expected, last_real = current_expected, current_real
228 current_expected += self._period
229