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 """
36 ROS time and duration representations, as well as internal routines
37 for managing wallclock versus a simulated clock. The important data
38 classes are L{Time} and L{Duration}, which represent the ROS 'time'
39 and 'duration' primitives, respectively.
40 """
41
42 import sys
43 import threading
44 import time
45 import traceback
46
47 import rospy.exceptions
48
49 import genpy
50
51
52
53
54 _rostime_initialized = False
55 _rostime_current = None
56 _rostime_cond = threading.Condition()
60 """
61 Duration represents the ROS 'duration' primitive type, which
62 consists of two integers: seconds and nanoseconds. The Duration
63 class allows you to add and subtract Duration instances, including
64 adding and subtracting from L{Time} instances.
65
66 Usage::
67 five_seconds = Duration(5)
68 five_nanoseconds = Duration(0, 5)
69
70 print 'Fields are', five_seconds.secs, five_seconds.nsecs
71
72 # Duration arithmetic
73 ten_seconds = five_seconds + five_seconds
74 five_secs_ago = rospy.Time.now() - five_seconds # Time minus Duration is a Time
75
76 true_val = ten_second > five_seconds
77 """
78 __slots__ = []
79
81 """
82 Create new Duration instance. secs and nsecs are integers and
83 correspond to the ROS 'duration' primitive type.
84
85 @param secs: seconds
86 @type secs: int
87 @param nsecs: nanoseconds
88 @type nsecs: int
89 """
90 super(Duration, self).__init__(secs, nsecs)
91
93 return 'rospy.Duration[%d]' % self.to_nsec()
94
95 -class Time(genpy.Time):
96 """
97 Time represents the ROS 'time' primitive type, which consists of two
98 integers: seconds since epoch and nanoseconds since seconds. Time
99 instances are mutable.
100
101 The L{Time.now()} factory method can initialize Time to the
102 current ROS time and L{from_sec()} can be used to create a
103 Time instance from the Python's time.time() float seconds
104 representation.
105
106 The Time class allows you to subtract Time instances to compute
107 Durations, as well as add Durations to Time to create new Time
108 instances.
109
110 Usage::
111 now = rospy.Time.now()
112 zero_time = rospy.Time()
113
114 print 'Fields are', now.secs, now.nsecs
115
116 # Time arithmetic
117 five_secs_ago = now - rospy.Duration(5) # Time minus Duration is a Time
118 five_seconds = now - five_secs_ago # Time minus Time is a Duration
119 true_val = now > five_secs_ago
120
121 # NOTE: in general, you will want to avoid using time.time() in ROS code
122 import time
123 py_time = rospy.Time.from_sec(time.time())
124 """
125 __slots__ = []
126
128 """
129 Constructor: secs and nsecs are integers and correspond to the
130 ROS 'time' primitive type. You may prefer to use the static
131 L{from_sec()} and L{now()} factory methods instead.
132
133 @param secs: seconds since epoch
134 @type secs: int
135 @param nsecs: nanoseconds since seconds (since epoch)
136 @type nsecs: int
137 """
138 super(Time, self).__init__(secs, nsecs)
139
141 return 'rospy.Time[%d]' % self.to_nsec()
142
143 @staticmethod
145 """
146 Create new L{Time} instance representing current time. This
147 can either be wall-clock time or a simulated clock. It is
148 strongly recommended that you use the now() factory to create
149 current time representations instead of reading wall-clock
150 time and create Time instances from it.
151
152 @return: L{Time} instance for current time
153 @rtype: L{Time}
154 """
155 return get_rostime()
156
157 @classmethod
159 """
160 Use Time.from_sec() instead. Retained for backwards compatibility.
161
162 @param float_secs: time value in time.time() format
163 @type float_secs: float
164 @return: Time instance for specified time
165 @rtype: L{Time}
166 """
167 return cls.from_sec(float_secs)
168
182
184 """
185 Get the current time as a L{Time} object
186 @return: current time as a L{rospy.Time} object
187 @rtype: L{Time}
188 """
189 if not _rostime_initialized:
190 raise rospy.exceptions.ROSInitException("time is not initialized. Have you called init_node()?")
191 if _rostime_current is not None:
192
193 return _rostime_current
194 else:
195
196 float_secs = time.time()
197 secs = int(float_secs)
198 nsecs = int((float_secs - secs) * 1000000000)
199 return Time(secs, nsecs)
200
202 """
203 Get the current time as float secs (time.time() format)
204 @return: time in secs (time.time() format)
205 @rtype: float
206 """
207 return Time.now().to_sec()
208
210 """
211 Internal use.
212 Mark rostime as initialized. This flag enables other routines to
213 throw exceptions if rostime is being used before the underlying
214 system is initialized.
215 @param val: value for initialization state
216 @type val: bool
217 """
218 global _rostime_initialized
219 _rostime_initialized = val
220
222 """
223 Internal use.
224 @return: True if rostime has been initialized
225 @rtype: bool
226 """
227 return _rostime_initialized
228
230 """
231 internal API for helper routines that need to wait on time updates
232 @return: rostime conditional var
233 @rtype: threading.Cond
234 """
235 return _rostime_cond
236
238 """
239 Internal use for ROS-time routines.
240 @return: True if ROS is currently using wallclock time
241 @rtype: bool
242 """
243 return _rostime_current == None
244
257
259 """
260 Internal use.
261 Windows interrupts time.sleep with an IOError exception
262 when a signal is caught. Even when the signal is handled
263 by a callback, it will then proceed to throw IOError when
264 the handling has completed.
265
266 Refer to https://code.ros.org/trac/ros/ticket/3421.
267
268 So we create a platform dependant wrapper to handle this
269 here.
270 """
271 if sys.platform in ['win32']:
272 try:
273 time.sleep(duration)
274 except IOError:
275 pass
276 else:
277 time.sleep(duration)
278