cache_unittest.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <gtest/gtest.h>
00031 #include <tf2/time_cache.h>
00032 #include <sys/time.h>
00033 #include "tf2/LinearMath/Quaternion.h"
00034 #include <stdexcept>
00035 
00036 #include <geometry_msgs/TransformStamped.h>
00037 
00038 #include <cmath>
00039 
00040 std::vector<double> values;
00041 unsigned int step = 0;
00042 
00043 void seed_rand()
00044 {
00045   values.clear();
00046   for (unsigned int i = 0; i < 1000; i++)
00047   {
00048     int pseudo_rand = std::floor(i * M_PI);
00049     values.push_back(( pseudo_rand % 100)/50.0 - 1.0);
00050     //printf("Seeding with %f\n", values.back());
00051   }
00052 };
00053 
00054 
00055 double get_rand() 
00056 { 
00057   if (values.size() == 0) throw std::runtime_error("you need to call seed_rand first");
00058   if (step >= values.size()) 
00059     step = 0;
00060   else
00061     step++;
00062   return values[step];
00063 }
00064 
00065 using namespace tf2;
00066 
00067 
00068 void setIdentity(TransformStorage& stor)
00069 {
00070   stor.translation_.setValue(0.0, 0.0, 0.0);
00071   stor.rotation_.setValue(0.0, 0.0, 0.0, 1.0);
00072 }
00073 
00074 TEST(TimeCache, Repeatability)
00075 {
00076   unsigned int runs = 100;
00077   
00078   tf2::TimeCache  cache;
00079 
00080   TransformStorage stor;
00081   setIdentity(stor);
00082   
00083   for ( uint64_t i = 1; i < runs ; i++ )
00084   {
00085     stor.frame_id_ = i;
00086     stor.stamp_ = ros::Time().fromNSec(i);
00087     
00088     cache.insertData(stor);
00089   }
00090 
00091   for ( uint64_t i = 1; i < runs ; i++ )
00092 
00093   {
00094     cache.getData(ros::Time().fromNSec(i), stor);
00095     EXPECT_EQ(stor.frame_id_, i);
00096     EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i));
00097   }
00098   
00099 }
00100 
00101 TEST(TimeCache, RepeatabilityReverseInsertOrder)
00102 {
00103   unsigned int runs = 100;
00104   
00105   tf2::TimeCache  cache;
00106 
00107   TransformStorage stor;
00108   setIdentity(stor);
00109   
00110   for ( int i = runs -1; i >= 0 ; i-- )
00111   {
00112     stor.frame_id_ = i;
00113     stor.stamp_ = ros::Time().fromNSec(i);
00114     
00115     cache.insertData(stor);
00116   }
00117   for ( uint64_t i = 1; i < runs ; i++ )
00118 
00119   {
00120     cache.getData(ros::Time().fromNSec(i), stor);
00121     EXPECT_EQ(stor.frame_id_, i);
00122     EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i));
00123   }
00124   
00125 }
00126 
00127 #if 0    // jfaust: this doesn't seem to actually be testing random insertion?
00128 TEST(TimeCache, RepeatabilityRandomInsertOrder)
00129 {
00130 
00131   seed_rand();
00132   
00133   tf2::TimeCache  cache;
00134   double my_vals[] = {13,2,5,4,9,7,3,11,15,14,12,1,6,10,0,8};
00135   std::vector<double> values (my_vals, my_vals + sizeof(my_vals)/sizeof(double)); 
00136   unsigned int runs = values.size();
00137 
00138   TransformStorage stor;
00139   setIdentity(stor);
00140   for ( uint64_t i = 0; i <runs ; i++ )
00141   {
00142     values[i] = 10.0 * get_rand();
00143     std::stringstream ss;
00144     ss << values[i];
00145     stor.header.frame_id = ss.str();
00146     stor.frame_id_ = i;
00147     stor.stamp_ = ros::Time().fromNSec(i);
00148     
00149     cache.insertData(stor);
00150   }
00151   for ( uint64_t i = 1; i < runs ; i++ )
00152 
00153   {
00154     cache.getData(ros::Time().fromNSec(i), stor);
00155     EXPECT_EQ(stor.frame_id_, i);
00156     EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i));
00157     std::stringstream ss;
00158     ss << values[i];
00159     EXPECT_EQ(stor.header.frame_id, ss.str());
00160   }
00161   
00162 }
00163 #endif
00164 
00165 TEST(TimeCache, ZeroAtFront)
00166 {
00167   uint64_t runs = 100;
00168 
00169   tf2::TimeCache  cache;
00170 
00171   TransformStorage stor;
00172   setIdentity(stor);
00173   
00174   for ( uint64_t i = 1; i < runs ; i++ )
00175   {
00176     stor.frame_id_ = i;
00177     stor.stamp_ = ros::Time().fromNSec(i);
00178     
00179     cache.insertData(stor);
00180   }
00181 
00182   stor.frame_id_ = runs;
00183   stor.stamp_ = ros::Time().fromNSec(runs);
00184   cache.insertData(stor);
00185 
00186   for ( uint64_t i = 1; i < runs ; i++ )
00187 
00188   {
00189     cache.getData(ros::Time().fromNSec(i), stor);
00190     EXPECT_EQ(stor.frame_id_, i);
00191     EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i));
00192   }
00193 
00194   cache.getData(ros::Time(), stor);
00195   EXPECT_EQ(stor.frame_id_, runs);
00196   EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(runs));
00197 
00198   stor.frame_id_ = runs;
00199   stor.stamp_ = ros::Time().fromNSec(runs+1);
00200   cache.insertData(stor);
00201 
00202 
00203   //Make sure we get a different value now that a new values is added at the front
00204   cache.getData(ros::Time(), stor);
00205   EXPECT_EQ(stor.frame_id_, runs);
00206   EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(runs+1));
00207   
00208 }
00209 
00210 TEST(TimeCache, CartesianInterpolation)
00211 {
00212   uint64_t runs = 100;
00213   double epsilon = 2e-6;
00214   seed_rand();
00215   
00216   tf2::TimeCache  cache;
00217   std::vector<double> xvalues(2);
00218   std::vector<double> yvalues(2);
00219   std::vector<double> zvalues(2);
00220 
00221   uint64_t offset = 200;
00222 
00223   TransformStorage stor;
00224   setIdentity(stor);
00225   
00226   for ( uint64_t i = 1; i < runs ; i++ )
00227   {
00228 
00229     for (uint64_t step = 0; step < 2 ; step++)
00230     {
00231       xvalues[step] = 10.0 * get_rand();
00232       yvalues[step] = 10.0 * get_rand();
00233       zvalues[step] = 10.0 * get_rand();
00234     
00235       stor.translation_.setValue(xvalues[step], yvalues[step], zvalues[step]);
00236       stor.frame_id_ = 2;
00237       stor.stamp_ = ros::Time().fromNSec(step * 100 + offset);
00238       cache.insertData(stor);
00239     }
00240     
00241     for (int pos = 0; pos < 100 ; pos ++)
00242     {
00243       cache.getData(ros::Time().fromNSec(offset + pos), stor);
00244       double x_out = stor.translation_.x();
00245       double y_out = stor.translation_.y();
00246       double z_out = stor.translation_.z();
00247       //      printf("pose %d, %f %f %f, expected %f %f %f\n", pos, x_out, y_out, z_out, 
00248       //       xvalues[0] + (xvalues[1] - xvalues[0]) * (double)pos/100.,
00249       //       yvalues[0] + (yvalues[1] - yvalues[0]) * (double)pos/100.0,
00250       //       zvalues[0] + (xvalues[1] - zvalues[0]) * (double)pos/100.0);
00251       EXPECT_NEAR(xvalues[0] + (xvalues[1] - xvalues[0]) * (double)pos/100.0, x_out, epsilon);
00252       EXPECT_NEAR(yvalues[0] + (yvalues[1] - yvalues[0]) * (double)pos/100.0, y_out, epsilon);
00253       EXPECT_NEAR(zvalues[0] + (zvalues[1] - zvalues[0]) * (double)pos/100.0, z_out, epsilon);
00254     }
00255     
00256 
00257     cache.clearList();
00258   }
00259 
00260   
00261 }
00262 
00264 TEST(TimeCache, ReparentingInterpolationProtection)
00265 {
00266   double epsilon = 1e-6;
00267   uint64_t offset = 555;
00268 
00269   seed_rand();
00270 
00271   tf2::TimeCache cache;
00272   std::vector<double> xvalues(2);
00273   std::vector<double> yvalues(2);
00274   std::vector<double> zvalues(2);
00275 
00276   TransformStorage stor;
00277   setIdentity(stor);
00278 
00279   for (uint64_t step = 0; step < 2 ; step++)
00280   {
00281     xvalues[step] = 10.0 * get_rand();
00282     yvalues[step] = 10.0 * get_rand();
00283     zvalues[step] = 10.0 * get_rand();
00284 
00285     stor.translation_.setValue(xvalues[step], yvalues[step], zvalues[step]);
00286     stor.frame_id_ = step + 4;
00287     stor.stamp_ = ros::Time().fromNSec(step * 100 + offset);
00288     cache.insertData(stor);
00289   }
00290   
00291   for (int pos = 0; pos < 100 ; pos ++)
00292   {
00293     EXPECT_TRUE(cache.getData(ros::Time().fromNSec(offset + pos), stor));
00294     double x_out = stor.translation_.x();
00295     double y_out = stor.translation_.y();
00296     double z_out = stor.translation_.z();
00297     EXPECT_NEAR(xvalues[0], x_out, epsilon);
00298     EXPECT_NEAR(yvalues[0], y_out, epsilon);
00299     EXPECT_NEAR(zvalues[0], z_out, epsilon);
00300   }
00301 }
00302 
00303 TEST(Bullet, Slerp)
00304 {
00305 
00306   uint64_t runs = 100;
00307   seed_rand();
00308 
00309   tf2::Quaternion q1, q2;
00310   q1.setEuler(0,0,0);
00311   
00312   for (uint64_t i = 0 ; i < runs ; i++)
00313   {
00314     q2.setEuler(1.0 * get_rand(),
00315                 1.0 * get_rand(),
00316                 1.0 * get_rand());
00317     
00318     
00319     tf2::Quaternion q3 = slerp(q1,q2,0.5);
00320     
00321     EXPECT_NEAR(q3.angle(q1), q2.angle(q3), 1e-5);
00322   }
00323 
00324 }
00325 
00326 
00327 TEST(TimeCache, AngularInterpolation)
00328 {
00329   uint64_t runs = 100;
00330   double epsilon = 1e-6;
00331   seed_rand();
00332   
00333   tf2::TimeCache  cache;
00334   std::vector<double> yawvalues(2);
00335   std::vector<double> pitchvalues(2);
00336   std::vector<double> rollvalues(2);
00337   uint64_t offset = 200;
00338 
00339   std::vector<tf2::Quaternion> quats(2);
00340 
00341   TransformStorage stor;
00342   setIdentity(stor);
00343   
00344   for ( uint64_t i = 1; i < runs ; i++ )
00345   {
00346 
00347     for (uint64_t step = 0; step < 2 ; step++)
00348     {
00349       yawvalues[step] = 10.0 * get_rand() / 100.0;
00350       pitchvalues[step] = 0;//10.0 * get_rand();
00351       rollvalues[step] = 0;//10.0 * get_rand();
00352       quats[step].setRPY(yawvalues[step], pitchvalues[step], rollvalues[step]);
00353       stor.rotation_ = quats[step];
00354       stor.frame_id_ = 3;
00355       stor.stamp_ = ros::Time().fromNSec(offset + (step * 100)); //step = 0 or 1
00356       cache.insertData(stor);
00357     }
00358     
00359     for (int pos = 0; pos < 100 ; pos ++)
00360     {
00361       EXPECT_TRUE(cache.getData(ros::Time().fromNSec(offset + pos), stor)); //get the transform for the position
00362       tf2::Quaternion quat (stor.rotation_);
00363 
00364       //Generate a ground truth quaternion directly calling slerp
00365       tf2::Quaternion ground_truth = quats[0].slerp(quats[1], pos/100.0);
00366       
00367       //Make sure the transformed one and the direct call match
00368       EXPECT_NEAR(0, angle(ground_truth, quat), epsilon);
00369             
00370     }
00371     
00372     cache.clearList();
00373   }
00374 
00375   
00376 }
00377 
00378 TEST(TimeCache, DuplicateEntries)
00379 {
00380 
00381   TimeCache cache;
00382 
00383   TransformStorage stor;
00384   setIdentity(stor);
00385   stor.frame_id_ = 3;
00386   stor.stamp_ = ros::Time().fromNSec(1);
00387 
00388   cache.insertData(stor);
00389 
00390   cache.insertData(stor);
00391 
00392 
00393   cache.getData(ros::Time().fromNSec(1), stor);
00394   
00395   //printf(" stor is %f\n", stor.translation_.x());
00396   EXPECT_TRUE(!std::isnan(stor.translation_.x()));
00397   EXPECT_TRUE(!std::isnan(stor.translation_.y()));
00398   EXPECT_TRUE(!std::isnan(stor.translation_.z()));
00399   EXPECT_TRUE(!std::isnan(stor.rotation_.x()));
00400   EXPECT_TRUE(!std::isnan(stor.rotation_.y()));
00401   EXPECT_TRUE(!std::isnan(stor.rotation_.z()));
00402   EXPECT_TRUE(!std::isnan(stor.rotation_.w()));
00403 }
00404 
00405 int main(int argc, char **argv){
00406   testing::InitGoogleTest(&argc, argv);
00407   return RUN_ALL_TESTS();
00408 }


tf2
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Thu Aug 27 2015 13:09:58