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