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 <ros/ros.h>
00034 #include "LinearMath/btVector3.h"
00035
00036 void seed_rand()
00037 {
00038
00039 timeval temp_time_struct;
00040 gettimeofday(&temp_time_struct,NULL);
00041 srand(temp_time_struct.tv_usec);
00042 };
00043
00044 void generate_rand_vectors(double scale, uint64_t runs, std::vector<double>& xvalues, std::vector<double>& yvalues, std::vector<double>&zvalues)
00045 {
00046 seed_rand();
00047 for ( uint64_t i = 0; i < runs ; i++ )
00048 {
00049 xvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00050 yvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00051 zvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00052 }
00053 }
00054
00055
00056 using namespace tf;
00057
00058 TEST(tf, setTransformNoInsertOnSelfTransform)
00059 {
00060 tf::Transformer mTR(true);
00061 StampedTransform tranStamped(btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(10.0), "same_frame", "same_frame");
00062 EXPECT_FALSE(mTR.setTransform(tranStamped));
00063 }
00064
00065 TEST(tf, setTransformNoInsertWithNan)
00066 {
00067 tf::Transformer mTR(true);
00068 StampedTransform tranStamped(btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(10.0), "same_frame", "other_frame");
00069 EXPECT_TRUE(mTR.setTransform(tranStamped));
00070
00071 tranStamped.setOrigin(tf::Point(1.0,1.0,0.0/0.0));
00072 EXPECT_TRUE(std::isnan(tranStamped.getOrigin().z()));
00073 EXPECT_FALSE(mTR.setTransform(tranStamped));
00074
00075 }
00076
00077 TEST(tf, setTransformNoInsertWithNoFrameID)
00078 {
00079 tf::Transformer mTR(true);
00080 StampedTransform tranStamped(btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(10.0), "parent_frame", "");
00081 EXPECT_FALSE(mTR.setTransform(tranStamped));
00082 }
00083
00084 TEST(tf, setTransformNoInsertWithNoParentID)
00085 {
00086 tf::Transformer mTR(true);
00087 StampedTransform tranStamped(btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(10.0), "", "my_frame");
00088 EXPECT_FALSE(mTR.setTransform(tranStamped));
00089 }
00090
00091 TEST(tf, TransformTransformsCartesian)
00092 {
00093 uint64_t runs = 400;
00094 double epsilon = 1e-6;
00095 seed_rand();
00096
00097 tf::Transformer mTR(true);
00098 std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
00099 for ( uint64_t i = 0; i < runs ; i++ )
00100 {
00101 xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00102 yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00103 zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00104
00105 StampedTransform tranStamped(btTransform(btQuaternion(0,0,0), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10.0 + i), "my_parent", "child");
00106 mTR.setTransform(tranStamped);
00107
00108 }
00109
00110
00111
00112
00113 for ( uint64_t i = 0; i < runs ; i++ )
00114
00115 {
00116 Stamped<Pose> inpose (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(10.0 + i), "child");
00117
00118 try{
00119 Stamped<Pose> outpose;
00120 outpose.setIdentity();
00121 mTR.transformPose("my_parent",inpose, outpose);
00122 EXPECT_NEAR(outpose.getOrigin().x(), xvalues[i], epsilon);
00123 EXPECT_NEAR(outpose.getOrigin().y(), yvalues[i], epsilon);
00124 EXPECT_NEAR(outpose.getOrigin().z(), zvalues[i], epsilon);
00125 }
00126 catch (tf::TransformException & ex)
00127 {
00128 std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl;
00129 bool exception_improperly_thrown = true;
00130 EXPECT_FALSE(exception_improperly_thrown);
00131 }
00132 }
00133
00134 Stamped<Pose> inpose (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(runs), "child");
00135 Stamped<Pose> outpose;
00136 outpose.setIdentity();
00137 mTR.transformPose("child",inpose, outpose);
00138 EXPECT_NEAR(outpose.getOrigin().x(), 0, epsilon);
00139 EXPECT_NEAR(outpose.getOrigin().y(), 0, epsilon);
00140 EXPECT_NEAR(outpose.getOrigin().z(), 0, epsilon);
00141
00142
00143 }
00144
00147 TEST(tf, TransformTransformToOwnFrame)
00148 {
00149 uint64_t runs = 400;
00150 double epsilon = 1e-6;
00151 seed_rand();
00152
00153 tf::Transformer mTR(true);
00154 std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs), yawvalues(runs), pitchvalues(runs), rollvalues(runs);
00155 for ( uint64_t i = 0; i < runs ; i++ )
00156 {
00157 xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00158 yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00159 zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00160 yawvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00161 pitchvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00162 rollvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00163
00164 StampedTransform tranStamped(btTransform(btQuaternion(yawvalues[i],pitchvalues[i],rollvalues[i]), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i), "parent", "child");
00165 mTR.setTransform(tranStamped);
00166
00167 }
00168
00169
00170
00171
00172 for ( uint64_t i = 0; i < runs ; i++ )
00173
00174 {
00175 Stamped<Pose> inpose (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(10 + i), "child");
00176 Stamped<Pose> inpose2 (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(10 + i), "parent");
00177
00178 try{
00179 Stamped<Pose> outpose;
00180 outpose.setIdentity();
00181 mTR.transformPose("child",inpose, outpose);
00182 EXPECT_NEAR(outpose.getOrigin().x(), 0, epsilon);
00183 EXPECT_NEAR(outpose.getOrigin().y(), 0, epsilon);
00184 EXPECT_NEAR(outpose.getOrigin().z(), 0, epsilon);
00185 EXPECT_NEAR(outpose.getRotation().w(), 1, epsilon);
00186
00187
00188 outpose.setIdentity();
00189 mTR.transformPose("parent",inpose2, outpose);
00190 EXPECT_NEAR(outpose.getOrigin().x(), 0, epsilon);
00191 EXPECT_NEAR(outpose.getOrigin().y(), 0, epsilon);
00192 EXPECT_NEAR(outpose.getOrigin().z(), 0, epsilon);
00193 EXPECT_NEAR(outpose.getRotation().w(), 1, epsilon);
00194 }
00195 catch (tf::TransformException & ex)
00196 {
00197 std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl;
00198 bool exception_improperly_thrown = true;
00199 EXPECT_FALSE(exception_improperly_thrown);
00200 }
00201 }
00202
00203 Stamped<Pose> inpose (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(runs), "child");
00204 Stamped<Pose> outpose;
00205 outpose.setIdentity();
00206 mTR.transformPose("child",inpose, outpose);
00207 EXPECT_NEAR(outpose.getOrigin().x(), 0, epsilon);
00208 EXPECT_NEAR(outpose.getOrigin().y(), 0, epsilon);
00209 EXPECT_NEAR(outpose.getOrigin().z(), 0, epsilon);
00210
00211
00212 }
00213
00214 TEST(tf, TransformPointCartesian)
00215 {
00216 uint64_t runs = 400;
00217 double epsilon = 1e-6;
00218 seed_rand();
00219
00220 tf::Transformer mTR(true);
00221 std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
00222 for ( uint64_t i = 0; i < runs ; i++ )
00223 {
00224 xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00225 yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00226 zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00227
00228 StampedTransform tranStamped(btTransform(btQuaternion(0,0,0), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i), "my_parent", "child");
00229 mTR.setTransform(tranStamped);
00230
00231 }
00232
00233
00234
00235
00236 for ( uint64_t i = 0; i < runs ; i++ )
00237
00238 {
00239 double x =10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00240 double y =10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00241 double z =10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00242 Stamped<Point> invec (btVector3(x,y,z), ros::Time().fromNSec(10 + i), "child");
00243
00244 try{
00245 Stamped<Point> outvec(btVector3(0,0,0), ros::Time().fromNSec(10 + i), "child");
00246
00247 mTR.transformPoint("my_parent",invec, outvec);
00248 EXPECT_NEAR(outvec.x(), xvalues[i]+x, epsilon);
00249 EXPECT_NEAR(outvec.y(), yvalues[i]+y, epsilon);
00250 EXPECT_NEAR(outvec.z(), zvalues[i]+z, epsilon);
00251 }
00252 catch (tf::TransformException & ex)
00253 {
00254 std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl;
00255 bool exception_improperly_thrown = true;
00256 EXPECT_FALSE(exception_improperly_thrown);
00257 }
00258 }
00259
00260 }
00261
00262 TEST(tf, TransformVectorCartesian)
00263 {
00264 uint64_t runs = 400;
00265 double epsilon = 1e-6;
00266 seed_rand();
00267
00268 tf::Transformer mTR(true);
00269 std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
00270 for ( uint64_t i = 0; i < runs ; i++ )
00271 {
00272 xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00273 yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00274 zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00275
00276 StampedTransform tranStamped(btTransform(btQuaternion(0,0,0), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i), "my_parent", "child");
00277 mTR.setTransform(tranStamped);
00278
00279 }
00280
00281
00282
00283
00284 for ( uint64_t i = 0; i < runs ; i++ )
00285
00286 {
00287 double x =10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00288 double y =10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00289 double z =10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00290 Stamped<Point> invec (btVector3(x,y,z), ros::Time().fromNSec(10 + i), "child");
00291
00292 try{
00293 Stamped<Vector3> outvec(btVector3(0,0,0), ros::Time().fromNSec(10 + i), "child");
00294
00295 mTR.transformVector("my_parent",invec, outvec);
00296 EXPECT_NEAR(outvec.x(), x, epsilon);
00297 EXPECT_NEAR(outvec.y(), y, epsilon);
00298 EXPECT_NEAR(outvec.z(), z, epsilon);
00299 }
00300 catch (tf::TransformException & ex)
00301 {
00302 std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl;
00303 bool exception_improperly_thrown = true;
00304 EXPECT_FALSE(exception_improperly_thrown);
00305 }
00306 }
00307
00308 }
00309
00310 TEST(tf, TransformQuaternionCartesian)
00311 {
00312 uint64_t runs = 400;
00313 double epsilon = 1e-6;
00314 seed_rand();
00315
00316 tf::Transformer mTR(true);
00317 std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
00318 for ( uint64_t i = 0; i < runs ; i++ )
00319 {
00320 xvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00321 yvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00322 zvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00323
00324
00325 StampedTransform tranStamped(btTransform(btQuaternion(0,0,0), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i), "my_parent", "child");
00326 mTR.setTransform(tranStamped);
00327
00328 }
00329
00330
00331
00332
00333 for ( uint64_t i = 0; i < runs ; i++ )
00334
00335 {
00336 Stamped<btQuaternion> invec (btQuaternion(xvalues[i],yvalues[i],zvalues[i]), ros::Time().fromNSec(10 + i), "child");
00337
00338
00339 try{
00340 Stamped<btQuaternion> outvec(btQuaternion(xvalues[i],yvalues[i],zvalues[i]), ros::Time().fromNSec(10 + i), "child");
00341
00342 mTR.transformQuaternion("my_parent",invec, outvec);
00343 EXPECT_NEAR(outvec.angle(invec) , 0, epsilon);
00344 }
00345 catch (tf::TransformException & ex)
00346 {
00347 std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl;
00348 bool exception_improperly_thrown = true;
00349 EXPECT_FALSE(exception_improperly_thrown);
00350 }
00351 }
00352
00353 }
00354
00355 TEST(data, Vector3Conversions)
00356 {
00357
00358 uint64_t runs = 400;
00359 double epsilon = 1e-6;
00360
00361 std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
00362 generate_rand_vectors(1.0, runs, xvalues, yvalues,zvalues);
00363
00364 for ( uint64_t i = 0; i < runs ; i++ )
00365 {
00366 btVector3 btv = btVector3(xvalues[i], yvalues[i], zvalues[i]);
00367 btVector3 btv_out = btVector3(0,0,0);
00368 geometry_msgs::Vector3 msgv;
00369 vector3TFToMsg(btv, msgv);
00370 vector3MsgToTF(msgv, btv_out);
00371 EXPECT_NEAR(btv.x(), btv_out.x(), epsilon);
00372 EXPECT_NEAR(btv.y(), btv_out.y(), epsilon);
00373 EXPECT_NEAR(btv.z(), btv_out.z(), epsilon);
00374 }
00375
00376 }
00377
00378 TEST(data, Vector3StampedConversions)
00379 {
00380
00381 uint64_t runs = 400;
00382 double epsilon = 1e-6;
00383
00384 std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
00385 generate_rand_vectors(1.0, runs, xvalues, yvalues,zvalues);
00386
00387 for ( uint64_t i = 0; i < runs ; i++ )
00388 {
00389 Stamped<btVector3> btv = Stamped<btVector3>(btVector3(xvalues[i], yvalues[i], zvalues[i]), ros::Time().fromNSec(1), "no frame");
00390 Stamped<btVector3> btv_out;
00391 geometry_msgs::Vector3Stamped msgv;
00392 vector3StampedTFToMsg(btv, msgv);
00393 vector3StampedMsgToTF(msgv, btv_out);
00394 EXPECT_NEAR(btv.x(), btv_out.x(), epsilon);
00395 EXPECT_NEAR(btv.y(), btv_out.y(), epsilon);
00396 EXPECT_NEAR(btv.z(), btv_out.z(), epsilon);
00397 EXPECT_STREQ(btv.frame_id_.c_str(), btv_out.frame_id_.c_str());
00398 EXPECT_EQ(btv.stamp_, btv_out.stamp_);
00399 }
00400 }
00401
00402 TEST(data, QuaternionConversions)
00403 {
00404
00405 uint64_t runs = 400;
00406 double epsilon = 1e-6;
00407
00408 std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
00409 generate_rand_vectors(1.0, runs, xvalues, yvalues,zvalues);
00410
00411 for ( uint64_t i = 0; i < runs ; i++ )
00412 {
00413 btQuaternion btv = btQuaternion(xvalues[i], yvalues[i], zvalues[i]);
00414 btQuaternion btv_out = btQuaternion(0,0,0);
00415 geometry_msgs::Quaternion msgv;
00416 quaternionTFToMsg(btv, msgv);
00417 quaternionMsgToTF(msgv, btv_out);
00418 EXPECT_NEAR(btv.x(), btv_out.x(), epsilon);
00419 EXPECT_NEAR(btv.y(), btv_out.y(), epsilon);
00420 EXPECT_NEAR(btv.z(), btv_out.z(), epsilon);
00421 EXPECT_NEAR(btv.w(), btv_out.w(), epsilon);
00422 }
00423
00424 }
00425
00426 TEST(data, QuaternionStampedConversions)
00427 {
00428
00429 uint64_t runs = 400;
00430 double epsilon = 1e-6;
00431
00432 std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
00433 generate_rand_vectors(1.0, runs, xvalues, yvalues,zvalues);
00434
00435 for ( uint64_t i = 0; i < runs ; i++ )
00436 {
00437 Stamped<btQuaternion> btv = Stamped<btQuaternion>(btQuaternion(xvalues[i], yvalues[i], zvalues[i]), ros::Time().fromNSec(1), "no frame");
00438 Stamped<btQuaternion> btv_out;
00439 geometry_msgs::QuaternionStamped msgv;
00440 quaternionStampedTFToMsg(btv, msgv);
00441 quaternionStampedMsgToTF(msgv, btv_out);
00442 EXPECT_NEAR(btv.x(), btv_out.x(), epsilon);
00443 EXPECT_NEAR(btv.y(), btv_out.y(), epsilon);
00444 EXPECT_NEAR(btv.z(), btv_out.z(), epsilon);
00445 EXPECT_NEAR(btv.w(), btv_out.w(), epsilon);
00446 EXPECT_STREQ(btv.frame_id_.c_str(), btv_out.frame_id_.c_str());
00447 EXPECT_EQ(btv.stamp_, btv_out.stamp_);
00448 }
00449 }
00450
00451 TEST(data, TransformConversions)
00452 {
00453
00454 uint64_t runs = 400;
00455 double epsilon = 1e-6;
00456
00457 std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
00458 generate_rand_vectors(1.0, runs, xvalues, yvalues,zvalues);
00459 std::vector<double> xvalues2(runs), yvalues2(runs), zvalues2(runs);
00460 generate_rand_vectors(1.0, runs, xvalues, yvalues,zvalues);
00461
00462 for ( uint64_t i = 0; i < runs ; i++ )
00463 {
00464 btTransform btv = btTransform(btQuaternion(xvalues2[i], yvalues2[i], zvalues2[i]), btVector3(xvalues[i], yvalues[i], zvalues[i]));
00465 btTransform btv_out;
00466 geometry_msgs::Transform msgv;
00467 transformTFToMsg(btv, msgv);
00468 transformMsgToTF(msgv, btv_out);
00469 EXPECT_NEAR(btv.getOrigin().x(), btv_out.getOrigin().x(), epsilon);
00470 EXPECT_NEAR(btv.getOrigin().y(), btv_out.getOrigin().y(), epsilon);
00471 EXPECT_NEAR(btv.getOrigin().z(), btv_out.getOrigin().z(), epsilon);
00472 EXPECT_NEAR(btv.getRotation().x(), btv_out.getRotation().x(), epsilon);
00473 EXPECT_NEAR(btv.getRotation().y(), btv_out.getRotation().y(), epsilon);
00474 EXPECT_NEAR(btv.getRotation().z(), btv_out.getRotation().z(), epsilon);
00475 EXPECT_NEAR(btv.getRotation().w(), btv_out.getRotation().w(), epsilon);
00476 }
00477
00478 }
00479
00480 TEST(data, PoseStampedConversions)
00481 {
00482
00483 uint64_t runs = 400;
00484 double epsilon = 1e-6;
00485
00486 std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
00487 generate_rand_vectors(1.0, runs, xvalues, yvalues,zvalues);
00488 std::vector<double> xvalues2(runs), yvalues2(runs), zvalues2(runs);
00489 generate_rand_vectors(1.0, runs, xvalues, yvalues,zvalues);
00490
00491 for ( uint64_t i = 0; i < runs ; i++ )
00492 {
00493 Stamped<Pose> btv = Stamped<Pose>(btTransform(btQuaternion(xvalues2[i], yvalues2[i], zvalues2[i]), btVector3(xvalues[i], yvalues[i], zvalues[i])), ros::Time().fromNSec(1), "no frame");
00494 Stamped<Pose> btv_out;
00495 geometry_msgs::PoseStamped msgv;
00496 poseStampedTFToMsg(btv, msgv);
00497 poseStampedMsgToTF(msgv, btv_out);
00498 EXPECT_NEAR(btv.getOrigin().x(), btv_out.getOrigin().x(), epsilon);
00499 EXPECT_NEAR(btv.getOrigin().y(), btv_out.getOrigin().y(), epsilon);
00500 EXPECT_NEAR(btv.getOrigin().z(), btv_out.getOrigin().z(), epsilon);
00501 EXPECT_NEAR(btv.getRotation().x(), btv_out.getRotation().x(), epsilon);
00502 EXPECT_NEAR(btv.getRotation().y(), btv_out.getRotation().y(), epsilon);
00503 EXPECT_NEAR(btv.getRotation().z(), btv_out.getRotation().z(), epsilon);
00504 EXPECT_NEAR(btv.getRotation().w(), btv_out.getRotation().w(), epsilon);
00505 EXPECT_STREQ(btv.frame_id_.c_str(), btv_out.frame_id_.c_str());
00506 EXPECT_EQ(btv.stamp_, btv_out.stamp_);
00507 }
00508 }
00509
00510 TEST(tf, ListOneInverse)
00511 {
00512 unsigned int runs = 4;
00513 double epsilon = 1e-6;
00514 seed_rand();
00515
00516 tf::Transformer mTR(true);
00517 std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
00518 for ( uint64_t i = 0; i < runs ; i++ )
00519 {
00520 xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00521 yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00522 zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00523
00524 StampedTransform tranStamped(btTransform(btQuaternion(0,0,0), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i), "my_parent", "child");
00525 mTR.setTransform(tranStamped);
00526 }
00527
00528
00529
00530
00531 for ( uint64_t i = 0; i < runs ; i++ )
00532
00533 {
00534 Stamped<Pose> inpose (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(10 + i), "child");
00535
00536 try{
00537 Stamped<Pose> outpose;
00538 outpose.setIdentity();
00539 mTR.transformPose("my_parent",inpose, outpose);
00540 EXPECT_NEAR(outpose.getOrigin().x(), xvalues[i], epsilon);
00541 EXPECT_NEAR(outpose.getOrigin().y(), yvalues[i], epsilon);
00542 EXPECT_NEAR(outpose.getOrigin().z(), zvalues[i], epsilon);
00543 }
00544 catch (tf::TransformException & ex)
00545 {
00546 std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl;
00547 bool exception_improperly_thrown = true;
00548 EXPECT_FALSE(exception_improperly_thrown);
00549 }
00550 }
00551
00552 }
00553
00554 TEST(tf, ListTwoInverse)
00555 {
00556 unsigned int runs = 4;
00557 double epsilon = 1e-6;
00558 seed_rand();
00559
00560 tf::Transformer mTR(true);
00561 std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
00562 for ( unsigned int i = 0; i < runs ; i++ )
00563 {
00564 xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00565 yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00566 zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00567
00568 StampedTransform tranStamped(btTransform(btQuaternion(0,0,0), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i), "my_parent", "child");
00569 mTR.setTransform(tranStamped);
00570 StampedTransform tranStamped2(btTransform(btQuaternion(0,0,0), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i), "child", "grandchild");
00571 mTR.setTransform(tranStamped2);
00572 }
00573
00574
00575
00576
00577 for ( unsigned int i = 0; i < runs ; i++ )
00578
00579 {
00580 Stamped<Pose> inpose (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(10 + i), "grandchild");
00581
00582 try{
00583 Stamped<Pose> outpose;
00584 outpose.setIdentity();
00585 mTR.transformPose("my_parent",inpose, outpose);
00586 EXPECT_NEAR(outpose.getOrigin().x(), 2*xvalues[i], epsilon);
00587 EXPECT_NEAR(outpose.getOrigin().y(), 2*yvalues[i], epsilon);
00588 EXPECT_NEAR(outpose.getOrigin().z(), 2*zvalues[i], epsilon);
00589 }
00590 catch (tf::TransformException & ex)
00591 {
00592 std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl;
00593 bool exception_improperly_thrown = true;
00594 EXPECT_FALSE(exception_improperly_thrown);
00595 }
00596 }
00597
00598 }
00599
00600
00601 TEST(tf, ListOneForward)
00602 {
00603 unsigned int runs = 4;
00604 double epsilon = 1e-6;
00605 seed_rand();
00606
00607 tf::Transformer mTR(true);
00608 std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
00609 for ( uint64_t i = 0; i < runs ; i++ )
00610 {
00611 xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00612 yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00613 zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00614
00615 StampedTransform tranStamped(btTransform(btQuaternion(0,0,0), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i), "my_parent", "child");
00616 mTR.setTransform(tranStamped);
00617 }
00618
00619
00620
00621
00622 for ( uint64_t i = 0; i < runs ; i++ )
00623
00624 {
00625 Stamped<Pose> inpose (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(10 + i), "my_parent");
00626
00627 try{
00628 Stamped<Pose> outpose;
00629 outpose.setIdentity();
00630 mTR.transformPose("child",inpose, outpose);
00631 EXPECT_NEAR(outpose.getOrigin().x(), -xvalues[i], epsilon);
00632 EXPECT_NEAR(outpose.getOrigin().y(), -yvalues[i], epsilon);
00633 EXPECT_NEAR(outpose.getOrigin().z(), -zvalues[i], epsilon);
00634 }
00635 catch (tf::TransformException & ex)
00636 {
00637 std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl;
00638 bool exception_improperly_thrown = true;
00639 EXPECT_FALSE(exception_improperly_thrown);
00640 }
00641 }
00642
00643 }
00644
00645 TEST(tf, ListTwoForward)
00646 {
00647 unsigned int runs = 4;
00648 double epsilon = 1e-6;
00649 seed_rand();
00650
00651 tf::Transformer mTR(true);
00652 std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
00653 for ( unsigned int i = 0; i < runs ; i++ )
00654 {
00655 xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00656 yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00657 zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00658
00659 StampedTransform tranStamped(btTransform(btQuaternion(0,0,0), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i), "my_parent", "child");
00660 mTR.setTransform(tranStamped);
00661 StampedTransform tranStamped2(btTransform(btQuaternion(0,0,0), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i), "child", "grandchild");
00662 mTR.setTransform(tranStamped2);
00663 }
00664
00665
00666
00667
00668 for ( unsigned int i = 0; i < runs ; i++ )
00669
00670 {
00671 Stamped<Pose> inpose (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(10 + i), "my_parent");
00672
00673 try{
00674 Stamped<Pose> outpose;
00675 outpose.setIdentity();
00676 mTR.transformPose("grandchild",inpose, outpose);
00677 EXPECT_NEAR(outpose.getOrigin().x(), -2*xvalues[i], epsilon);
00678 EXPECT_NEAR(outpose.getOrigin().y(), -2*yvalues[i], epsilon);
00679 EXPECT_NEAR(outpose.getOrigin().z(), -2*zvalues[i], epsilon);
00680 }
00681 catch (tf::TransformException & ex)
00682 {
00683 std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl;
00684 bool exception_improperly_thrown = true;
00685 EXPECT_FALSE(exception_improperly_thrown);
00686 }
00687 }
00688
00689 }
00690
00691 TEST(tf, TransformThrougRoot)
00692 {
00693 unsigned int runs = 4;
00694 double epsilon = 1e-6;
00695 seed_rand();
00696
00697 tf::Transformer mTR(true);
00698 std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
00699 for ( unsigned int i = 0; i < runs ; i++ )
00700 {
00701 xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00702 yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00703 zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00704
00705 StampedTransform tranStamped(btTransform(btQuaternion(0,0,0), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(1000 + i*100), "my_parent", "childA");
00706 mTR.setTransform(tranStamped);
00707 StampedTransform tranStamped2(btTransform(btQuaternion(0,0,0), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(1000 + i*100), "my_parent", "childB");
00708 mTR.setTransform(tranStamped2);
00709 }
00710
00711
00712
00713
00714 for ( unsigned int i = 0; i < runs ; i++ )
00715
00716 {
00717 Stamped<Pose> inpose (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(1000 + i*100), "childA");
00718
00719 try{
00720 Stamped<Pose> outpose;
00721 outpose.setIdentity();
00722 mTR.transformPose("childB",inpose, outpose);
00723 EXPECT_NEAR(outpose.getOrigin().x(), 0*xvalues[i], epsilon);
00724 EXPECT_NEAR(outpose.getOrigin().y(), 0*yvalues[i], epsilon);
00725 EXPECT_NEAR(outpose.getOrigin().z(), 0*zvalues[i], epsilon);
00726 }
00727 catch (tf::TransformException & ex)
00728 {
00729 std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl;
00730 bool exception_improperly_thrown = true;
00731 EXPECT_FALSE(exception_improperly_thrown);
00732 }
00733 }
00734
00735 }
00736
00737 TEST(tf, TransformThroughNO_PARENT)
00738 {
00739 unsigned int runs = 4;
00740 double epsilon = 1e-6;
00741 seed_rand();
00742
00743 tf::Transformer mTR(true);
00744 std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
00745 for ( unsigned int i = 0; i < runs ; i++ )
00746 {
00747 xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00748 yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00749 zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00750
00751 StampedTransform tranStamped(btTransform(btQuaternion(0,0,0), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i), "my_parentA", "childA");
00752 mTR.setTransform(tranStamped);
00753 StampedTransform tranStamped2(btTransform(btQuaternion(0,0,0), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i), "my_parentB", "childB");
00754 mTR.setTransform(tranStamped2);
00755 }
00756
00757
00758
00759
00760 for ( unsigned int i = 0; i < runs ; i++ )
00761
00762 {
00763 Stamped<btTransform> inpose (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(10 + i), "childA");
00764 bool exception_thrown = false;
00765
00766 try{
00767 Stamped<btTransform> outpose;
00768 outpose.setIdentity();
00769 mTR.transformPose("childB",inpose, outpose);
00770 EXPECT_NEAR(outpose.getOrigin().x(), 0*xvalues[i], epsilon);
00771 EXPECT_NEAR(outpose.getOrigin().y(), 0*yvalues[i], epsilon);
00772 EXPECT_NEAR(outpose.getOrigin().z(), 0*zvalues[i], epsilon);
00773 }
00774 catch (tf::TransformException & ex)
00775 {
00776 exception_thrown = true;
00777 }
00778 EXPECT_TRUE(exception_thrown);
00779 }
00780
00781 }
00782
00783
00784 TEST(tf, getParent)
00785 {
00786
00787 std::vector<std::string> children;
00788 std::vector<std::string> parents;
00789
00790 children.push_back("a");
00791 parents.push_back("c");
00792
00793 children.push_back("b");
00794 parents.push_back("c");
00795
00796 children.push_back("c");
00797 parents.push_back("e");
00798
00799 children.push_back("d");
00800 parents.push_back("e");
00801
00802 children.push_back("e");
00803 parents.push_back("f");
00804
00805 children.push_back("f");
00806 parents.push_back("j");
00807
00808 tf::Transformer mTR(true);
00809
00810 for (uint64_t i = 0; i < children.size(); i++)
00811 {
00812 StampedTransform tranStamped(btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(10), parents[i], children[i]);
00813 mTR.setTransform(tranStamped);
00814 }
00815
00816
00817
00818 std::string output;
00819 for (uint64_t i = 0; i < children.size(); i++)
00820 {
00821 EXPECT_TRUE(mTR.getParent(children[i], ros::Time().fromNSec(10), output));
00822 EXPECT_STREQ(tf::resolve("",parents[i]).c_str(), output.c_str());
00823 }
00824
00825 EXPECT_FALSE(mTR.getParent("j", ros::Time().fromNSec(10), output));
00826
00827 EXPECT_FALSE(mTR.getParent("no_value", ros::Time().fromNSec(10), output));
00828
00829 }
00830
00831
00832 TEST(tf, NO_PARENT_SET)
00833 {
00834 double epsilon = 1e-6;
00835
00836 std::vector<std::string> children;
00837 std::vector<std::string> parents;
00838
00839
00840
00841 children.push_back("b");
00842 parents.push_back("a");
00843 children.push_back("a");
00844 parents.push_back("NO_PARENT");
00845
00846 tf::Transformer mTR(true);
00847
00848 for (uint64_t i = 0; i < children.size(); i++)
00849 {
00850 StampedTransform tranStamped(btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(10), parents[i], children[i]);
00851 mTR.setTransform(tranStamped);
00852 }
00853
00854
00855
00856
00857 Stamped<btTransform> inpose (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(10), "a");
00858 Stamped<btTransform> outpose;
00859 outpose.setIdentity();
00860 mTR.transformPose("a",inpose, outpose);
00861 EXPECT_NEAR(outpose.getOrigin().x(), 0, epsilon);
00862 EXPECT_NEAR(outpose.getOrigin().y(), 0, epsilon);
00863 EXPECT_NEAR(outpose.getOrigin().z(), 0, epsilon);
00864
00865 }
00866
00867 TEST(tf, waitForTransform)
00868 {
00869 EXPECT_TRUE(ros::ok());
00870
00871 tf::Transformer mTR(true);
00872
00873
00874 std::string error_str;
00875 EXPECT_FALSE(mTR.waitForTransform("parent", "me", ros::Time().fromNSec(10000000), ros::Duration().fromSec(1.0), ros::Duration().fromSec(0.01), &error_str));
00876 EXPECT_STREQ("Do not call waitForTransform unless you are using another thread for populating data. Without a dedicated thread it will always timeout. If you have a seperate thread servicing tf messages, call setUsingDedicatedThread(true)", error_str.c_str());
00877
00878
00879 EXPECT_FALSE(mTR.waitForTransform("parent", "me", ros::Time().fromNSec(10000000), ros::Duration().fromSec(1.0), ros::Duration().fromSec(0.01)));
00880
00881
00882
00883
00884
00885 mTR.setUsingDedicatedThread(true);
00886
00887
00888 ros::Duration timeout = ros::Duration().fromSec(1.0);
00889 ros::Duration poll_freq = ros::Duration().fromSec(0.1);
00890 double eps = 0.2;
00891
00892
00893 ros::Time start_time = ros::Time::now();
00894 EXPECT_FALSE(mTR.waitForTransform("parent", "me", ros::Time().fromNSec(10000000), timeout));
00895 ros::Time stop_time = ros::Time::now();
00896 EXPECT_TRUE(fabs(((stop_time-start_time)-timeout).toSec()) < eps);
00897
00898
00899 start_time = ros::Time::now();
00900 EXPECT_FALSE(mTR.waitForTransform("parent", "me", ros::Time().fromNSec(10000000), timeout, poll_freq));
00901 stop_time = ros::Time::now();
00902 EXPECT_TRUE(fabs(((stop_time-start_time)-timeout).toSec()) < eps);
00903
00904
00905
00906 mTR.setTransform( StampedTransform(btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(10000000), "parent", "me"));
00907
00908 start_time = ros::Time::now();
00909 EXPECT_TRUE(mTR.waitForTransform("parent", "me", ros::Time().fromNSec(10000000),timeout));
00910 stop_time = ros::Time::now();
00911 EXPECT_TRUE(fabs(((stop_time-start_time)).toSec()) < eps);
00912
00913
00914 start_time = ros::Time::now();
00915 EXPECT_TRUE(mTR.waitForTransform("parent", "me", ros::Time().fromNSec(10000000),timeout, poll_freq));
00916 stop_time = ros::Time::now();
00917 EXPECT_TRUE(fabs(((stop_time-start_time)).toSec()) < eps);
00918 }
00919
00920
00921 TEST(tf, Exceptions)
00922 {
00923
00924 tf::Transformer mTR(true);
00925
00926
00927 Stamped<btTransform> outpose;
00928
00929
00930 EXPECT_FALSE(mTR.canTransform("parent", "me", ros::Time().fromNSec(10000000)));
00931 try
00932 {
00933 mTR.transformPose("parent",Stamped<Pose>(btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(10000000) , "me"), outpose);
00934 EXPECT_FALSE("ConnectivityException Not Thrown");
00935 }
00936 catch ( tf::LookupException &ex)
00937 {
00938 EXPECT_TRUE("Lookupgh Exception Caught");
00939 }
00940 catch (tf::TransformException& ex)
00941 {
00942 printf("%s\n",ex.what());
00943 EXPECT_FALSE("Other Exception Caught");
00944 }
00945
00946 mTR.setTransform( StampedTransform(btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(100000), "parent", "me"));
00947
00948
00949 EXPECT_FALSE(mTR.canTransform("parent", "me", ros::Time().fromNSec(200000)));
00950 try
00951 {
00952 mTR.transformPose("parent",Stamped<Pose>(btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(200000) , "me"), outpose);
00953 EXPECT_TRUE("ExtrapolationException Not Thrown");
00954 }
00955 catch ( tf::ExtrapolationException &ex)
00956 {
00957 EXPECT_TRUE("Extrapolation Exception Caught");
00958 }
00959 catch (tf::TransformException& ex)
00960 {
00961 printf("%s\n",ex.what());
00962 EXPECT_FALSE("Other Exception Caught");
00963 }
00964
00965
00966 mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(300000), "parent", "me"));
00967
00968
00969
00970 EXPECT_TRUE(mTR.canTransform("parent", "me", ros::Time().fromNSec(200000)));
00971 try
00972 {
00973 mTR.transformPose("parent",Stamped<Pose>(btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(200000) , "me"), outpose);
00974 EXPECT_TRUE("ExtrapolationException Not Thrown");
00975 }
00976 catch ( tf::ExtrapolationException &ex)
00977 {
00978 EXPECT_FALSE("Extrapolation Exception Caught");
00979 }
00980 catch (tf::TransformException& ex)
00981 {
00982 printf("%s\n",ex.what());
00983 EXPECT_FALSE("Other Exception Caught");
00984 }
00985
00986
00987
00988
00989 EXPECT_TRUE(mTR.canTransform("me", "parent", ros::Time().fromNSec(200000)));
00990 try
00991 {
00992 mTR.transformPose("me",Stamped<Pose>(btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(200000) , "parent"), outpose);
00993 EXPECT_TRUE("ExtrapolationException Not Thrown");
00994 }
00995 catch ( tf::ExtrapolationException &ex)
00996 {
00997 EXPECT_FALSE("Extrapolation Exception Caught");
00998 }
00999 catch (tf::TransformException& ex)
01000 {
01001 printf("%s\n",ex.what());
01002 EXPECT_FALSE("Other Exception Caught");
01003 }
01004
01005
01006
01007
01008 EXPECT_FALSE(mTR.canTransform("parent", "me", ros::Time().fromNSec(1000)));
01009 try
01010 {
01011 mTR.transformPose("parent",Stamped<Pose> (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(1000) , "me"), outpose);
01012 EXPECT_FALSE("ExtrapolationException Not Thrown");
01013 }
01014 catch ( tf::ExtrapolationException &ex)
01015 {
01016 EXPECT_TRUE("Extrapolation Exception Caught");
01017 }
01018 catch (tf::TransformException& ex)
01019 {
01020 printf("%s\n",ex.what());
01021 EXPECT_FALSE("Other Exception Caught");
01022 }
01023
01024 EXPECT_FALSE(mTR.canTransform("me", "parent", ros::Time().fromNSec(1000)));
01025 try
01026 {
01027 mTR.transformPose("me",Stamped<Pose> (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(1000) , "parent"), outpose);
01028 EXPECT_FALSE("ExtrapolationException Not Thrown");
01029 }
01030 catch ( tf::ExtrapolationException &ex)
01031 {
01032 EXPECT_TRUE("Extrapolation Exception Caught");
01033 }
01034 catch (tf::TransformException& ex)
01035 {
01036 printf("%s\n",ex.what());
01037 EXPECT_FALSE("Other Exception Caught");
01038 }
01039
01040
01041
01042
01043
01044
01045 EXPECT_FALSE(mTR.canTransform("parent", "me", ros::Time().fromNSec(350000)));
01046 try
01047 {
01048 mTR.transformPose("parent", Stamped<Pose> (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(350000) , "me"), outpose);
01049 EXPECT_FALSE("ExtrapolationException Not Thrown");
01050 }
01051 catch ( tf::ExtrapolationException &ex)
01052 {
01053 EXPECT_TRUE("Extrapolation Exception Caught");
01054 }
01055 catch (tf::TransformException& ex)
01056 {
01057 printf("%s\n",ex.what());
01058 EXPECT_FALSE("Other Exception Caught");
01059 }
01060
01061
01062 EXPECT_FALSE(mTR.canTransform("parent", "me", ros::Time().fromNSec(350000)));
01063 try
01064 {
01065 mTR.transformPose("me", Stamped<Pose> (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(350000) , "parent"), outpose);
01066 EXPECT_FALSE("ExtrapolationException Not Thrown");
01067 }
01068 catch ( tf::ExtrapolationException &ex)
01069 {
01070 EXPECT_TRUE("Extrapolation Exception Caught");
01071 }
01072 catch (tf::TransformException& ex)
01073 {
01074 printf("%s\n",ex.what());
01075 EXPECT_FALSE("Other Exception Caught");
01076 }
01077
01078
01079
01080
01081 }
01082
01083
01084
01085 TEST(tf, NoExtrapolationExceptionFromParent)
01086 {
01087 tf::Transformer mTR(true, ros::Duration().fromNSec(1000000));
01088
01089
01090
01091 mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(1000), "parent", "a"));
01092 mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(10000), "parent", "a"));
01093
01094
01095 mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(1000), "parent", "b"));
01096 mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(10000), "parent", "b"));
01097
01098 mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(1000), "parent's parent", "parent"));
01099 mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(1000), "parent's parent's parent", "parent's parent"));
01100
01101 mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(10000), "parent's parent", "parent"));
01102 mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(10000), "parent's parent's parent", "parent's parent"));
01103
01104 Stamped<Point> output;
01105
01106 try
01107 {
01108 mTR.transformPoint( "b", Stamped<Point>(Point(1,1,1), ros::Time().fromNSec(2000), "a"), output);
01109 }
01110 catch (ExtrapolationException &ex)
01111 {
01112 EXPECT_FALSE("Shouldn't have gotten this exception");
01113 }
01114
01115
01116
01117 };
01118
01119
01120
01121 TEST(tf, ExtrapolationFromOneValue)
01122 {
01123 tf::Transformer mTR(true, ros::Duration().fromNSec(1000000));
01124
01125
01126
01127 mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(1000), "parent", "a"));
01128
01129 mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(1000), "parent's parent", "parent"));
01130
01131
01132 Stamped<Point> output;
01133
01134 bool excepted = false;
01135
01136 try
01137 {
01138 mTR.transformPoint( "parent", Stamped<Point>(Point(1,1,1), ros::Time().fromNSec(10), "a"), output);
01139 }
01140 catch (ExtrapolationException &ex)
01141 {
01142 excepted = true;
01143 }
01144
01145 EXPECT_TRUE(excepted);
01146
01147 excepted = false;
01148
01149 try
01150 {
01151 mTR.transformPoint( "parent", Stamped<Point>(Point(1,1,1), ros::Time().fromNSec(100000), "a"), output);
01152 }
01153 catch (ExtrapolationException &ex)
01154 {
01155 excepted = true;
01156 }
01157
01158 EXPECT_TRUE(excepted);
01159
01160
01161 excepted = false;
01162 try
01163 {
01164 mTR.transformPoint( "parent's parent", Stamped<Point>(Point(1,1,1), ros::Time().fromNSec(1), "a"), output);
01165 }
01166 catch (ExtrapolationException &ex)
01167 {
01168 excepted = true;
01169 }
01170
01171 EXPECT_TRUE(excepted);
01172
01173
01174 excepted = false;
01175 try
01176 {
01177 mTR.transformPoint( "parent's parent", Stamped<Point>(Point(1,1,1), ros::Time().fromNSec(10000), "a"), output);
01178 }
01179 catch (ExtrapolationException &ex)
01180 {
01181 excepted = true;
01182 }
01183
01184 EXPECT_TRUE(excepted);
01185
01186 mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(20000), "parent", "a"));
01187
01188 excepted = false;
01189 try
01190 {
01191 mTR.transformPoint( "parent", Stamped<Point>(Point(1,1,1), ros::Time().fromNSec(10000), "a"), output);
01192 }
01193 catch (ExtrapolationException &ex)
01194 {
01195 excepted = true;
01196 }
01197
01198 EXPECT_FALSE(excepted);
01199
01200 };
01201
01202
01203
01204 TEST(tf, getLatestCommonTime)
01205 {
01206 tf::Transformer mTR(true);
01207 mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(1000), "parent", "a"));
01208 mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(2000), "parent's parent", "parent"));
01209
01210
01211 ros::Time t;
01212 mTR.getLatestCommonTime("a", "parent's parent", t, NULL);
01213 EXPECT_EQ(t, ros::Time().fromNSec(1000));
01214
01215
01216 EXPECT_EQ(tf::LOOKUP_ERROR, mTR.getLatestCommonTime("a", "not valid", t, NULL));
01217 EXPECT_EQ(t, ros::Time());
01218
01219
01220 mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(3000), "parent", "a"));
01221 mTR.getLatestCommonTime("a", "parent's parent",t, NULL);
01222 EXPECT_EQ(t, ros::Time().fromNSec(2000));
01223
01224
01225 mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(4000), "parent", "b"));
01226 mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(3000), "b", "c"));
01227 mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(9000), "c", "d"));
01228 mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(5000), "f", "e"));
01229
01230
01231 mTR.getLatestCommonTime("a", "b",t, NULL);
01232 EXPECT_EQ(t, ros::Time().fromNSec(3000));
01233
01234
01235 mTR.getLatestCommonTime("a", "c", t, NULL);
01236 EXPECT_EQ(t, ros::Time().fromNSec(3000));
01237
01238 mTR.getLatestCommonTime("c", "a", t, NULL);
01239 EXPECT_EQ(t, ros::Time().fromNSec(3000));
01240
01241
01242 mTR.getLatestCommonTime("a", "d", t, NULL);
01243 EXPECT_EQ(t, ros::Time().fromNSec(3000));
01244
01245 mTR.getLatestCommonTime("d", "a", t, NULL);
01246 EXPECT_EQ(t, ros::Time().fromNSec(3000));
01247
01248
01249 mTR.getLatestCommonTime("e", "f", t, NULL);
01250 EXPECT_EQ(t, ros::Time().fromNSec(5000));
01251
01252 mTR.getLatestCommonTime("f", "e", t, NULL);
01253 EXPECT_EQ(t, ros::Time().fromNSec(5000));
01254
01255
01256 mTR.setExtrapolationLimit(ros::Duration().fromNSec(20000));
01257
01258
01259 tf::Stamped<tf::Point> output, output2;
01260 try
01261 {
01262 mTR.transformPoint( "parent", Stamped<Point>(Point(1,1,1), ros::Time(), "b"), output);
01263 mTR.transformPoint( "a", ros::Time(),Stamped<Point>(Point(1,1,1), ros::Time(), "b"), "c", output2);
01264 }
01265 catch (tf::TransformException &ex)
01266 {
01267 printf("%s\n", ex.what());
01268 EXPECT_FALSE("Shouldn't get this Exception");
01269 }
01270
01271 EXPECT_EQ(output.stamp_, ros::Time().fromNSec(4000));
01272 EXPECT_EQ(output2.stamp_, ros::Time().fromNSec(3000));
01273
01274
01275
01276 ros::Time now1 = ros::Time::now();
01277 ros::Time time_output;
01278 mTR.getLatestCommonTime("a", "a", time_output, NULL);
01279 EXPECT_LE(now1.toSec(), time_output.toSec());
01280 EXPECT_LE(time_output.toSec(), ros::Time::now().toSec());
01281
01282
01283 }
01284
01285 TEST(tf, RepeatedTimes)
01286 {
01287 Transformer mTR;
01288 mTR.setTransform( StampedTransform (btTransform(btQuaternion(1,0,0), btVector3(0,0,0)), ros::Time().fromNSec(4000), "parent", "b"));
01289 mTR.setTransform( StampedTransform (btTransform(btQuaternion(1,1,0), btVector3(0,0,0)), ros::Time().fromNSec(4000), "parent", "b"));
01290
01291 tf::StampedTransform output;
01292 try{
01293 mTR.lookupTransform("parent", "b" , ros::Time().fromNSec(4000), output);
01294 EXPECT_TRUE(!std::isnan(output.getOrigin().x()));
01295 EXPECT_TRUE(!std::isnan(output.getOrigin().y()));
01296 EXPECT_TRUE(!std::isnan(output.getOrigin().z()));
01297 EXPECT_TRUE(!std::isnan(output.getRotation().x()));
01298 EXPECT_TRUE(!std::isnan(output.getRotation().y()));
01299 EXPECT_TRUE(!std::isnan(output.getRotation().z()));
01300 EXPECT_TRUE(!std::isnan(output.getRotation().w()));
01301 }
01302 catch (...)
01303 {
01304 EXPECT_FALSE("Excetion improperly thrown");
01305 }
01306
01307
01308 }
01309
01310 TEST(tf, frameExists)
01311 {
01312 Transformer mTR;
01313
01314
01315 EXPECT_FALSE(mTR.frameExists("/b"));;
01316 EXPECT_FALSE(mTR.frameExists("/parent"));
01317 EXPECT_FALSE(mTR.frameExists("/other"));
01318 EXPECT_FALSE(mTR.frameExists("/frame"));
01319
01320
01321 EXPECT_FALSE(mTR.frameExists("b"));;
01322 EXPECT_FALSE(mTR.frameExists("parent"));
01323 EXPECT_FALSE(mTR.frameExists("other"));
01324 EXPECT_FALSE(mTR.frameExists("frame"));
01325
01326 mTR.setTransform( StampedTransform (btTransform(btQuaternion(1,0,0), btVector3(0,0,0)), ros::Time().fromNSec(4000), "/parent", "/b"));
01327
01328
01329 EXPECT_TRUE(mTR.frameExists("/b"));
01330 EXPECT_TRUE(mTR.frameExists("/parent"));
01331 EXPECT_FALSE(mTR.frameExists("/other"));
01332 EXPECT_FALSE(mTR.frameExists("/frame"));
01333
01334
01335 EXPECT_TRUE(mTR.frameExists("b"));
01336 EXPECT_TRUE(mTR.frameExists("parent"));
01337 EXPECT_FALSE(mTR.frameExists("other"));
01338 EXPECT_FALSE(mTR.frameExists("frame"));
01339
01340 mTR.setTransform( StampedTransform (btTransform(btQuaternion(1,1,0), btVector3(0,0,0)), ros::Time().fromNSec(4000), "/frame", "/other"));
01341
01342
01343 EXPECT_TRUE(mTR.frameExists("/b"));
01344 EXPECT_TRUE(mTR.frameExists("/parent"));
01345 EXPECT_TRUE(mTR.frameExists("/other"));
01346 EXPECT_TRUE(mTR.frameExists("/frame"));
01347
01348
01349 EXPECT_TRUE(mTR.frameExists("b"));
01350 EXPECT_TRUE(mTR.frameExists("parent"));
01351 EXPECT_TRUE(mTR.frameExists("other"));
01352 EXPECT_TRUE(mTR.frameExists("frame"));
01353
01354 }
01355
01356 TEST(tf, resolve)
01357 {
01358
01359 EXPECT_STREQ("/id", tf::resolve("","id").c_str());
01360
01361 EXPECT_STREQ("/asdf/id", tf::resolve("asdf","id").c_str());
01362
01363 EXPECT_STREQ("/asdf/id", tf::resolve("/asdf","id").c_str());
01364
01365 EXPECT_STREQ("/id", tf::resolve("asdf","/id").c_str());
01366
01367 EXPECT_STREQ("/id", tf::resolve("/asdf","/id").c_str());
01368
01369 }
01370
01371 TEST(tf, canTransform)
01372 {
01373 Transformer mTR;
01374
01375
01376 EXPECT_TRUE(mTR.canTransform("some_frame","some_frame", ros::Time()));
01377 EXPECT_TRUE(mTR.canTransform("some_frame","some_frame", ros::Time::now()));
01378
01379
01380 for (int i = 10; i < 20; i++)
01381 {
01382 mTR.setTransform( StampedTransform (btTransform(btQuaternion(1,0,0), btVector3(0,0,0)), ros::Time().fromSec(i), "parent", "child"));
01383 mTR.setTransform( StampedTransform (btTransform(btQuaternion(1,0,0), btVector3(0,0,0)), ros::Time().fromSec(i), "parent", "other_child"));
01384 }
01385
01386
01387 ros::Time zero_time = ros::Time().fromSec(0);
01388 ros::Time old_time = ros::Time().fromSec(5);
01389 ros::Time valid_time = ros::Time().fromSec(15);
01390 ros::Time future_time = ros::Time().fromSec(25);
01391
01392
01393
01394 EXPECT_TRUE(mTR.canTransform("some_frame","some_frame", zero_time));
01395 EXPECT_TRUE(mTR.canTransform("some_frame","some_frame", old_time));
01396 EXPECT_TRUE(mTR.canTransform("some_frame","some_frame", valid_time));
01397 EXPECT_TRUE(mTR.canTransform("some_frame","some_frame", future_time));
01398
01399
01400
01401
01402 EXPECT_TRUE(mTR.canTransform("child", "parent", valid_time));
01403 EXPECT_TRUE(mTR.canTransform("child", "other_child", valid_time));
01404
01405
01406 EXPECT_TRUE(mTR.canTransform("child", "parent", zero_time));
01407 EXPECT_TRUE(mTR.canTransform("child", "other_child", zero_time));
01408
01409
01410 EXPECT_FALSE(mTR.canTransform("child", "parent", old_time));
01411 EXPECT_FALSE(mTR.canTransform("child", "other_child", old_time));
01412
01413
01414 EXPECT_FALSE(mTR.canTransform("child", "parent", future_time));
01415 EXPECT_FALSE(mTR.canTransform("child", "other_child", future_time));
01416
01417
01418 EXPECT_TRUE(mTR.canTransform("child", "child", zero_time));
01419 EXPECT_TRUE(mTR.canTransform("child", "child", old_time));
01420 EXPECT_TRUE(mTR.canTransform("child", "child", valid_time));
01421 EXPECT_TRUE(mTR.canTransform("child", "child", future_time));
01422
01423
01424
01425
01426
01427 EXPECT_TRUE(mTR.canTransform("child", zero_time, "parent", valid_time, "child"));
01428 EXPECT_TRUE(mTR.canTransform("child", zero_time, "other_child", valid_time, "child"));
01429
01430 EXPECT_TRUE(mTR.canTransform("child", old_time, "parent", valid_time, "child"));
01431 EXPECT_TRUE(mTR.canTransform("child", old_time, "other_child", valid_time, "child"));
01432
01433 EXPECT_TRUE(mTR.canTransform("child", valid_time, "parent", valid_time, "child"));
01434 EXPECT_TRUE(mTR.canTransform("child", valid_time, "other_child", valid_time, "child"));
01435
01436 EXPECT_TRUE(mTR.canTransform("child", future_time, "parent", valid_time, "child"));
01437 EXPECT_TRUE(mTR.canTransform("child", future_time, "other_child", valid_time, "child"));
01438
01439
01440 EXPECT_FALSE(mTR.canTransform("child", valid_time, "parent", old_time, "child"));
01441 EXPECT_FALSE(mTR.canTransform("child", valid_time, "other_child", old_time, "child"));
01442
01443 EXPECT_FALSE(mTR.canTransform("child", valid_time, "parent", future_time, "child"));
01444 EXPECT_FALSE(mTR.canTransform("child", valid_time, "other_child", future_time, "child"));
01445
01446
01447
01448 EXPECT_TRUE(mTR.canTransform("child", zero_time, "parent", valid_time, "parent"));
01449
01450 EXPECT_FALSE(mTR.canTransform("child", old_time, "parent", valid_time, "parent"));
01451
01452 EXPECT_TRUE(mTR.canTransform("child", valid_time, "parent", valid_time, "parent"));
01453
01454 EXPECT_FALSE(mTR.canTransform("child", future_time, "parent", valid_time, "parent"));
01455
01456
01457 EXPECT_TRUE(mTR.canTransform("child", valid_time, "parent", zero_time, "parent"));
01458
01459 EXPECT_TRUE(mTR.canTransform("child", valid_time, "parent", old_time, "parent"));
01460
01461 EXPECT_TRUE(mTR.canTransform("child", valid_time, "parent", valid_time, "parent"));
01462
01463 EXPECT_TRUE(mTR.canTransform("child", valid_time, "parent", future_time, "parent"));
01464
01465 }
01466
01467 TEST(tf, lookupTransform)
01468 {
01469 Transformer mTR;
01470
01471 for (int i = 10; i < 20; i++)
01472 {
01473 mTR.setTransform( StampedTransform (btTransform(btQuaternion(1,0,0), btVector3(0,0,0)), ros::Time().fromSec(i), "parent", "child"));
01474 mTR.setTransform( StampedTransform (btTransform(btQuaternion(1,0,0), btVector3(0,0,0)), ros::Time().fromSec(i), "parent", "other_child"));
01475 }
01476
01477
01478 ros::Time zero_time = ros::Time().fromSec(0);
01479 ros::Time old_time = ros::Time().fromSec(5);
01480 ros::Time valid_time = ros::Time().fromSec(15);
01481 ros::Time future_time = ros::Time().fromSec(25);
01482
01483
01484 tf::StampedTransform output;
01485
01486
01487
01488 try
01489 {
01490
01491 mTR.lookupTransform("some_frame","some_frame", zero_time, output);
01492 mTR.lookupTransform("some_frame","some_frame", old_time, output);
01493 mTR.lookupTransform("some_frame","some_frame", valid_time, output);
01494 mTR.lookupTransform("some_frame","some_frame", future_time, output);
01495 mTR.lookupTransform("child","child", future_time, output);
01496 mTR.lookupTransform("other_child","other_child", future_time, output);
01497
01498
01499 mTR.lookupTransform("child", "parent", valid_time, output);
01500 mTR.lookupTransform("child", "other_child", valid_time, output);
01501
01502
01503 mTR.lookupTransform("child", "parent", zero_time, output);
01504 mTR.lookupTransform("child", "other_child", zero_time, output);
01505 }
01506 catch (tf::TransformException &ex)
01507 {
01508 printf("Exception improperly thrown: %s", ex.what());
01509 EXPECT_FALSE("Exception thrown");
01510 }
01511 try
01512 {
01513
01514 mTR.lookupTransform("child", "parent", old_time, output);
01515 EXPECT_FALSE("Exception should have been thrown");
01516 }
01517 catch (tf::TransformException)
01518 {
01519 EXPECT_TRUE("Exception Thrown Correctly");
01520 }
01521 try {
01522
01523 mTR.lookupTransform("child", "parent", future_time, output);
01524 EXPECT_FALSE("Exception should have been thrown");
01525 }
01526 catch (tf::TransformException)
01527 {
01528 EXPECT_TRUE("Exception Thrown Correctly");
01529 }
01530
01531 try {
01532
01533 mTR.lookupTransform("child", "child", zero_time, output);
01534 mTR.lookupTransform("child", "child", old_time, output);
01535 mTR.lookupTransform("child", "child", valid_time, output);
01536 mTR.lookupTransform("child", "child", future_time, output);
01537
01538
01539
01540
01541
01542 mTR.lookupTransform("child", zero_time, "parent", valid_time, "child", output);
01543 mTR.lookupTransform("child", zero_time, "other_child", valid_time, "child", output);
01544
01545 mTR.lookupTransform("child", old_time, "parent", valid_time, "child", output);
01546 mTR.lookupTransform("child", old_time, "other_child", valid_time, "child", output);
01547
01548 mTR.lookupTransform("child", valid_time, "parent", valid_time, "child", output);
01549 mTR.lookupTransform("child", valid_time, "other_child", valid_time, "child", output);
01550
01551 mTR.lookupTransform("child", future_time, "parent", valid_time, "child", output);
01552 mTR.lookupTransform("child", future_time, "other_child", valid_time, "child", output);
01553 }
01554 catch (tf::TransformException &ex)
01555 {
01556 printf("Exception improperly thrown: %s", ex.what());
01557 EXPECT_FALSE("Exception incorrectly thrown");
01558 }
01559
01560 try {
01561
01562 mTR.lookupTransform("child", valid_time, "parent", old_time, "child", output);
01563 EXPECT_FALSE("Exception should have been thrown");
01564 }
01565 catch (tf::TransformException)
01566 {
01567 EXPECT_TRUE("Exception Thrown Correctly");
01568 }
01569
01570 try {
01571
01572 mTR.lookupTransform("child", valid_time, "parent", future_time, "child", output);
01573 EXPECT_FALSE("Exception should have been thrown");
01574 }
01575 catch (tf::TransformException)
01576 {
01577 EXPECT_TRUE("Exception Thrown Correctly");
01578 }
01579
01580 try {
01581
01582
01583 mTR.lookupTransform("child", zero_time, "parent", valid_time, "parent", output);
01584
01585 mTR.lookupTransform("child", valid_time, "parent", valid_time, "parent", output);
01586 }
01587 catch (tf::TransformException &ex)
01588 {
01589 printf("Exception improperly thrown: %s", ex.what());
01590 EXPECT_FALSE("Exception incorrectly thrown");
01591 }
01592
01593 try {
01594
01595 mTR.lookupTransform("child", old_time, "parent", valid_time, "parent", output);
01596 EXPECT_FALSE("Exception should have been thrown");
01597 }
01598 catch (tf::TransformException)
01599 {
01600 EXPECT_TRUE("Exception Thrown Correctly");
01601 }
01602 try {
01603
01604 mTR.lookupTransform("child", future_time, "parent", valid_time, "parent", output);
01605 EXPECT_FALSE("Exception should have been thrown");
01606 }
01607 catch (tf::TransformException)
01608 {
01609 EXPECT_TRUE("Exception Thrown Correctly");
01610 }
01611
01612 try {
01613
01614 mTR.lookupTransform("child", valid_time, "parent", zero_time, "parent", output);
01615
01616 mTR.lookupTransform("child", valid_time, "parent", old_time, "parent", output);
01617
01618 mTR.lookupTransform("child", valid_time, "parent", valid_time, "parent", output);
01619
01620 mTR.lookupTransform("child", valid_time, "parent", future_time, "parent", output);
01621 }
01622 catch (tf::TransformException &ex)
01623 {
01624 printf("Exception improperly thrown: %s", ex.what());
01625 EXPECT_FALSE("Exception improperly thrown");
01626 }
01627
01628
01629
01630 try
01631 {
01632 ros::Time now1 = ros::Time::now();
01633
01634 mTR.lookupTransform("a", "a", ros::Time(),output);
01635 EXPECT_LE(now1.toSec(), output.stamp_.toSec());
01636 EXPECT_LE(output.stamp_.toSec(), ros::Time::now().toSec());
01637 }
01638 catch (tf::TransformException &ex)
01639 {
01640 printf("Exception improperly thrown: %s", ex.what());
01641 EXPECT_FALSE("Exception improperly thrown");
01642 }
01643
01644 }
01645
01646
01647 TEST(tf, getFrameStrings)
01648 {
01649 Transformer mTR;
01650
01651
01652 mTR.setTransform( StampedTransform (btTransform(btQuaternion(1,0,0), btVector3(0,0,0)), ros::Time().fromNSec(4000), "/parent", "/b"));
01653 std::vector <std::string> frames_string;
01654 mTR.getFrameStrings(frames_string);
01655 ASSERT_EQ(frames_string.size(), (unsigned)2);
01656 EXPECT_STREQ(frames_string[0].c_str(), std::string("/b").c_str());
01657 EXPECT_STREQ(frames_string[1].c_str(), std::string("/parent").c_str());
01658
01659
01660 mTR.setTransform( StampedTransform (btTransform(btQuaternion(1,1,0), btVector3(0,0,0)), ros::Time().fromNSec(4000), "/frame", "/other"));
01661
01662 mTR.getFrameStrings(frames_string);
01663 ASSERT_EQ(frames_string.size(), (unsigned)4);
01664 EXPECT_STREQ(frames_string[0].c_str(), std::string("/b").c_str());
01665 EXPECT_STREQ(frames_string[1].c_str(), std::string("/parent").c_str());
01666 EXPECT_STREQ(frames_string[2].c_str(), std::string("/other").c_str());
01667 EXPECT_STREQ(frames_string[3].c_str(), std::string("/frame").c_str());
01668
01669 }
01670
01671 bool expectInvalidQuaternion(tf::Quaternion q)
01672 {
01673 try
01674 {
01675 tf::assertQuaternionValid(q);
01676 printf("this should have thrown\n");
01677 return false;
01678 }
01679 catch (tf::InvalidArgument &ex)
01680 {
01681 return true;
01682 }
01683 catch (...)
01684 {
01685 printf("A different type of exception was expected\n");
01686 return false;
01687 }
01688 return false;
01689 }
01690
01691 bool expectValidQuaternion(tf::Quaternion q)
01692 {
01693 try
01694 {
01695 tf::assertQuaternionValid(q);
01696 }
01697 catch (tf::TransformException &ex)
01698 {
01699 return false;
01700 }
01701 return true;
01702 }
01703
01704 bool expectInvalidQuaternion(geometry_msgs::Quaternion q)
01705 {
01706 try
01707 {
01708 tf::assertQuaternionValid(q);
01709 printf("this should have thrown\n");
01710 return false;
01711 }
01712 catch (tf::InvalidArgument &ex)
01713 {
01714 return true;
01715 }
01716 catch (...)
01717 {
01718 printf("A different type of exception was expected\n");
01719 return false;
01720 }
01721 return false;
01722 }
01723
01724 bool expectValidQuaternion(geometry_msgs::Quaternion q)
01725 {
01726 try
01727 {
01728 tf::assertQuaternionValid(q);
01729 }
01730 catch (tf::TransformException &ex)
01731 {
01732 return false;
01733 }
01734 return true;
01735 }
01736
01737
01738 TEST(tf, assertQuaternionValid)
01739 {
01740 tf::Quaternion q(1,0,0,0);
01741 EXPECT_TRUE(expectValidQuaternion(q));
01742 q.setX(0);
01743 EXPECT_TRUE(expectInvalidQuaternion(q));
01744 q.setY(1);
01745 EXPECT_TRUE(expectValidQuaternion(q));
01746 q.setZ(1);
01747 EXPECT_TRUE(expectInvalidQuaternion(q));
01748 q.setY(0);
01749 EXPECT_TRUE(expectValidQuaternion(q));
01750 q.setW(1);
01751 EXPECT_TRUE(expectInvalidQuaternion(q));
01752 q.setZ(0);
01753 EXPECT_TRUE(expectValidQuaternion(q));
01754 q.setZ(sqrt(2.0)/2.0);
01755 EXPECT_TRUE(expectInvalidQuaternion(q));
01756 q.setW(sqrt(2.0)/2.0);
01757 EXPECT_TRUE(expectValidQuaternion(q));
01758
01759 q.setZ(sqrt(2.0)/2.0 + 0.01);
01760 EXPECT_TRUE(expectInvalidQuaternion(q));
01761
01762 q.setZ(sqrt(2.0)/2.0 - 0.01);
01763 EXPECT_TRUE(expectInvalidQuaternion(q));
01764
01765
01766
01767
01768
01769
01770
01771
01772
01773 }
01774 TEST(tf, assertQuaternionMsgValid)
01775 {
01776 geometry_msgs::Quaternion q;
01777 q.x = 1;
01778
01779 EXPECT_TRUE(expectValidQuaternion(q));
01780 q.x = 0;
01781 EXPECT_TRUE(expectInvalidQuaternion(q));
01782 q.y = 1;
01783 EXPECT_TRUE(expectValidQuaternion(q));
01784 q.z = 1;
01785 EXPECT_TRUE(expectInvalidQuaternion(q));
01786 q.y = 0;
01787 EXPECT_TRUE(expectValidQuaternion(q));
01788 q.w = 1;
01789 EXPECT_TRUE(expectInvalidQuaternion(q));
01790 q.z = 0;
01791 EXPECT_TRUE(expectValidQuaternion(q));
01792 q.z = sqrt(2.0)/2.0;
01793 EXPECT_TRUE(expectInvalidQuaternion(q));
01794 q.w = sqrt(2.0)/2.0;
01795 EXPECT_TRUE(expectValidQuaternion(q));
01796
01797 q.z = sqrt(2.0)/2.0 + 0.01;
01798 EXPECT_TRUE(expectInvalidQuaternion(q));
01799
01800 q.z = sqrt(2.0)/2.0 - 0.01;
01801 EXPECT_TRUE(expectInvalidQuaternion(q));
01802
01803
01804
01805
01806
01807
01808
01809
01810
01811 }
01812
01813 TEST(data, StampedOperatorEqualEqual)
01814 {
01815 tf::Pose pose0, pose1, pose0a;
01816 pose0.setIdentity();
01817 pose0a.setIdentity();
01818 pose1.setIdentity();
01819 pose1.setOrigin(btVector3(1, 0, 0));
01820 tf::Stamped<tf::Pose> stamped_pose_reference(pose0a, ros::Time(), "frame_id");
01821 tf::Stamped<tf::Pose> stamped_pose0A(pose0, ros::Time(), "frame_id");
01822 EXPECT_TRUE(stamped_pose0A == stamped_pose_reference);
01823 tf::Stamped<tf::Pose> stamped_pose0B(pose0, ros::Time(), "frame_id_not_equal");
01824 EXPECT_FALSE(stamped_pose0B == stamped_pose_reference);
01825 tf::Stamped<tf::Pose> stamped_pose0C(pose0, ros::Time(1.0), "frame_id");
01826 EXPECT_FALSE(stamped_pose0C == stamped_pose_reference);
01827 tf::Stamped<tf::Pose> stamped_pose0D(pose0, ros::Time(1.0), "frame_id_not_equal");
01828 EXPECT_FALSE(stamped_pose0D == stamped_pose_reference);
01829 tf::Stamped<tf::Pose> stamped_pose0E(pose1, ros::Time(), "frame_id_not_equal");
01830 EXPECT_FALSE(stamped_pose0E == stamped_pose_reference);
01831 tf::Stamped<tf::Pose> stamped_pose0F(pose1, ros::Time(1.0), "frame_id");
01832 EXPECT_FALSE(stamped_pose0F == stamped_pose_reference);
01833 tf::Stamped<tf::Pose> stamped_pose0G(pose1, ros::Time(1.0), "frame_id_not_equal");
01834 EXPECT_FALSE(stamped_pose0G == stamped_pose_reference);
01835 tf::Stamped<tf::Pose> stamped_pose0H(pose1, ros::Time(), "frame_id");
01836 EXPECT_FALSE(stamped_pose0H == stamped_pose_reference);
01837
01838 }
01839
01840 TEST(data, StampedTransformOperatorEqualEqual)
01841 {
01842 tf::Transform transform0, transform1, transform0a;
01843 transform0.setIdentity();
01844 transform0a.setIdentity();
01845 transform1.setIdentity();
01846 transform1.setOrigin(btVector3(1, 0, 0));
01847 tf::StampedTransform stamped_transform_reference(transform0a, ros::Time(), "frame_id", "child_frame_id");
01848 tf::StampedTransform stamped_transform0A(transform0, ros::Time(), "frame_id", "child_frame_id");
01849 EXPECT_TRUE(stamped_transform0A == stamped_transform_reference);
01850 tf::StampedTransform stamped_transform0B(transform0, ros::Time(), "frame_id_not_equal", "child_frame_id");
01851 EXPECT_FALSE(stamped_transform0B == stamped_transform_reference);
01852 tf::StampedTransform stamped_transform0C(transform0, ros::Time(1.0), "frame_id", "child_frame_id");
01853 EXPECT_FALSE(stamped_transform0C == stamped_transform_reference);
01854 tf::StampedTransform stamped_transform0D(transform0, ros::Time(1.0), "frame_id_not_equal", "child_frame_id");
01855 EXPECT_FALSE(stamped_transform0D == stamped_transform_reference);
01856 tf::StampedTransform stamped_transform0E(transform1, ros::Time(), "frame_id_not_equal", "child_frame_id");
01857 EXPECT_FALSE(stamped_transform0E == stamped_transform_reference);
01858 tf::StampedTransform stamped_transform0F(transform1, ros::Time(1.0), "frame_id", "child_frame_id");
01859 EXPECT_FALSE(stamped_transform0F == stamped_transform_reference);
01860 tf::StampedTransform stamped_transform0G(transform1, ros::Time(1.0), "frame_id_not_equal", "child_frame_id");
01861 EXPECT_FALSE(stamped_transform0G == stamped_transform_reference);
01862 tf::StampedTransform stamped_transform0H(transform1, ros::Time(), "frame_id", "child_frame_id");
01863 EXPECT_FALSE(stamped_transform0H == stamped_transform_reference);
01864
01865
01866
01867 tf::StampedTransform stamped_transform1A(transform0, ros::Time(), "frame_id", "child_frame_id2");
01868 EXPECT_FALSE(stamped_transform1A == stamped_transform_reference);
01869 tf::StampedTransform stamped_transform1B(transform0, ros::Time(), "frame_id_not_equal", "child_frame_id2");
01870 EXPECT_FALSE(stamped_transform1B == stamped_transform_reference);
01871 tf::StampedTransform stamped_transform1C(transform0, ros::Time(1.0), "frame_id", "child_frame_id2");
01872 EXPECT_FALSE(stamped_transform1C == stamped_transform_reference);
01873 tf::StampedTransform stamped_transform1D(transform0, ros::Time(1.0), "frame_id_not_equal", "child_frame_id2");
01874 EXPECT_FALSE(stamped_transform1D == stamped_transform_reference);
01875 tf::StampedTransform stamped_transform1E(transform1, ros::Time(), "frame_id_not_equal", "child_frame_id2");
01876 EXPECT_FALSE(stamped_transform1E == stamped_transform_reference);
01877 tf::StampedTransform stamped_transform1F(transform1, ros::Time(1.0), "frame_id", "child_frame_id2");
01878 EXPECT_FALSE(stamped_transform1F == stamped_transform_reference);
01879 tf::StampedTransform stamped_transform1G(transform1, ros::Time(1.0), "frame_id_not_equal", "child_frame_id2");
01880 EXPECT_FALSE(stamped_transform1G == stamped_transform_reference);
01881 tf::StampedTransform stamped_transform1H(transform1, ros::Time(), "frame_id", "child_frame_id2");
01882 EXPECT_FALSE(stamped_transform1H == stamped_transform_reference);
01883
01884 }
01885
01886 TEST(data, StampedOperatorEqual)
01887 {
01888 tf::Pose pose0, pose1, pose0a;
01889 pose0.setIdentity();
01890 pose1.setIdentity();
01891 pose1.setOrigin(btVector3(1, 0, 0));
01892 tf::Stamped<tf::Pose> stamped_pose0(pose0, ros::Time(), "frame_id");
01893 tf::Stamped<tf::Pose> stamped_pose1(pose1, ros::Time(1.0), "frame_id_not_equal");
01894 EXPECT_FALSE(stamped_pose1 == stamped_pose0);
01895 stamped_pose1 = stamped_pose0;
01896 EXPECT_TRUE(stamped_pose1 == stamped_pose0);
01897
01898 }
01899
01900 int main(int argc, char **argv){
01901 testing::InitGoogleTest(&argc, argv);
01902 ros::Time::init();
01903 ros::init(argc, argv, "tf_unittest");
01904 return RUN_ALL_TESTS();
01905 }