tf_unittest_future.cpp
Go to the documentation of this file.
00001 #include <gtest/gtest.h>
00002 #include <tf/tf.h>
00003 #include <sys/time.h>
00004 
00005 #include "tf/LinearMath/Vector3.h"
00006 
00007 using namespace tf;
00008 
00009 void seed_rand()
00010 {
00011   //Seed random number generator with current microseond count
00012   timeval temp_time_struct;
00013   gettimeofday(&temp_time_struct,NULL);
00014   srand(temp_time_struct.tv_usec);
00015 };
00016 
00017 
00018 
00019 TEST(tf, SignFlipExtrapolate)
00020 {
00021   double epsilon = 1e-3;
00022 
00023   double truex, truey, trueyaw1, trueyaw2;
00024 
00025   truex = 5.220;
00026   truey = 1.193;
00027   trueyaw1 = 2.094;
00028   trueyaw2 = 2.199;
00029   ros::Time ts0;
00030   ts0.fromSec(46.6);
00031   ros::Time ts1;
00032   ts1.fromSec(46.7);
00033   ros::Time ts2;
00034   ts2.fromSec(46.8);
00035   
00036   TransformStorage tout;
00037   double yaw, pitch, roll;
00038 
00039   TransformStorage t0(StampedTransform
00040                       (tf::Transform(tf::Quaternion(0.000, 0.000,  -0.8386707128751809, 0.5446388118427071),
00041                                    tf::Vector3(1.0330764266905630, 5.2545257423922198, -0.000)),
00042                        ts0, "odom", "other0"), 3);
00043   TransformStorage t1(StampedTransform
00044                       (tf::Transform(tf::Quaternion(0.000, 0.000,  0.8660255375641606, -0.4999997682866531),
00045                                    tf::Vector3(1.5766646418987809, 5.1177550046707436, -0.000)),
00046                        ts1, "odom", "other1"), 3);
00047   TransformStorage t2(StampedTransform
00048                       (tf::Transform(tf::Quaternion(0.000, 0.000, 0.8910066733792211, -0.4539902069358919),
00049                                    tf::Vector3(2.1029791754869160, 4.9249128183465967, -0.000)),
00050                        ts2, "odom", "other2"), 3);
00051 
00052   tf::TimeCache tc;
00053   tf::Transform res;
00054 
00055   tc.interpolate(t0, t1, ts1, tout);
00056   res = tout.inverse();
00057   res.getBasis().getEulerZYX(yaw,pitch,roll);
00058 
00059   EXPECT_NEAR(res.getOrigin().x(), truex, epsilon);
00060   EXPECT_NEAR(res.getOrigin().y(), truey, epsilon);
00061   EXPECT_NEAR(yaw, trueyaw1, epsilon);
00062 
00063   tc.interpolate(t0, t1, ts2, tout);
00064   res = tout.inverse();
00065   res.getBasis().getEulerZYX(yaw,pitch,roll);
00066 
00067   EXPECT_NEAR(res.getOrigin().x(), truex, epsilon);
00068   EXPECT_NEAR(res.getOrigin().y(), truey, epsilon);
00069   EXPECT_NEAR(yaw, trueyaw2, epsilon);
00070 
00071   tc.interpolate(t1, t2, ts2, tout);
00072   res = tout.inverse();
00073   res.getBasis().getEulerZYX(yaw,pitch,roll);
00074 
00075   EXPECT_NEAR(res.getOrigin().x(), truex, epsilon);
00076   EXPECT_NEAR(res.getOrigin().y(), truey, epsilon);
00077   EXPECT_NEAR(yaw, trueyaw2, epsilon);
00078 }
00079 
00080 
00081 
00084 int main(int argc, char **argv){
00085   testing::InitGoogleTest(&argc, argv);
00086   return RUN_ALL_TESTS();
00087 }
00088 


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Fri Aug 11 2017 02:21:55