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 #include <vector>
00031
00032 #include <gtest/gtest.h>
00033 #include <ros/rate.h>
00034 #include <ros/time.h>
00035 #include <sys/time.h>
00036
00037 #include <boost/date_time/posix_time/ptime.hpp>
00038
00039 using namespace ros;
00040
00042
00043 double epsilon = 1e-9;
00044
00045 void seed_rand()
00046 {
00047
00048 timeval temp_time_struct;
00049 gettimeofday(&temp_time_struct,NULL);
00050 srand(temp_time_struct.tv_usec);
00051 };
00052
00053 void generate_rand_times(uint32_t range, uint64_t runs, std::vector<ros::Time>& values1, std::vector<ros::Time>& values2)
00054 {
00055 seed_rand();
00056 values1.clear();
00057 values2.clear();
00058 values1.reserve(runs);
00059 values2.reserve(runs);
00060 for ( uint32_t i = 0; i < runs ; i++ )
00061 {
00062 values1.push_back(ros::Time( (rand() * range / RAND_MAX), (rand() * 1000000000ULL/RAND_MAX)));
00063 values2.push_back(ros::Time( (rand() * range / RAND_MAX), (rand() * 1000000000ULL/RAND_MAX)));
00064 }
00065 }
00066
00067 void generate_rand_durations(uint32_t range, uint64_t runs, std::vector<ros::Duration>& values1, std::vector<ros::Duration>& values2)
00068 {
00069 seed_rand();
00070 values1.clear();
00071 values2.clear();
00072 values1.reserve(runs * 4);
00073 values2.reserve(runs * 4);
00074 for ( uint32_t i = 0; i < runs ; i++ )
00075 {
00076
00077 values1.push_back(ros::Duration( (rand() * range / RAND_MAX), (rand() * 1000000000ULL/RAND_MAX)));
00078 values2.push_back(ros::Duration( (rand() * range / RAND_MAX), (rand() * 1000000000ULL/RAND_MAX)));
00079 EXPECT_GE(values1.back(), ros::Duration(0,0));
00080 EXPECT_GE(values2.back(), ros::Duration(0,0));
00081
00082
00083 values1.push_back(ros::Duration( -(rand() * range / RAND_MAX), -(rand() * 1000000000ULL/RAND_MAX)));
00084 values2.push_back(ros::Duration( -(rand() * range / RAND_MAX), -(rand() * 1000000000ULL/RAND_MAX)));
00085 EXPECT_LE(values1.back(), ros::Duration(0,0));
00086 EXPECT_LE(values2.back(), ros::Duration(0,0));
00087
00088
00089 values1.push_back(ros::Duration( (rand() * range / RAND_MAX), (rand() * 1000000000ULL/RAND_MAX)));
00090 values2.push_back(ros::Duration( -(rand() * range / RAND_MAX), -(rand() * 1000000000ULL/RAND_MAX)));
00091 EXPECT_GE(values1.back(), ros::Duration(0,0));
00092 EXPECT_LE(values2.back(), ros::Duration(0,0));
00093
00094
00095 values1.push_back(ros::Duration( -(rand() * range / RAND_MAX), -(rand() * 1000000000ULL/RAND_MAX)));
00096 values2.push_back(ros::Duration( (rand() * range / RAND_MAX), (rand() * 1000000000ULL/RAND_MAX)));
00097 EXPECT_LE(values1.back(), ros::Duration(0,0));
00098 EXPECT_GE(values2.back(), ros::Duration(0,0));
00099 }
00100 }
00101
00102 TEST(Time, size)
00103 {
00104 ASSERT_EQ(sizeof(Time), 8);
00105 ASSERT_EQ(sizeof(Duration), 8);
00106 }
00107
00108 TEST(Time, Comparitors)
00109 {
00110 std::vector<ros::Time> v1;
00111 std::vector<ros::Time> v2;
00112 generate_rand_times(100, 1000, v1,v2);
00113
00114 for (uint32_t i = 0; i < v1.size(); i++)
00115 {
00116 if (v1[i].sec * 1000000000ULL + v1[i].nsec < v2[i].sec * 1000000000ULL + v2[i].nsec)
00117 {
00118 EXPECT_LT(v1[i], v2[i]);
00119
00120
00121 EXPECT_LE(v1[i], v2[i]);
00122 EXPECT_NE(v1[i], v2[i]);
00123 }
00124 else if (v1[i].sec * 1000000000ULL + v1[i].nsec > v2[i].sec * 1000000000ULL + v2[i].nsec)
00125 {
00126 EXPECT_GT(v1[i], v2[i]);
00127 EXPECT_GE(v1[i], v2[i]);
00128 EXPECT_NE(v1[i], v2[i]);
00129 }
00130 else
00131 {
00132 EXPECT_EQ(v1[i], v2[i]);
00133 EXPECT_LE(v1[i], v2[i]);
00134 EXPECT_GE(v1[i], v2[i]);
00135 }
00136
00137 }
00138
00139 }
00140
00141 TEST(Time, ToFromDouble)
00142 {
00143 std::vector<ros::Time> v1;
00144 std::vector<ros::Time> v2;
00145 generate_rand_times(100, 1000, v1,v2);
00146
00147 for (uint32_t i = 0; i < v1.size(); i++)
00148 {
00149 EXPECT_EQ(v1[i].toSec(), v1[i].fromSec(v1[i].toSec()).toSec());
00150
00151 }
00152
00153 }
00154
00155 TEST(Time, RoundingError)
00156 {
00157 double someInt = 1031.0;
00158 double t = std::nextafter(someInt, 0);
00159
00160
00161 ros::Time exampleTime;
00162 exampleTime.fromSec(t);
00163
00164
00165
00166 EXPECT_EQ(exampleTime.sec, 1031);
00167 EXPECT_EQ(exampleTime.nsec, 0);
00168 }
00169
00170 TEST(Time, OperatorPlus)
00171 {
00172 Time t(100, 0);
00173 Duration d(100, 0);
00174 Time r = t + d;
00175 EXPECT_EQ(r.sec, 200UL);
00176 EXPECT_EQ(r.nsec, 0UL);
00177
00178 t = Time(0, 100000UL);
00179 d = Duration(0, 100UL);
00180 r = t + d;
00181 EXPECT_EQ(r.sec, 0UL);
00182 EXPECT_EQ(r.nsec, 100100UL);
00183
00184 t = Time(0, 0);
00185 d = Duration(10, 2000003000UL);
00186 r = t + d;
00187 EXPECT_EQ(r.sec, 12UL);
00188 EXPECT_EQ(r.nsec, 3000UL);
00189 }
00190
00191 TEST(Time, OperatorMinus)
00192 {
00193 Time t(100, 0);
00194 Duration d(100, 0);
00195 Time r = t - d;
00196 EXPECT_EQ(r.sec, 0UL);
00197 EXPECT_EQ(r.nsec, 0UL);
00198
00199 t = Time(0, 100000UL);
00200 d = Duration(0, 100UL);
00201 r = t - d;
00202 EXPECT_EQ(r.sec, 0UL);
00203 EXPECT_EQ(r.nsec, 99900UL);
00204
00205 t = Time(30, 0);
00206 d = Duration(10, 2000003000UL);
00207 r = t - d;
00208 EXPECT_EQ(r.sec, 17UL);
00209 EXPECT_EQ(r.nsec, 999997000ULL);
00210 }
00211
00212 TEST(Time, OperatorPlusEquals)
00213 {
00214 Time t(100, 0);
00215 Duration d(100, 0);
00216 t += d;
00217 EXPECT_EQ(t.sec, 200UL);
00218 EXPECT_EQ(t.nsec, 0UL);
00219
00220 t = Time(0, 100000UL);
00221 d = Duration(0, 100UL);
00222 t += d;
00223 EXPECT_EQ(t.sec, 0UL);
00224 EXPECT_EQ(t.nsec, 100100UL);
00225
00226 t = Time(0, 0);
00227 d = Duration(10, 2000003000UL);
00228 t += d;
00229 EXPECT_EQ(t.sec, 12UL);
00230 EXPECT_EQ(t.nsec, 3000UL);
00231 }
00232
00233 TEST(Time, OperatorMinusEquals)
00234 {
00235 Time t(100, 0);
00236 Duration d(100, 0);
00237 t -= d;
00238 EXPECT_EQ(t.sec, 0UL);
00239 EXPECT_EQ(t.nsec, 0UL);
00240
00241 t = Time(0, 100000UL);
00242 d = Duration(0, 100UL);
00243 t -= d;
00244 EXPECT_EQ(t.sec, 0UL);
00245 EXPECT_EQ(t.nsec, 99900UL);
00246
00247 t = Time(30, 0);
00248 d = Duration(10, 2000003000UL);
00249 t -= d;
00250 EXPECT_EQ(t.sec, 17UL);
00251 EXPECT_EQ(t.nsec, 999997000ULL);
00252 }
00253
00254 TEST(Time, SecNSecConstructor)
00255 {
00256 Time t(100, 2000003000UL);
00257 EXPECT_EQ(t.sec, 102UL);
00258 EXPECT_EQ(t.nsec, 3000UL);
00259 }
00260
00261 TEST(Time, DontMungeStreamState)
00262 {
00263 std::ostringstream oss;
00264 Time t(100, 2000003000UL);
00265 oss << std::setfill('N');
00266 oss << std::setw(13);
00267 oss << t;
00268
00269 EXPECT_EQ(oss.width(), 13);
00270 EXPECT_EQ(oss.fill(), 'N');
00271 }
00272
00273 TEST(Time, ToFromBoost)
00274 {
00275 std::vector<ros::Time> v1;
00276 std::vector<ros::Time> v2;
00277 generate_rand_times(100, 1000, v1,v2);
00278
00279 for (uint32_t i = 0; i < v1.size(); i++)
00280 {
00281 Time t = v1[i];
00282
00283 t.nsec = uint32_t(t.nsec / 1000.0) * 1000;
00284 boost::posix_time::ptime b = t.toBoost();
00285 Time tt = Time::fromBoost(b);
00286 EXPECT_EQ(t, tt);
00287 }
00288 }
00289
00290
00291
00292 TEST(Duration, Comparitors)
00293 {
00294 std::vector<ros::Duration> v1;
00295 std::vector<ros::Duration> v2;
00296 generate_rand_durations(100, 1000, v1,v2);
00297
00298 for (uint32_t i = 0; i < v1.size(); i++)
00299 {
00300 if (v1[i].sec * 1000000000LL + v1[i].nsec < v2[i].sec * 1000000000LL + v2[i].nsec)
00301 {
00302 EXPECT_LT(v1[i], v2[i]);
00303
00304
00305 EXPECT_LE(v1[i], v2[i]);
00306 EXPECT_NE(v1[i], v2[i]);
00307 }
00308 else if (v1[i].sec * 1000000000LL + v1[i].nsec > v2[i].sec * 1000000000LL + v2[i].nsec)
00309 {
00310 EXPECT_GT(v1[i], v2[i]);
00311
00312
00313 EXPECT_GE(v1[i], v2[i]);
00314 EXPECT_NE(v1[i], v2[i]);
00315 }
00316 else
00317 {
00318 EXPECT_EQ(v1[i], v2[i]);
00319 EXPECT_LE(v1[i], v2[i]);
00320 EXPECT_GE(v1[i], v2[i]);
00321 }
00322
00323 }
00324 }
00325
00326 TEST(Duration, ToFromSec)
00327 {
00328 std::vector<ros::Duration> v1;
00329 std::vector<ros::Duration> v2;
00330 generate_rand_durations(100, 1000, v1,v2);
00331
00332 for (uint32_t i = 0; i < v1.size(); i++)
00333 {
00334 EXPECT_EQ(v1[i].toSec(), v1[i].fromSec(v1[i].toSec()).toSec());
00335 EXPECT_GE(ros::Duration(v1[i].toSec()).nsec, 0);
00336 }
00337
00338 EXPECT_EQ(ros::Duration(-0.5), ros::Duration(-1LL, 500000000LL));
00339 EXPECT_EQ(ros::Duration(-0.5), ros::Duration(0, -500000000LL));
00340 }
00341
00342
00343 TEST(Duration, OperatorPlus)
00344 {
00345 std::vector<ros::Duration> v1;
00346 std::vector<ros::Duration> v2;
00347 generate_rand_durations(100, 1000, v1,v2);
00348
00349 for (uint32_t i = 0; i < v1.size(); i++)
00350 {
00351 EXPECT_NEAR(v1[i].toSec() + v2[i].toSec(), (v1[i] + v2[i]).toSec(), epsilon);
00352 ros::Duration temp = v1[i];
00353 EXPECT_NEAR(v1[i].toSec() + v2[i].toSec(), (temp += v2[i]).toSec(), epsilon);
00354
00355 }
00356
00357 }
00358
00359 TEST(Duration, OperatorMinus)
00360 {
00361 std::vector<ros::Duration> v1;
00362 std::vector<ros::Duration> v2;
00363 generate_rand_durations(100, 1000, v1,v2);
00364
00365 for (uint32_t i = 0; i < v1.size(); i++)
00366 {
00367 EXPECT_NEAR(v1[i].toSec() - v2[i].toSec(), (v1[i] - v2[i]).toSec(), epsilon);
00368 ros::Duration temp = v1[i];
00369 EXPECT_NEAR(v1[i].toSec() - v2[i].toSec(), (temp -= v2[i]).toSec(), epsilon);
00370
00371 EXPECT_NEAR(- v2[i].toSec(), (-v2[i]).toSec(), epsilon);
00372
00373 }
00374
00375 }
00376
00377 TEST(Duration, OperatorTimes)
00378 {
00379 std::vector<ros::Duration> v1;
00380 std::vector<ros::Duration> v2;
00381 generate_rand_durations(100, 1000, v1,v2);
00382
00383 for (uint32_t i = 0; i < v1.size(); i++)
00384 {
00385 EXPECT_NEAR(v1[i].toSec() * v2[i].toSec(), (v1[i] * v2[i].toSec()).toSec(), epsilon);
00386 ros::Duration temp = v1[i];
00387 EXPECT_NEAR(v1[i].toSec() * v2[i].toSec(), (temp *= v2[i].toSec()).toSec(), epsilon);
00388
00389 }
00390
00391 }
00392
00393 TEST(Duration, OperatorPlusEquals)
00394 {
00395 Duration t(100, 0);
00396 Duration d(100, 0);
00397 t += d;
00398 EXPECT_EQ(t.sec, 200L);
00399 EXPECT_EQ(t.nsec, 0L);
00400
00401 t = Duration(0, 100000L);
00402 d = Duration(0, 100L);
00403 t += d;
00404 EXPECT_EQ(t.sec, 0L);
00405 EXPECT_EQ(t.nsec, 100100L);
00406
00407 t = Duration(0, 0);
00408 d = Duration(10, 2000003000L);
00409 t += d;
00410 EXPECT_EQ(t.sec, 12L);
00411 EXPECT_EQ(t.nsec, 3000L);
00412 }
00413
00414 TEST(Duration, OperatorMinusEquals)
00415 {
00416 Duration t(100, 0);
00417 Duration d(100, 0);
00418 t -= d;
00419 EXPECT_EQ(t.sec, 0L);
00420 EXPECT_EQ(t.nsec, 0L);
00421
00422 t = Duration(0, 100000L);
00423 d = Duration(0, 100L);
00424 t -= d;
00425 EXPECT_EQ(t.sec, 0L);
00426 EXPECT_EQ(t.nsec, 99900L);
00427
00428 t = Duration(30, 0);
00429 d = Duration(10, 2000003000L);
00430 t -= d;
00431 EXPECT_EQ(t.sec, 17L);
00432 EXPECT_EQ(t.nsec, 999997000L);
00433 }
00434
00435 void alarmHandler(int sig)
00436 {
00437
00438 }
00439
00440 TEST(Duration, sleepWithSignal)
00441 {
00442 signal(SIGALRM, alarmHandler);
00443 alarm(1);
00444 Time start = Time::now();
00445 Duration d(2.0);
00446 d.sleep();
00447 Time end = Time::now();
00448
00449 ASSERT_GT(end - start, d);
00450 }
00451
00452 TEST(Rate, constructFromDuration){
00453 Duration d(4, 0);
00454 Rate r(d);
00455 EXPECT_EQ(r.expectedCycleTime(), d);
00456 }
00457
00458 TEST(Rate, sleep_return_value_true){
00459 Rate r(Duration(0.2));
00460 Duration(r.expectedCycleTime() * 0.5).sleep();
00461 EXPECT_TRUE(r.sleep());
00462 }
00463
00464 TEST(Rate, sleep_return_value_false){
00465 Rate r(Duration(0.2));
00466 Duration(r.expectedCycleTime() * 2).sleep();
00467 EXPECT_FALSE(r.sleep());
00468 }
00469
00470 TEST(WallRate, constructFromDuration){
00471 Duration d(4, 0);
00472 WallRate r(d);
00473 WallDuration wd(4, 0);
00474 EXPECT_EQ(r.expectedCycleTime(), wd);
00475 }
00476
00478
00480
00481
00482 int main(int argc, char **argv){
00483 testing::InitGoogleTest(&argc, argv);
00484 ros::Time::init();
00485 return RUN_ALL_TESTS();
00486 }