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