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 threading
43 import time
44 import traceback
45
46 import rospy.exceptions
47
48 import roslib.rosenv
49 import roslib.rostime
50
51
52
53
54 _rostime_initialized = False
55 _rostime_current = None
56 _rostime_cond = threading.Condition()
57
58
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 roslib.rostime.Duration.__init__(self, secs, nsecs)
91
92 -class Time(roslib.rostime.Time):
93 """
94 Time represents the ROS 'time' primitive type, which consists of two
95 integers: seconds since epoch and nanoseconds since seconds. Time
96 instances are mutable.
97
98 The L{Time.now()} factory method can initialize Time to the
99 current ROS time and L{from_sec()} can be used to create a
100 Time instance from the Python's time.time() float seconds
101 representation.
102
103 The Time class allows you to subtract Time instances to compute
104 Durations, as well as add Durations to Time to create new Time
105 instances.
106
107 Usage::
108 now = rospy.Time.now()
109 zero_time = rospy.Time()
110
111 print 'Fields are', now.secs, now.nsecs
112
113 # Time arithmetic
114 five_secs_ago = now - rospy.Duration(5) # Time minus Duration is a Time
115 five_seconds = now - five_secs_ago # Time minus Time is a Duration
116 true_val = now > five_secs_ago
117
118 # NOTE: in general, you will want to avoid using time.time() in ROS code
119 import time
120 py_time = rospy.Time.from_sec(time.time())
121 """
122 __slots__ = []
123
125 """
126 Constructor: secs and nsecs are integers and correspond to the
127 ROS 'time' primitive type. You may prefer to use the static
128 L{from_sec()} and L{now()} factory methods instead.
129
130 @param secs: seconds since epoch
131 @type secs: int
132 @param nsecs: nanoseconds since seconds (since epoch)
133 @type nsecs: int
134 """
135 roslib.rostime.Time.__init__(self, secs, nsecs)
136
138 """
139 Create new L{Time} instance representing current time. This
140 can either be wall-clock time or a simulated clock. It is
141 strongly recommended that you use the now() factory to create
142 current time representations instead of reading wall-clock
143 time and create Time instances from it.
144
145 @return: L{Time} instance for current time
146 @rtype: L{Time}
147 """
148 if not _rostime_initialized:
149 raise rospy.exceptions.ROSInitException("time is not initialized. Have you called init_node()?")
150 if _rostime_current is not None:
151
152 return _rostime_current
153 else:
154
155 float_secs = time.time()
156 secs = int(float_secs)
157 nsecs = int((float_secs - secs) * 1000000000)
158 return Time(secs, nsecs)
159
160 now = staticmethod(now)
161
162
163
165 """
166 Use Time.from_sec() instead. Retained for backwards compatibility.
167
168 @param float_secs: time value in time.time() format
169 @type float_secs: float
170 @return: Time instance for specified time
171 @rtype: L{Time}
172 """
173 return Time.from_sec(float_secs)
174
175 from_seconds = staticmethod(from_seconds)
176
178 """
179 Create new Time instance from a float seconds representation
180 (e.g. time.time()).
181
182 @param float_secs: time value in time.time() format
183 @type float_secs: float
184 @return: Time instance for specified time
185 @rtype: L{Time}
186 """
187 secs = int(float_secs)
188 nsecs = int((float_secs - secs) * 1000000000)
189 return Time(secs, nsecs)
190
191 from_sec = staticmethod(from_sec)
192
206
208 """
209 Get the current time as a L{Time} object
210 @return: current time as a L{rospy.Time} object
211 @rtype: L{Time}
212 """
213 return Time.now()
214
216 """
217 Get the current time as float secs (time.time() format)
218 @return: time in secs (time.time() format)
219 @rtype: float
220 """
221 return Time.now().to_sec()
222
224 """
225 Internal use.
226 Mark rostime as initialized. This flag enables other routines to
227 throw exceptions if rostime is being used before the underlying
228 system is initialized.
229 @param val: value for initialization state
230 @type val: bool
231 """
232 global _rostime_initialized
233 _rostime_initialized = val
234
236 """
237 Internal use.
238 @return: True if rostime has been initialized
239 @rtype: bool
240 """
241 return _rostime_initialized
242
244 """
245 internal API for helper routines that need to wait on time updates
246 @return: rostime conditional var
247 @rtype: threading.Cond
248 """
249 return _rostime_cond
250
252 """
253 Internal use for ROS-time routines.
254 @return: True if ROS is currently using wallclock time
255 @rtype: bool
256 """
257 return _rostime_current == None
258
271