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 <gtest/gtest.h>
00031 #include <tf/tf.h>
00032 #include <sys/time.h>
00033 #include "tf/LinearMath/Vector3.h"
00034 #include "tf/LinearMath/Matrix3x3.h"
00035
00036
00037 void seed_rand()
00038 {
00039
00040 timeval temp_time_struct;
00041 gettimeofday(&temp_time_struct,NULL);
00042 srand(temp_time_struct.tv_usec);
00043 };
00044
00045 using namespace tf;
00046
00047
00048 TEST(TimeCache, Repeatability)
00049 {
00050 unsigned int runs = 100;
00051
00052 seed_rand();
00053
00054 tf::TimeCache cache;
00055 std::vector<double> values(runs);
00056
00057 StampedTransform t;
00058 t.setIdentity();
00059
00060 for ( uint64_t i = 1; i < runs ; i++ )
00061 {
00062 values[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00063 std::stringstream ss;
00064 ss << values[i];
00065 t.frame_id_ = ss.str();
00066 t.stamp_ = ros::Time().fromNSec(i);
00067
00068 TransformStorage stor(t, i, 0);
00069
00070 cache.insertData(stor);
00071 }
00072
00073 TransformStorage out;
00074 for ( uint64_t i = 1; i < runs ; i++ )
00075 {
00076 cache.getData(ros::Time().fromNSec(i), out);
00077 EXPECT_EQ(out.frame_id_, i);
00078 EXPECT_EQ(out.stamp_, ros::Time().fromNSec(i));
00079 }
00080
00081 }
00082
00083
00084 TEST(TimeCache, RepeatabilityReverseInsertOrder)
00085 {
00086 unsigned int runs = 100;
00087
00088 seed_rand();
00089
00090 tf::TimeCache cache;
00091 std::vector<double> values(runs);
00092
00093 StampedTransform t;
00094 t.setIdentity();
00095
00096 for ( int i = runs -1; i >= 0 ; i-- )
00097 {
00098 values[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00099 t.stamp_ = ros::Time().fromNSec(i);
00100
00101 TransformStorage stor(t, i, 0);
00102
00103 cache.insertData(stor);
00104 }
00105
00106 TransformStorage out;
00107 for ( uint64_t i = 1; i < runs ; i++ )
00108 {
00109 cache.getData(ros::Time().fromNSec(i), out);
00110 EXPECT_EQ(out.frame_id_, i);
00111 EXPECT_EQ(out.stamp_, ros::Time().fromNSec(i));
00112 }
00113
00114 }
00115
00116 TEST(TimeCache, RepeatabilityRandomInsertOrder)
00117 {
00118 seed_rand();
00119
00120 tf::TimeCache cache;
00121 double my_vals[] = {13,2,5,4,9,7,3,11,15,14,12,1,6,10,0,8};
00122 std::vector<double> values (my_vals, my_vals + sizeof(my_vals)/sizeof(double));
00123 unsigned int runs = values.size();
00124
00125 StampedTransform t;
00126 t.setIdentity();
00127
00128 for ( uint64_t i = 0; i <runs ; i++ )
00129 {
00130 values[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00131 t.stamp_ = ros::Time().fromNSec(i);
00132
00133 TransformStorage stor(t, i, 0);
00134
00135 cache.insertData(stor);
00136 }
00137
00138 TransformStorage out;
00139 for ( uint64_t i = 1; i < runs ; i++ )
00140 {
00141 cache.getData(ros::Time().fromNSec(i), out);
00142 EXPECT_EQ(out.frame_id_, i);
00143 EXPECT_EQ(out.stamp_, ros::Time().fromNSec(i));
00144 }
00145 }
00146
00147 TEST(TimeCache, ZeroAtFront)
00148 {
00149 uint64_t runs = 100;
00150
00151 seed_rand();
00152
00153 tf::TimeCache cache;
00154 std::vector<double> values(runs);
00155
00156 StampedTransform t;
00157 t.setIdentity();
00158
00159 for ( uint64_t i = 0; i <runs ; i++ )
00160 {
00161 values[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00162 t.stamp_ = ros::Time().fromNSec(i);
00163
00164 TransformStorage stor(t, i, 0);
00165
00166 cache.insertData(stor);
00167 }
00168
00169 t.stamp_ = ros::Time().fromNSec(runs);
00170 TransformStorage stor(t, runs, 0);
00171 cache.insertData(stor);
00172
00173 for ( uint64_t i = 1; i < runs ; i++ )
00174 {
00175 cache.getData(ros::Time().fromNSec(i), stor);
00176 EXPECT_EQ(stor.frame_id_, i);
00177 EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i));
00178 }
00179
00180 cache.getData(ros::Time(), stor);
00181 EXPECT_EQ(stor.frame_id_, runs);
00182 EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(runs));
00183
00184
00185 t.stamp_ = ros::Time().fromNSec(runs+1);
00186 TransformStorage stor2(t, runs+1, 0);
00187 cache.insertData(stor2);
00188
00189
00190 cache.getData(ros::Time(), stor);
00191 EXPECT_EQ(stor.frame_id_, runs+1);
00192 EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(runs+1));
00193 }
00194
00195 TEST(TimeCache, CartesianInterpolation)
00196 {
00197 uint64_t runs = 100;
00198 double epsilon = 1e-6;
00199 seed_rand();
00200
00201 tf::TimeCache cache;
00202 std::vector<double> xvalues(2);
00203 std::vector<double> yvalues(2);
00204 std::vector<double> zvalues(2);
00205
00206 uint64_t offset = 200;
00207
00208 StampedTransform t;
00209 t.setIdentity();
00210
00211 for ( uint64_t i = 1; i < runs ; i++ )
00212 {
00213 for (uint64_t step = 0; step < 2 ; step++)
00214 {
00215 xvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00216 yvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00217 zvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00218
00219 t.setOrigin(tf::Vector3(xvalues[step], yvalues[step], zvalues[step]));
00220 t.stamp_ = ros::Time().fromNSec(step * 100 + offset);
00221 TransformStorage stor(t, 2, 0);
00222 cache.insertData(stor);
00223 }
00224
00225 TransformStorage out;
00226 for (int pos = 0; pos < 100 ; pos++)
00227 {
00228 cache.getData(ros::Time().fromNSec(offset + pos), out);
00229 double x_out = out.translation_.x();
00230 double y_out = out.translation_.y();
00231 double z_out = out.translation_.z();
00232 EXPECT_NEAR(xvalues[0] + (xvalues[1] - xvalues[0]) * (double)pos/100.0, x_out, epsilon);
00233 EXPECT_NEAR(yvalues[0] + (yvalues[1] - yvalues[0]) * (double)pos/100.0, y_out, epsilon);
00234 EXPECT_NEAR(zvalues[0] + (zvalues[1] - zvalues[0]) * (double)pos/100.0, z_out, epsilon);
00235 }
00236
00237
00238 cache.clearList();
00239 }
00240 }
00241
00242
00243
00245
00246
00247
00248
00249
00250
00251
00252
00253
00254
00255
00256
00257
00258
00259
00260
00261
00262
00263
00264
00265
00266
00267
00268
00269
00270
00271
00272
00273
00274
00275
00276
00277
00278
00279
00280
00281
00282
00283
00284
00285
00286
00287
00288
00289
00290
00291
00292
00293
00294
00295
00296
00297
00298
00299
00300
00301
00302
00303
00304
00305
00306
00307
00308
00309
00310
00311
00312
00313
00314
00315
00316
00317
00318
00319
00320
00321
00322
00323
00324
00325
00326
00327
00328
00329
00330
00331
00332
00333
00334
00335
00336
00337
00338
00339
00340
00341
00342 TEST(Bullet, Slerp)
00343 {
00344 uint64_t runs = 100;
00345 seed_rand();
00346
00347 tf::Quaternion q1, q2;
00348 q1.setEuler(0,0,0);
00349
00350 for (uint64_t i = 0 ; i < runs ; i++)
00351 {
00352 q2.setEuler(1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX,
00353 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX,
00354 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX);
00355
00356
00357 tf::Quaternion q3 = slerp(q1,q2,0.5);
00358
00359 EXPECT_NEAR(q3.angle(q1), q2.angle(q3), 1e-5);
00360 }
00361
00362 }
00363
00364
00365 TEST(TimeCache, AngularInterpolation)
00366 {
00367 uint64_t runs = 100;
00368 double epsilon = 1e-6;
00369 seed_rand();
00370
00371 tf::TimeCache cache;
00372 std::vector<double> yawvalues(2);
00373 std::vector<double> pitchvalues(2);
00374 std::vector<double> rollvalues(2);
00375 uint64_t offset = 200;
00376
00377 std::vector<tf::Quaternion> quats(2);
00378
00379 StampedTransform t;
00380 t.setIdentity();
00381
00382 for ( uint64_t i = 1; i < runs ; i++ )
00383 {
00384 for (uint64_t step = 0; step < 2 ; step++)
00385 {
00386 yawvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX / 100.0;
00387 pitchvalues[step] = 0;
00388 rollvalues[step] = 0;
00389 quats[step].setRPY(yawvalues[step], pitchvalues[step], rollvalues[step]);
00390 t.setRotation(quats[step]);
00391 t.stamp_ = ros::Time().fromNSec(offset + (step * 100));
00392 TransformStorage stor(t, 3, 0);
00393 cache.insertData(stor);
00394 }
00395
00396 TransformStorage out;
00397 for (int pos = 0; pos < 100 ; pos ++)
00398 {
00399 cache.getData(ros::Time().fromNSec(offset + pos), out);
00400 tf::Quaternion quat = out.rotation_;
00401
00402
00403 tf::Quaternion ground_truth = quats[0].slerp(quats[1], pos/100.0);
00404
00405
00406 EXPECT_NEAR(0, angle(ground_truth, quat), epsilon);
00407 }
00408
00409 cache.clearList();
00410 }
00411 }
00412
00413 TEST(TimeCache, DuplicateEntries)
00414 {
00415
00416 TimeCache cache;
00417
00418 StampedTransform t;
00419 t.setIdentity();
00420 t.stamp_ = ros::Time().fromNSec(1);
00421 TransformStorage stor(t, 3, 0);
00422 cache.insertData(stor);
00423 cache.insertData(stor);
00424
00425
00426 cache.getData(ros::Time().fromNSec(1), stor);
00427
00428 EXPECT_TRUE(!std::isnan(stor.translation_.x()));
00429 EXPECT_TRUE(!std::isnan(stor.translation_.y()));
00430 EXPECT_TRUE(!std::isnan(stor.translation_.z()));
00431 EXPECT_TRUE(!std::isnan(stor.rotation_.x()));
00432 EXPECT_TRUE(!std::isnan(stor.rotation_.y()));
00433 EXPECT_TRUE(!std::isnan(stor.rotation_.z()));
00434 EXPECT_TRUE(!std::isnan(stor.rotation_.w()));
00435 }
00436
00437
00438 int main(int argc, char **argv){
00439 testing::InitGoogleTest(&argc, argv);
00440 return RUN_ALL_TESTS();
00441 }