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       uint64_t time = offset + pos;
00244       cache.getData(ros::Time().fromNSec(time), stor);
00245       uint64_t time_out = stor.stamp_.toNSec();
00246       double x_out = stor.translation_.x();
00247       double y_out = stor.translation_.y();
00248       double z_out = stor.translation_.z();
00249       //      printf("pose %d, %f %f %f, expected %f %f %f\n", pos, x_out, y_out, z_out, 
00250       //       xvalues[0] + (xvalues[1] - xvalues[0]) * (double)pos/100.,
00251       //       yvalues[0] + (yvalues[1] - yvalues[0]) * (double)pos/100.0,
00252       //       zvalues[0] + (xvalues[1] - zvalues[0]) * (double)pos/100.0);
00253       EXPECT_EQ(time, time_out);
00254       EXPECT_NEAR(xvalues[0] + (xvalues[1] - xvalues[0]) * (double)pos/100.0, x_out, epsilon);
00255       EXPECT_NEAR(yvalues[0] + (yvalues[1] - yvalues[0]) * (double)pos/100.0, y_out, epsilon);
00256       EXPECT_NEAR(zvalues[0] + (zvalues[1] - zvalues[0]) * (double)pos/100.0, z_out, epsilon);
00257     }
00258     
00259 
00260     cache.clearList();
00261   }
00262 
00263   
00264 }
00265 
00267 TEST(TimeCache, ReparentingInterpolationProtection)
00268 {
00269   double epsilon = 1e-6;
00270   uint64_t offset = 555;
00271 
00272   seed_rand();
00273 
00274   tf2::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   setIdentity(stor);
00281 
00282   for (uint64_t step = 0; step < 2 ; step++)
00283   {
00284     xvalues[step] = 10.0 * get_rand();
00285     yvalues[step] = 10.0 * get_rand();
00286     zvalues[step] = 10.0 * get_rand();
00287 
00288     stor.translation_.setValue(xvalues[step], yvalues[step], zvalues[step]);
00289     stor.frame_id_ = step + 4;
00290     stor.stamp_ = ros::Time().fromNSec(step * 100 + offset);
00291     cache.insertData(stor);
00292   }
00293   
00294   for (int pos = 0; pos < 100 ; pos ++)
00295   {
00296     EXPECT_TRUE(cache.getData(ros::Time().fromNSec(offset + pos), stor));
00297     double x_out = stor.translation_.x();
00298     double y_out = stor.translation_.y();
00299     double z_out = stor.translation_.z();
00300     EXPECT_NEAR(xvalues[0], x_out, epsilon);
00301     EXPECT_NEAR(yvalues[0], y_out, epsilon);
00302     EXPECT_NEAR(zvalues[0], z_out, epsilon);
00303   }
00304 }
00305 
00306 TEST(Bullet, Slerp)
00307 {
00308 
00309   uint64_t runs = 100;
00310   seed_rand();
00311 
00312   tf2::Quaternion q1, q2;
00313   q1.setEuler(0,0,0);
00314   
00315   for (uint64_t i = 0 ; i < runs ; i++)
00316   {
00317     q2.setEuler(1.0 * get_rand(),
00318                 1.0 * get_rand(),
00319                 1.0 * get_rand());
00320     
00321     
00322     tf2::Quaternion q3 = slerp(q1,q2,0.5);
00323     
00324     EXPECT_NEAR(q3.angle(q1), q2.angle(q3), 1e-5);
00325   }
00326 
00327 }
00328 
00329 
00330 TEST(TimeCache, AngularInterpolation)
00331 {
00332   uint64_t runs = 100;
00333   double epsilon = 1e-6;
00334   seed_rand();
00335   
00336   tf2::TimeCache  cache;
00337   std::vector<double> yawvalues(2);
00338   std::vector<double> pitchvalues(2);
00339   std::vector<double> rollvalues(2);
00340   uint64_t offset = 200;
00341 
00342   std::vector<tf2::Quaternion> quats(2);
00343 
00344   TransformStorage stor;
00345   setIdentity(stor);
00346   
00347   for ( uint64_t i = 1; i < runs ; i++ )
00348   {
00349 
00350     for (uint64_t step = 0; step < 2 ; step++)
00351     {
00352       yawvalues[step] = 10.0 * get_rand() / 100.0;
00353       pitchvalues[step] = 0;//10.0 * get_rand();
00354       rollvalues[step] = 0;//10.0 * get_rand();
00355       quats[step].setRPY(yawvalues[step], pitchvalues[step], rollvalues[step]);
00356       stor.rotation_ = quats[step];
00357       stor.frame_id_ = 3;
00358       stor.stamp_ = ros::Time().fromNSec(offset + (step * 100)); //step = 0 or 1
00359       cache.insertData(stor);
00360     }
00361     
00362     for (int pos = 0; pos < 100 ; pos ++)
00363     {
00364       uint64_t time = offset + pos;
00365       cache.getData(ros::Time().fromNSec(time), stor);
00366       uint64_t time_out = stor.stamp_.toNSec();
00367       tf2::Quaternion quat (stor.rotation_);
00368 
00369       //Generate a ground truth quaternion directly calling slerp
00370       tf2::Quaternion ground_truth = quats[0].slerp(quats[1], pos/100.0);
00371       
00372       //Make sure the transformed one and the direct call match
00373       EXPECT_EQ(time, time_out);
00374       EXPECT_NEAR(0, angle(ground_truth, quat), epsilon);
00375             
00376     }
00377     
00378     cache.clearList();
00379   }
00380 
00381   
00382 }
00383 
00384 TEST(TimeCache, DuplicateEntries)
00385 {
00386 
00387   TimeCache cache;
00388 
00389   TransformStorage stor;
00390   setIdentity(stor);
00391   stor.frame_id_ = 3;
00392   stor.stamp_ = ros::Time().fromNSec(1);
00393 
00394   cache.insertData(stor);
00395 
00396   cache.insertData(stor);
00397 
00398 
00399   cache.getData(ros::Time().fromNSec(1), stor);
00400   
00401   //printf(" stor is %f\n", stor.translation_.x());
00402   EXPECT_TRUE(!std::isnan(stor.translation_.x()));
00403   EXPECT_TRUE(!std::isnan(stor.translation_.y()));
00404   EXPECT_TRUE(!std::isnan(stor.translation_.z()));
00405   EXPECT_TRUE(!std::isnan(stor.rotation_.x()));
00406   EXPECT_TRUE(!std::isnan(stor.rotation_.y()));
00407   EXPECT_TRUE(!std::isnan(stor.rotation_.z()));
00408   EXPECT_TRUE(!std::isnan(stor.rotation_.w()));
00409 }
00410 
00411 int main(int argc, char **argv){
00412   testing::InitGoogleTest(&argc, argv);
00413   return RUN_ALL_TESTS();
00414 }
00415 


tf2
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Tue Jul 10 2018 05:46:21