00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 import roslib; roslib.load_manifest('test_rospy')
00037
00038 import os
00039 import sys
00040 import struct
00041 import unittest
00042 from cStringIO import StringIO
00043 import time
00044 import random
00045
00046 import rostest
00047
00048 import rospy.rostime
00049
00050
00051
00052 class TestRospyTime(unittest.TestCase):
00053
00054 def setUp(self):
00055 rospy.rostime.set_rostime_initialized(True)
00056
00057 def test_import_simtime(self):
00058
00059 import rospy.impl.simtime
00060
00061
00062 def test_switch_to_wallclock(self):
00063 rospy.rostime.switch_to_wallclock()
00064 self.assertAlmostEqual(time.time(), rospy.get_time(), 1)
00065
00066 def test_Time_get_setstate(self):
00067
00068 import copy, random
00069 a = rospy.Time(random.randint(0, 10000), random.randint(0, 10000))
00070 b = copy.deepcopy(a)
00071 self.assertEquals(a.secs, b.secs)
00072 self.assertEquals(a.nsecs, b.nsecs)
00073
00074 import cPickle, cStringIO
00075 buff = cStringIO.StringIO()
00076 cPickle.dump(a, buff)
00077 buff.seek(0)
00078 c = cPickle.load(buff)
00079 self.assertEquals(a.secs, c.secs)
00080 self.assertEquals(a.nsecs, c.nsecs)
00081
00082 def test_Duration_get_setstate(self):
00083
00084 import copy, random
00085 a = rospy.Duration(random.randint(0, 10000), random.randint(0, 10000))
00086 b = copy.deepcopy(a)
00087 self.assertEquals(a.secs, b.secs)
00088 self.assertEquals(a.nsecs, b.nsecs)
00089
00090 import cPickle, cStringIO
00091 buff = cStringIO.StringIO()
00092 cPickle.dump(a, buff)
00093 buff.seek(0)
00094 c = cPickle.load(buff)
00095 self.assertEquals(a.secs, c.secs)
00096 self.assertEquals(a.nsecs, c.nsecs)
00097
00098 def test_Time(self):
00099
00100 from rospy import Time, Duration
00101
00102 failed = False
00103 try:
00104 v = Duration.from_sec(0.1) > Time.from_sec(0.5)
00105 failed = True
00106 except: pass
00107 self.failIf(failed, "should have failed to compare")
00108 try:
00109 v = Time.from_sec(0.4) > Duration.from_sec(0.1)
00110 failed = True
00111 except: pass
00112 self.failIf(failed, "should have failed to compare")
00113
00114
00115 try:
00116 Time(-1)
00117 failed = True
00118 except: pass
00119 self.failIf(failed, "negative time not allowed")
00120 try:
00121 Time(1, -1000000001)
00122 failed = True
00123 except: pass
00124 self.failIf(failed, "negative time not allowed")
00125
00126
00127 import time
00128 t = time.time()
00129 v = Time.from_sec(t)
00130 self.assertEquals(v.to_sec(), t)
00131
00132 self.assertEquals(Time.from_sec(0), Time())
00133 self.assertEquals(Time.from_sec(1.), Time(1))
00134 self.assertEquals(Time.from_sec(v.to_sec()), v)
00135 self.assertEquals(v.from_sec(v.to_sec()), v)
00136
00137
00138 self.assertEquals(v.to_sec(), v.to_time())
00139
00140
00141
00142 try:
00143 v = Time(1,0) + Time(1, 0)
00144 failed = True
00145 except: pass
00146 self.failIf(failed, "Time + Time must fail")
00147
00148
00149 v = Time(1,0) + Duration(1, 0)
00150 self.assertEquals(Time(2, 0), v)
00151 v = Duration(1, 0) + Time(1,0)
00152 self.assertEquals(Time(2, 0), v)
00153 v = Time(1,1) + Duration(1, 1)
00154 self.assertEquals(Time(2, 2), v)
00155 v = Duration(1, 1) + Time(1,1)
00156 self.assertEquals(Time(2, 2), v)
00157
00158 v = Time(1) + Duration(0, 1000000000)
00159 self.assertEquals(Time(2), v)
00160 v = Duration(1) + Time(0, 1000000000)
00161 self.assertEquals(Time(2), v)
00162
00163 v = Time(100, 100) + Duration(300)
00164 self.assertEquals(Time(400, 100), v)
00165 v = Duration(300) + Time(100, 100)
00166 self.assertEquals(Time(400, 100), v)
00167
00168 v = Time(100, 100) + Duration(300, 300)
00169 self.assertEquals(Time(400, 400), v)
00170 v = Duration(300, 300) + Time(100, 100)
00171 self.assertEquals(Time(400, 400), v)
00172
00173 v = Time(100, 100) + Duration(300, -101)
00174 self.assertEquals(Time(399, 999999999), v)
00175 v = Duration(300, -101) + Time(100, 100)
00176 self.assertEquals(Time(399, 999999999), v)
00177
00178
00179 try:
00180 v = Time(1,0) - 1
00181 failed = True
00182 except: pass
00183 self.failIf(failed, "Time - non Duration must fail")
00184 class Foob(object): pass
00185 try:
00186 v = Time(1,0) - Foob()
00187 failed = True
00188 except: pass
00189 self.failIf(failed, "Time - non TVal must fail")
00190
00191
00192 v = Time(1,0) - Duration(1, 0)
00193 self.assertEquals(Time(), v)
00194
00195 v = Time(1,1) - Duration(-1, -1)
00196 self.assertEquals(Time(2, 2), v)
00197 v = Time(1) - Duration(0, 1000000000)
00198 self.assertEquals(Time(), v)
00199 v = Time(2) - Duration(0, 1000000000)
00200 self.assertEquals(Time(1), v)
00201 v = Time(400, 100) - Duration(300)
00202 self.assertEquals(Time(100, 100), v)
00203 v = Time(100, 100) - Duration(0, 101)
00204 self.assertEquals(Time(99, 999999999), v)
00205
00206
00207 v = Time(100, 100) - Time(100, 100)
00208 self.assertEquals(Duration(), v)
00209 v = Time(100, 100) - Time(100)
00210 self.assertEquals(Duration(0,100), v)
00211 v = Time(100) - Time(200)
00212 self.assertEquals(Duration(-100), v)
00213
00214 def test_Duration(self):
00215 Duration = rospy.Duration
00216
00217
00218 v = Duration(1000)
00219 self.assertEquals(v, Duration.from_sec(v.to_sec()))
00220 self.assertEquals(v, v.from_sec(v.to_sec()))
00221 v = Duration(0,1000)
00222 self.assertEquals(v, Duration.from_sec(v.to_sec()))
00223 self.assertEquals(v, v.from_sec(v.to_sec()))
00224
00225
00226 v = -Duration(1, -1)
00227 self.assertEquals(-1, v.secs)
00228 self.assertEquals(1, v.nsecs)
00229 v = -Duration(-1, -1)
00230 self.assertEquals(1, v.secs)
00231 self.assertEquals(1, v.nsecs)
00232 v = -Duration(-1, 1)
00233 self.assertEquals(0, v.secs)
00234 self.assertEquals(999999999, v.nsecs)
00235
00236
00237 failed = False
00238 try:
00239 v = Duration(1,0) + Time(1, 0)
00240 failed = True
00241 except: pass
00242 self.failIf(failed, "Duration + Time must fail")
00243 try:
00244 v = Duration(1,0) + 1
00245 failed = True
00246 except: pass
00247 self.failIf(failed, "Duration + int must fail")
00248
00249 v = Duration(1,0) + Duration(1, 0)
00250 self.assertEquals(2, v.secs)
00251 self.assertEquals(0, v.nsecs)
00252 self.assertEquals(Duration(2, 0), v)
00253 v = Duration(-1,-1) + Duration(1, 1)
00254 self.assertEquals(0, v.secs)
00255 self.assertEquals(0, v.nsecs)
00256 self.assertEquals(Duration(), v)
00257 v = Duration(1) + Duration(0, 1000000000)
00258 self.assertEquals(2, v.secs)
00259 self.assertEquals(0, v.nsecs)
00260 self.assertEquals(Duration(2), v)
00261 v = Duration(100, 100) + Duration(300)
00262 self.assertEquals(Duration(400, 100), v)
00263 v = Duration(100, 100) + Duration(300, 300)
00264 self.assertEquals(Duration(400, 400), v)
00265 v = Duration(100, 100) + Duration(300, -101)
00266 self.assertEquals(Duration(399, 999999999), v)
00267
00268
00269 try:
00270 v = Duration(1,0) - 1
00271 failed = True
00272 except: pass
00273 self.failIf(failed, "Duration - non duration must fail")
00274 try:
00275 v = Duration(1, 0) - Time(1,0)
00276 failed = True
00277 except: pass
00278 self.failIf(failed, "Duration - Time must fail")
00279
00280 v = Duration(1,0) - Duration(1, 0)
00281 self.assertEquals(Duration(), v)
00282 v = Duration(-1,-1) - Duration(1, 1)
00283 self.assertEquals(Duration(-3, 999999998), v)
00284 v = Duration(1) - Duration(0, 1000000000)
00285 self.assertEquals(Duration(), v)
00286 v = Duration(2) - Duration(0, 1000000000)
00287 self.assertEquals(Duration(1), v)
00288 v = Duration(100, 100) - Duration(300)
00289 self.assertEquals(Duration(-200, 100), v)
00290 v = Duration(100, 100) - Duration(300, 101)
00291 self.assertEquals(Duration(-201, 999999999), v)
00292
00293
00294 self.assertEquals(abs(Duration()), Duration())
00295 self.assertEquals(abs(Duration(1)), Duration(1))
00296 self.assertEquals(abs(Duration(-1)), Duration(1))
00297 self.assertEquals(abs(Duration(0,-1)), Duration(0,1))
00298 self.assertEquals(abs(Duration(-1,-1)), Duration(1,1))
00299
00300 def test_set_rostime(self):
00301 from rospy.rostime import _set_rostime
00302 from rospy import Time
00303
00304 self.assertAlmostEqual(time.time(), rospy.get_time(), 1)
00305
00306 for t in [Time.from_sec(1.0), Time.from_sec(4.0)]:
00307 _set_rostime(t)
00308 self.assertEquals(t, rospy.get_rostime())
00309 self.assertEquals(t.to_time(), rospy.get_time())
00310
00311 def test_get_rostime(self):
00312 rospy.rostime.switch_to_wallclock()
00313 self.assertAlmostEqual(time.time(), rospy.get_time(), 1)
00314 self.assertAlmostEqual(time.time(), rospy.get_rostime().to_time(), 1)
00315
00316
00317 def test_sleep(self):
00318
00319 rospy.rostime.switch_to_wallclock()
00320 rospy.sleep(0.1)
00321 rospy.sleep(rospy.Duration.from_sec(0.1))
00322
00323 from rospy.rostime import _set_rostime
00324 from rospy import Time
00325
00326 t = Time.from_sec(1.0)
00327 _set_rostime(t)
00328 self.assertEquals(t, rospy.get_rostime())
00329 self.assertEquals(t.to_time(), rospy.get_time())
00330
00331 import thread
00332
00333
00334 self.failIf(test_sleep_done)
00335 thread.start_new_thread(sleeper, ())
00336 time.sleep(1.0)
00337 self.failIf(test_sleep_done)
00338
00339 t = Time.from_sec(1000000.0)
00340 _set_rostime(t)
00341 time.sleep(0.5)
00342 self.assert_(test_sleep_done, "sleeper did not wake up")
00343
00344
00345 self.failIf(test_duration_sleep_done)
00346 thread.start_new_thread(duration_sleeper, ())
00347 time.sleep(1.0)
00348 self.failIf(test_duration_sleep_done)
00349
00350 t = Time.from_sec(2000000.0)
00351 _set_rostime(t)
00352 time.sleep(0.5)
00353 self.assert_(test_sleep_done, "sleeper did not wake up")
00354
00355
00356 self.failIf(test_backwards_sleep_done)
00357 thread.start_new_thread(backwards_sleeper, ())
00358 time.sleep(1.0)
00359 self.failIf(test_backwards_sleep_done)
00360
00361 t = Time.from_sec(1.0)
00362 _set_rostime(t)
00363 time.sleep(0.5)
00364 self.assert_(test_backwards_sleep_done, "backwards sleeper was not given an exception")
00365
00366 test_duration_sleep_done = False
00367 def duration_sleeper():
00368 global test_duration_sleep_done
00369 rospy.sleep(rospy.Duration(10000.0))
00370 test_duration_sleep_done = True
00371
00372 test_sleep_done = False
00373 def sleeper():
00374 global test_sleep_done
00375 rospy.sleep(10000.0)
00376 test_sleep_done = True
00377 test_backwards_sleep_done = False
00378 def backwards_sleeper():
00379 global test_backwards_sleep_done
00380 try:
00381 rospy.sleep(10000.0)
00382 except rospy.ROSException:
00383 test_backwards_sleep_done = True
00384
00385 if __name__ == '__main__':
00386 rostest.unitrun('test_rospy', sys.argv[0], TestRospyTime, coverage_packages=['rospy.rostime', 'rospy.impl.simtime', 'rospy.client'])