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
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
00059 TEST(tf_benchmark, canTransformCacheLength0)
00060 {
00061 tf::Transformer mTR(true);
00062
00063 uint64_t runs = 100000;
00064 ros::WallTime start_time = ros::WallTime::now();
00065
00066
00067 start_time = ros::WallTime::now();
00068 for (uint64_t i = 0 ; i < runs; i++)
00069 {
00070 EXPECT_FALSE(mTR.canTransform("target","source_frame", ros::Time::now()));
00071 }
00072 ros::WallDuration run_duration = ros::WallTime::now() - start_time;
00073 double frequency = (double)runs / run_duration.toSec() ;
00074 printf("frequency %f\n", frequency);
00075 EXPECT_GT( frequency, 10000.0);
00076
00077
00078
00079 }
00080
00081 TEST(tf_benchmark, canTransformCacheLength10000)
00082 {
00083 tf::Transformer mTR(true);
00084
00085 unsigned int cache_length = 10000;
00086 for (unsigned int i = 0; i < cache_length; i++)
00087 {
00088 StampedTransform tranStamped(btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(10.0+i), "my_parent", "child");
00089 mTR.setTransform(tranStamped);
00090 }
00091
00092
00093 uint64_t runs = 100000;
00094 ros::WallTime start_time = ros::WallTime::now();
00095
00096
00097
00098 start_time = ros::WallTime::now();
00099 for (uint64_t i = 0 ; i < runs; i++)
00100 {
00101 EXPECT_TRUE(mTR.canTransform("child","my_parent", ros::Time().fromNSec(10)));
00102 }
00103 ros::WallDuration run_duration = ros::WallTime::now() - start_time;
00104 double frequency = (double)runs / run_duration.toSec() ;
00105 printf("Worst Case Frequency %f\n", frequency);
00106 EXPECT_GT( frequency, 10000.0);
00107
00108
00109 start_time = ros::WallTime::now();
00110 for (uint64_t i = 0 ; i < runs; i++)
00111 {
00112 EXPECT_TRUE(mTR.canTransform("child","my_parent", ros::Time().fromNSec(10+cache_length/2)));
00113 }
00114 run_duration = ros::WallTime::now() - start_time;
00115 frequency = (double)runs / run_duration.toSec() ;
00116 printf("Intermediate Case Frequency %f\n", frequency);
00117 EXPECT_GT( frequency, 10000.0);
00118
00119
00120 start_time = ros::WallTime::now();
00121 for (uint64_t i = 0 ; i < runs; i++)
00122 {
00123 EXPECT_TRUE(mTR.canTransform("child","my_parent", ros::Time().fromNSec(10+cache_length -1)));
00124 }
00125 run_duration = ros::WallTime::now() - start_time;
00126 frequency = (double)runs / run_duration.toSec() ;
00127 printf("Best Case Frequency %f\n", frequency);
00128 EXPECT_GT( frequency, 10000.0);
00129
00130
00131
00132 }
00133
00134 TEST(tf_benchmark, benchmarkExhaustiveSearch)
00135 {
00136
00137 uint64_t runs = 40000;
00138 double epsilon = 1e-6;
00139 seed_rand();
00140
00141 tf::Transformer mTR(true);
00142 std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
00143 for ( uint64_t i = 0; i < runs ; i++ )
00144 {
00145 xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00146 yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00147 zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00148
00149 StampedTransform tranStamped(btTransform(btQuaternion(0,0,0), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10.0 + i), "my_parent", "child");
00150 mTR.setTransform(tranStamped);
00151
00152 }
00153
00154
00155
00156
00157 ros::WallTime start_time = ros::WallTime::now();
00158 for ( uint64_t i = 0; i < runs ; i++ )
00159
00160 {
00161 Stamped<btTransform> inpose (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(10.0 + i), "child");
00162
00163 try{
00164 Stamped<Pose> outpose;
00165 outpose.setIdentity();
00166 mTR.transformPose("my_parent",inpose, outpose);
00167 EXPECT_NEAR(outpose.getOrigin().x(), xvalues[i], epsilon);
00168 EXPECT_NEAR(outpose.getOrigin().y(), yvalues[i], epsilon);
00169 EXPECT_NEAR(outpose.getOrigin().z(), zvalues[i], epsilon);
00170 }
00171 catch (tf::TransformException & ex)
00172 {
00173 std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl;
00174 bool exception_improperly_thrown = true;
00175 EXPECT_FALSE(exception_improperly_thrown);
00176 }
00177 }
00178
00179 Stamped<Pose> inpose (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time().fromNSec(runs), "child");
00180 Stamped<Pose> outpose;
00181 outpose.setIdentity();
00182 mTR.transformPose("child",inpose, outpose);
00183 EXPECT_NEAR(outpose.getOrigin().x(), 0, epsilon);
00184 EXPECT_NEAR(outpose.getOrigin().y(), 0, epsilon);
00185 EXPECT_NEAR(outpose.getOrigin().z(), 0, epsilon);
00186
00187 ros::WallDuration run_duration = ros::WallTime::now() - start_time;
00188 double frequency = (double)runs / run_duration.toSec() ;
00189 printf("frequency %f\n", frequency);
00190 EXPECT_GT( frequency, 500.0);
00191
00192 }
00193
00194 int main(int argc, char **argv){
00195 testing::InitGoogleTest(&argc, argv);
00196 return RUN_ALL_TESTS();
00197 }