buffer_core_test.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/buffer_core.h>
00032 #include "tf2/exceptions.h"
00033 #include <sys/time.h>
00034 #include <ros/ros.h>
00035 #include "LinearMath/btVector3.h"
00036 #include "LinearMath/btTransform.h"
00037 #include "rostest/permuter.h"
00038 
00039 void seed_rand()
00040 {
00041   //Seed random number generator with current microseond count
00042   timeval temp_time_struct;
00043   gettimeofday(&temp_time_struct,NULL);
00044   srand(temp_time_struct.tv_usec);
00045 };
00046 
00047 void generate_rand_vectors(double scale, uint64_t runs, std::vector<double>& xvalues, std::vector<double>& yvalues, std::vector<double>&zvalues)
00048 {
00049   seed_rand();
00050   for ( uint64_t i = 0; i < runs ; i++ )
00051   {
00052     xvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00053     yvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00054     zvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00055   }
00056 }
00057 
00058 
00059 void setIdentity(geometry_msgs::Transform& trans) 
00060 {
00061   trans.translation.x = 0;
00062   trans.translation.y = 0;
00063   trans.translation.z = 0;
00064   trans.rotation.x = 0;
00065   trans.rotation.y = 0;
00066   trans.rotation.z = 0;
00067   trans.rotation.w = 1;
00068 }
00069 
00070 
00071 void push_back_i(std::vector<std::string>& children, std::vector<std::string>& parents, 
00072                  std::vector<double>& dx, std::vector<double>& dy)
00073 {
00074   /* 
00075      "a"
00076      v   (1,0)
00077      "b"
00078      v   (1,0)
00079      "c"
00080   */
00081     
00082   children.push_back("b");
00083   parents.push_back("a");
00084   dx.push_back(1.0);
00085   dy.push_back(0.0);
00086   children.push_back("c");
00087   parents.push_back("b");
00088   dx.push_back(1.0);
00089   dy.push_back(0.0);
00090 }
00091 
00092 
00093 void push_back_y(std::vector<std::string>& children, std::vector<std::string>& parents, 
00094                  std::vector<double>& dx, std::vector<double>& dy)
00095 {
00096     /*
00097       "a"
00098       v  (1,0)
00099       "b" ------(0,1)-----> "d"
00100       v  (1,0)              v  (0,1)
00101       "c"                   "e"
00102     */
00103     // a>b
00104     children.push_back("b");
00105     parents.push_back("a");
00106     dx.push_back(1.0);
00107     dy.push_back(0.0);
00108      // b>c
00109     children.push_back("c");
00110     parents.push_back("b");
00111     dx.push_back(1.0);
00112     dy.push_back(0.0);
00113      // b>d
00114     children.push_back("d");
00115     parents.push_back("b");
00116     dx.push_back(0.0);
00117     dy.push_back(1.0);
00118      // d>e
00119     children.push_back("e");
00120     parents.push_back("d");
00121     dx.push_back(0.0);
00122     dy.push_back(1.0);
00123 }
00124 
00125 void push_back_v(std::vector<std::string>& children, std::vector<std::string>& parents, 
00126                  std::vector<double>& dx, std::vector<double>& dy)
00127 {
00128   /*
00129     "a" ------(0,1)-----> "f"
00130     v  (1,0)              v  (0,1)
00131     "b"                   "g" 
00132     v  (1,0)
00133     "c"
00134   */
00135   // a>b
00136   children.push_back("b");
00137   parents.push_back("a");
00138   dx.push_back(1.0);
00139   dy.push_back(0.0);
00140   // b>c
00141   children.push_back("c");
00142   parents.push_back("b");
00143   dx.push_back(1.0);
00144   dy.push_back(0.0);
00145   // a>f
00146   children.push_back("f");
00147   parents.push_back("a");
00148   dx.push_back(0.0);
00149   dy.push_back(1.0);
00150   // f>g
00151   children.push_back("g");
00152   parents.push_back("f");
00153   dx.push_back(0.0);
00154   dy.push_back(1.0);
00155 
00156 }
00157 
00158 void push_back_1(std::vector<std::string>& children, std::vector<std::string>& parents, 
00159                  std::vector<double>& dx, std::vector<double>& dy)
00160 {
00161   children.push_back("2");
00162   parents.push_back("1");
00163   dx.push_back(1.0);
00164   dy.push_back(0.0);
00165 }
00166 
00167 void setupTree(tf2::BufferCore& mBC, const std::string& mode, const ros::Time & time, const ros::Duration& interpolation_space = ros::Duration())
00168 {
00169   ROS_DEBUG("Clearing Buffer Core for new test setup");
00170   mBC.clear();
00171   
00172   ROS_DEBUG("Setting up test tree for formation %s", mode.c_str());
00173 
00174   std::vector<std::string> children;
00175   std::vector<std::string> parents;
00176   std::vector<double> dx, dy;
00177   
00178   if (mode == "i")
00179   {
00180     push_back_i(children, parents, dx, dy);
00181   }
00182   else if (mode == "y")
00183   {
00184     push_back_y(children, parents, dx, dy);
00185   }
00186 
00187   else if (mode == "v")
00188   {
00189     push_back_v(children, parents, dx, dy);
00190   }
00191 
00192   else if (mode == "ring_45")
00193   {
00194     /* Form a ring of transforms at every 45 degrees on the unit circle.  */
00195 
00196     std::vector<std::string> frames;
00197         
00198 
00199     
00200     frames.push_back("a");
00201     frames.push_back("b");
00202     frames.push_back("c");
00203     frames.push_back("d");
00204     frames.push_back("e");
00205     frames.push_back("f");
00206     frames.push_back("g");
00207     frames.push_back("h");
00208     frames.push_back("i");
00209     
00210     for (uint8_t iteration = 0; iteration < 2; ++iteration)
00211     {
00212       double direction = 1;
00213       std::string frame_prefix;
00214       if (iteration == 0)
00215       {
00216         frame_prefix = "inverse_";
00217         direction = -1;
00218       }
00219       else
00220         frame_prefix ="";
00221       for (uint64_t i = 1; i <  frames.size(); i++)
00222       {
00223         geometry_msgs::TransformStamped ts;
00224         setIdentity(ts.transform);
00225         ts.transform.translation.x = direction * ( sqrt(2)/2 - 1);
00226         ts.transform.translation.y = direction * sqrt(2)/2;
00227         ts.transform.rotation.x = 0;
00228         ts.transform.rotation.y = 0;
00229         ts.transform.rotation.z = sin(direction * M_PI/8);
00230         ts.transform.rotation.w = cos(direction * M_PI/8);
00231         if (time > ros::Time() + (interpolation_space * .5))
00232           ts.header.stamp = time - (interpolation_space * .5);
00233         else
00234           ts.header.stamp = ros::Time();
00235             
00236         ts.header.frame_id = frame_prefix + frames[i-1];
00237         if (i > 1)
00238           ts.child_frame_id = frame_prefix + frames[i];
00239         else
00240           ts.child_frame_id = frames[i]; // connect first frame
00241         EXPECT_TRUE(mBC.setTransform(ts, "authority"));
00242         if (interpolation_space > ros::Duration())
00243         {
00244           ts.header.stamp = time + interpolation_space * .5;
00245           EXPECT_TRUE(mBC.setTransform(ts, "authority"));
00246         
00247         }
00248       }
00249     }
00250     return; // nonstandard setup return before standard executinog
00251   }  
00252   else if (mode == "1")
00253   {
00254     push_back_1(children, parents, dx, dy);
00255     
00256   }  
00257   else if (mode =="1_v")
00258   {
00259     push_back_1(children, parents, dx, dy);
00260     push_back_v(children, parents, dx, dy);
00261   }
00262   else
00263     EXPECT_FALSE("Undefined mode for tree setup.  Test harness improperly setup.");
00264 
00265 
00267   for (uint64_t i = 0; i <  children.size(); i++)
00268   {
00269     geometry_msgs::TransformStamped ts;
00270     setIdentity(ts.transform);
00271     ts.transform.translation.x = dx[i];
00272     ts.transform.translation.y = dy[i];
00273     if (time > ros::Time() + (interpolation_space * .5))
00274       ts.header.stamp = time - (interpolation_space * .5);
00275     else
00276       ts.header.stamp = ros::Time();
00277             
00278     ts.header.frame_id = parents[i];
00279     ts.child_frame_id = children[i];
00280     EXPECT_TRUE(mBC.setTransform(ts, "authority"));
00281     if (interpolation_space > ros::Duration())
00282     {
00283       ts.header.stamp = time + interpolation_space * .5;
00284       EXPECT_TRUE(mBC.setTransform(ts, "authority"));
00285       
00286     }
00287   }
00288 }
00289 
00290 
00291 TEST(BufferCore_setTransform, NoInsertOnSelfTransform)
00292 {
00293   tf2::BufferCore mBC;
00294   geometry_msgs::TransformStamped tranStamped;
00295   setIdentity(tranStamped.transform);
00296   tranStamped.header.stamp = ros::Time().fromNSec(10.0);
00297   tranStamped.header.frame_id = "same_frame";
00298   tranStamped.child_frame_id = "same_frame";
00299   EXPECT_FALSE(mBC.setTransform(tranStamped, "authority"));
00300 }
00301 
00302 TEST(BufferCore_setTransform, NoInsertWithNan)
00303 {
00304   tf2::BufferCore mBC;
00305   geometry_msgs::TransformStamped tranStamped;
00306   setIdentity(tranStamped.transform);
00307   tranStamped.header.stamp = ros::Time().fromNSec(10.0);
00308   tranStamped.header.frame_id = "same_frame";
00309   tranStamped.child_frame_id = "other_frame";
00310   EXPECT_TRUE(mBC.setTransform(tranStamped, "authority"));
00311   tranStamped.transform.translation.x = 0.0/0.0;
00312   EXPECT_TRUE(std::isnan(tranStamped.transform.translation.x));
00313   EXPECT_FALSE(mBC.setTransform(tranStamped, "authority"));
00314 
00315 }
00316 
00317 TEST(BufferCore_setTransform, NoInsertWithNoFrameID)
00318 {
00319   tf2::BufferCore mBC;
00320   geometry_msgs::TransformStamped tranStamped;
00321   setIdentity(tranStamped.transform);
00322   tranStamped.header.stamp = ros::Time().fromNSec(10.0);
00323   tranStamped.header.frame_id = "same_frame";
00324   tranStamped.child_frame_id = "";
00325   EXPECT_FALSE(mBC.setTransform(tranStamped, "authority"));
00326   tranStamped.child_frame_id = "/";
00327   EXPECT_FALSE(mBC.setTransform(tranStamped, "authority"));
00328 
00329 }
00330 
00331 TEST(BufferCore_setTransform, NoInsertWithNoParentID)
00332 {
00333   tf2::BufferCore mBC;
00334   geometry_msgs::TransformStamped tranStamped;
00335   setIdentity(tranStamped.transform);
00336   tranStamped.header.stamp = ros::Time().fromNSec(10.0);
00337   tranStamped.header.frame_id = "";
00338   tranStamped.child_frame_id = "some_frame";
00339   EXPECT_FALSE(mBC.setTransform(tranStamped, "authority"));
00340 
00341   tranStamped.header.frame_id = "/";
00342   EXPECT_FALSE(mBC.setTransform(tranStamped, "authority"));
00343 }
00344 
00345 /*
00346 TEST(tf, ListOneInverse)
00347 {
00348   unsigned int runs = 4;
00349   double epsilon = 1e-6;
00350   seed_rand();
00351   
00352   tf::Transformer mTR(true);
00353   std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
00354   for ( uint64_t i = 0; i < runs ; i++ )
00355   {
00356     xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00357     yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00358     zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00359 
00360     StampedTransform tranStamped (btTransform(btQuaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i),  "my_parent", "child");
00361     mTR.setTransform(tranStamped);
00362   }
00363 
00364   //  std::cout << mTR.allFramesAsString() << std::endl;
00365   //  std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl;
00366 
00367   for ( uint64_t i = 0; i < runs ; i++ )
00368 
00369   {
00370     Stamped<Pose> inpose (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(10 + i), "child");
00371 
00372     try{
00373     Stamped<Pose> outpose;
00374     outpose.setIdentity(); //to make sure things are getting mutated
00375     mTR.transformPose("my_parent",inpose, outpose);
00376     EXPECT_NEAR(outpose.getOrigin().x(), xvalues[i], epsilon);
00377     EXPECT_NEAR(outpose.getOrigin().y(), yvalues[i], epsilon);
00378     EXPECT_NEAR(outpose.getOrigin().z(), zvalues[i], epsilon);
00379     }
00380     catch (tf::TransformException & ex)
00381     {
00382       std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl;
00383       bool exception_improperly_thrown = true;
00384       EXPECT_FALSE(exception_improperly_thrown);
00385     }
00386   }
00387   
00388 }
00389 
00390 TEST(tf, ListTwoInverse)
00391 {
00392   unsigned int runs = 4;
00393   double epsilon = 1e-6;
00394   seed_rand();
00395   
00396   tf::Transformer mTR(true);
00397   std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
00398   for ( unsigned int i = 0; i < runs ; i++ )
00399   {
00400     xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00401     yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00402     zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00403 
00404     StampedTransform tranStamped(btTransform(btQuaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i),  "my_parent", "child");
00405     mTR.setTransform(tranStamped);
00406     StampedTransform tranStamped2(btTransform(btQuaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i),  "child", "grandchild");
00407     mTR.setTransform(tranStamped2);
00408   }
00409 
00410   //  std::cout << mTR.allFramesAsString() << std::endl;
00411   //  std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl;
00412 
00413   for ( unsigned int i = 0; i < runs ; i++ )
00414 
00415   {
00416     Stamped<Pose> inpose (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(10 + i), "grandchild");
00417 
00418     try{
00419     Stamped<Pose> outpose;
00420     outpose.setIdentity(); //to make sure things are getting mutated
00421     mTR.transformPose("my_parent",inpose, outpose);
00422     EXPECT_NEAR(outpose.getOrigin().x(), 2*xvalues[i], epsilon);
00423     EXPECT_NEAR(outpose.getOrigin().y(), 2*yvalues[i], epsilon);
00424     EXPECT_NEAR(outpose.getOrigin().z(), 2*zvalues[i], epsilon);
00425     }
00426     catch (tf::TransformException & ex)
00427     {
00428       std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl;
00429       bool exception_improperly_thrown = true;
00430       EXPECT_FALSE(exception_improperly_thrown);
00431     }
00432   }
00433   
00434 }
00435 
00436 
00437 TEST(tf, ListOneForward)
00438 {
00439   unsigned int runs = 4;
00440   double epsilon = 1e-6;
00441   seed_rand();
00442   
00443   tf::Transformer mTR(true);
00444   std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
00445   for ( uint64_t i = 0; i < runs ; i++ )
00446   {
00447     xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00448     yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00449     zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00450 
00451     StampedTransform tranStamped(btTransform(btQuaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i),  "my_parent", "child");
00452     mTR.setTransform(tranStamped);
00453   }
00454 
00455   //  std::cout << mTR.allFramesAsString() << std::endl;
00456   //  std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl;
00457 
00458   for ( uint64_t i = 0; i < runs ; i++ )
00459 
00460   {
00461     Stamped<Pose> inpose (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(10 + i), "my_parent");
00462 
00463     try{
00464     Stamped<Pose> outpose;
00465     outpose.setIdentity(); //to make sure things are getting mutated
00466     mTR.transformPose("child",inpose, outpose);
00467     EXPECT_NEAR(outpose.getOrigin().x(), -xvalues[i], epsilon);
00468     EXPECT_NEAR(outpose.getOrigin().y(), -yvalues[i], epsilon);
00469     EXPECT_NEAR(outpose.getOrigin().z(), -zvalues[i], epsilon);
00470     }
00471     catch (tf::TransformException & ex)
00472     {
00473       std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl;
00474       bool exception_improperly_thrown = true;
00475       EXPECT_FALSE(exception_improperly_thrown);
00476     }
00477   }
00478   
00479 }
00480 
00481 TEST(tf, ListTwoForward)
00482 {
00483   unsigned int runs = 4;
00484   double epsilon = 1e-6;
00485   seed_rand();
00486   
00487   tf::Transformer mTR(true);
00488   std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
00489   for ( unsigned int i = 0; i < runs ; i++ )
00490   {
00491     xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00492     yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00493     zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00494 
00495     StampedTransform tranStamped(btTransform(btQuaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i),  "my_parent", "child");
00496     mTR.setTransform(tranStamped);
00497     StampedTransform tranStamped2(btTransform(btQuaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i),  "child", "grandchild");
00498     mTR.setTransform(tranStamped2);
00499   }
00500 
00501   //  std::cout << mTR.allFramesAsString() << std::endl;
00502   //  std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl;
00503 
00504   for ( unsigned int i = 0; i < runs ; i++ )
00505 
00506   {
00507     Stamped<Pose> inpose (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(10 + i), "my_parent");
00508 
00509     try{
00510     Stamped<Pose> outpose;
00511     outpose.setIdentity(); //to make sure things are getting mutated
00512     mTR.transformPose("grandchild",inpose, outpose);
00513     EXPECT_NEAR(outpose.getOrigin().x(), -2*xvalues[i], epsilon);
00514     EXPECT_NEAR(outpose.getOrigin().y(), -2*yvalues[i], epsilon);
00515     EXPECT_NEAR(outpose.getOrigin().z(), -2*zvalues[i], epsilon);
00516     }
00517     catch (tf::TransformException & ex)
00518     {
00519       std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl;
00520       bool exception_improperly_thrown = true;
00521       EXPECT_FALSE(exception_improperly_thrown);
00522     }
00523   }
00524   
00525 }
00526 
00527 TEST(tf, TransformThrougRoot)
00528 {
00529   unsigned int runs = 4;
00530   double epsilon = 1e-6;
00531   seed_rand();
00532   
00533   tf::Transformer mTR(true);
00534   std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
00535   for ( unsigned int i = 0; i < runs ; i++ )
00536   {
00537     xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00538     yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00539     zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00540 
00541     StampedTransform tranStamped(btTransform(btQuaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(1000 + i*100),  "my_parent", "childA");
00542     mTR.setTransform(tranStamped);
00543     StampedTransform tranStamped2(btTransform(btQuaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(1000 + i*100),  "my_parent", "childB");
00544     mTR.setTransform(tranStamped2);
00545   }
00546 
00547   //  std::cout << mTR.allFramesAsString() << std::endl;
00548   //  std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl;
00549 
00550   for ( unsigned int i = 0; i < runs ; i++ )
00551 
00552   {
00553     Stamped<Pose> inpose (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(1000 + i*100), "childA");
00554 
00555     try{
00556     Stamped<Pose> outpose;
00557     outpose.setIdentity(); //to make sure things are getting mutated
00558     mTR.transformPose("childB",inpose, outpose);
00559     EXPECT_NEAR(outpose.getOrigin().x(), 0*xvalues[i], epsilon);
00560     EXPECT_NEAR(outpose.getOrigin().y(), 0*yvalues[i], epsilon);
00561     EXPECT_NEAR(outpose.getOrigin().z(), 0*zvalues[i], epsilon);
00562     }
00563     catch (tf::TransformException & ex)
00564     {
00565       std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl;
00566       bool exception_improperly_thrown = true;
00567       EXPECT_FALSE(exception_improperly_thrown);
00568     }
00569   }
00570   
00571 }
00572 
00573 TEST(tf, TransformThroughNO_PARENT)
00574 {
00575   unsigned int runs = 4;
00576   double epsilon = 1e-6;
00577   seed_rand();
00578   
00579   tf::Transformer mTR(true);
00580   std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
00581   for ( unsigned int i = 0; i < runs ; i++ )
00582   {
00583     xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00584     yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00585     zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00586 
00587     StampedTransform tranStamped(btTransform(btQuaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i),  "my_parentA", "childA");
00588     mTR.setTransform(tranStamped);
00589     StampedTransform tranStamped2(btTransform(btQuaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i),  "my_parentB", "childB");
00590     mTR.setTransform(tranStamped2);
00591   }
00592 
00593   //  std::cout << mTR.allFramesAsString() << std::endl;
00594   //  std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl;
00595 
00596   for ( unsigned int i = 0; i < runs ; i++ )
00597 
00598   {
00599     Stamped<btTransform> inpose (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(10 + i), "childA");
00600     bool exception_thrown = false;
00601 
00602     try{
00603     Stamped<btTransform> outpose;
00604     outpose.setIdentity(); //to make sure things are getting mutated
00605     mTR.transformPose("childB",inpose, outpose);
00606     EXPECT_NEAR(outpose.getOrigin().x(), 0*xvalues[i], epsilon);
00607     EXPECT_NEAR(outpose.getOrigin().y(), 0*yvalues[i], epsilon);
00608     EXPECT_NEAR(outpose.getOrigin().z(), 0*zvalues[i], epsilon);
00609     }
00610     catch (tf::TransformException & ex)
00611     {
00612       exception_thrown = true;
00613     }
00614     EXPECT_TRUE(exception_thrown);
00615   }
00616   
00617 }
00618 
00619 */
00620 
00621 
00622 TEST(BufferCore_lookupTransform, i_configuration)
00623 {
00624   double epsilon = 1e-6;
00625   
00626 
00627 
00628   rostest::Permuter permuter;
00629 
00630   std::vector<ros::Time> times;
00631   times.push_back(ros::Time(1.0));
00632   times.push_back(ros::Time(10.0));
00633   times.push_back(ros::Time(0.0));
00634   ros::Time eval_time;
00635   permuter.addOptionSet(times, &eval_time);
00636 
00637   std::vector<ros::Duration> durations;
00638   durations.push_back(ros::Duration(1.0));
00639   durations.push_back(ros::Duration(0.001));
00640   durations.push_back(ros::Duration(0.1));
00641   ros::Duration interpolation_space;
00642   //  permuter.addOptionSet(durations, &interpolation_space);
00643 
00644   std::vector<std::string> frames;
00645   frames.push_back("a");
00646   frames.push_back("b");
00647   frames.push_back("c");
00648   std::string source_frame;
00649   permuter.addOptionSet(frames, &source_frame);
00650   
00651   std::string target_frame;
00652   permuter.addOptionSet(frames, &target_frame);
00653 
00654   while  (permuter.step())
00655   {
00656 
00657     tf2::BufferCore mBC;
00658     setupTree(mBC, "i", eval_time, interpolation_space);
00659 
00660     geometry_msgs::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, eval_time);
00661     //printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec());  
00662     EXPECT_EQ(outpose.header.stamp, eval_time);
00663     EXPECT_EQ(outpose.header.frame_id, source_frame);
00664     EXPECT_EQ(outpose.child_frame_id, target_frame);
00665     EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon);
00666     EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
00667     EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
00668     EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
00669     EXPECT_NEAR(outpose.transform.rotation.z, 0, epsilon);
00670     EXPECT_NEAR(outpose.transform.rotation.w, 1, epsilon);
00671     
00672     //Zero distance
00673     if (source_frame == target_frame)
00674     {
00675       EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon);
00676     }
00677     else if ((source_frame == "a" && target_frame =="b") ||
00678              (source_frame == "b" && target_frame =="c"))
00679     {
00680       EXPECT_NEAR(outpose.transform.translation.x, 1, epsilon);
00681     }
00682     else if ((source_frame == "b" && target_frame =="a") ||
00683              (source_frame == "c" && target_frame =="b"))
00684     {
00685       EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
00686     }
00687     else if (source_frame == "a" && target_frame =="c")
00688     {
00689       EXPECT_NEAR(outpose.transform.translation.x, 2, epsilon);
00690     }
00691     else if (source_frame == "c" && target_frame =="a")
00692     {
00693       EXPECT_NEAR(outpose.transform.translation.x, -2, epsilon);
00694     }
00695     else
00696     {
00697       EXPECT_FALSE("i configuration: Shouldn't get here");
00698       printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec());
00699     }
00700     
00701   }
00702 }
00703 
00704 /* Check 1 result return false if test parameters unmet */
00705 bool check_1_result(const geometry_msgs::TransformStamped& outpose, const std::string& source_frame, const std::string& target_frame, const ros::Time& eval_time, double epsilon)
00706 {
00707   //printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec());  
00708   EXPECT_EQ(outpose.header.stamp, eval_time);
00709   EXPECT_EQ(outpose.header.frame_id, source_frame);
00710   EXPECT_EQ(outpose.child_frame_id, target_frame);
00711   EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon);
00712   EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
00713   EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
00714   EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
00715   EXPECT_NEAR(outpose.transform.rotation.z, 0, epsilon);
00716   EXPECT_NEAR(outpose.transform.rotation.w, 1, epsilon);
00717     
00718   //Zero distance
00719   if (source_frame == target_frame)
00720   {
00721     EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon);
00722   }
00723   else if (source_frame == "1" && target_frame =="2")
00724   {
00725     EXPECT_NEAR(outpose.transform.translation.x, 1, epsilon);
00726   }
00727   else if (source_frame == "2" && target_frame =="1")
00728   {
00729     EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
00730   }
00731   else
00732   {
00733     //printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec());
00734     return false;
00735   }
00736   return true;
00737 }
00738 
00739 /* Check v result return false if test parameters unmet */
00740 bool check_v_result(const geometry_msgs::TransformStamped& outpose, const std::string& source_frame, const std::string& target_frame, const ros::Time& eval_time, double epsilon)
00741 {
00742   //printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec());  
00743   EXPECT_EQ(outpose.header.stamp, eval_time);
00744   EXPECT_EQ(outpose.header.frame_id, source_frame);
00745   EXPECT_EQ(outpose.child_frame_id, target_frame);
00746   EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
00747   EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
00748   EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
00749   EXPECT_NEAR(outpose.transform.rotation.z, 0, epsilon);
00750   EXPECT_NEAR(outpose.transform.rotation.w, 1, epsilon);
00751     
00752   //Zero distance
00753   if (source_frame == target_frame)
00754   {
00755     EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon);
00756   }
00757   else if ((source_frame == "a" && target_frame =="b") ||
00758            (source_frame == "b" && target_frame =="c"))
00759   {
00760     EXPECT_NEAR(outpose.transform.translation.x, 1, epsilon);
00761     EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon);
00762   }
00763   else if ((source_frame == "b" && target_frame =="a") ||
00764            (source_frame == "c" && target_frame =="b"))
00765   {
00766     EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
00767     EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon);
00768   }
00769   else if ((source_frame == "a" && target_frame =="f") ||
00770            (source_frame == "f" && target_frame =="g"))
00771   {
00772     EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon);
00773     EXPECT_NEAR(outpose.transform.translation.y, 1, epsilon);
00774   }
00775   else if ((source_frame == "f" && target_frame =="a") ||
00776            (source_frame == "g" && target_frame =="f"))
00777   {
00778     EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon);
00779     EXPECT_NEAR(outpose.transform.translation.y, -1, epsilon);
00780   }
00781   else if (source_frame == "a" && target_frame =="g")
00782   {
00783     EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon);
00784     EXPECT_NEAR(outpose.transform.translation.y, 2, epsilon);
00785   }
00786   else if (source_frame == "g" && target_frame =="a")
00787   {
00788     EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon);
00789     EXPECT_NEAR(outpose.transform.translation.y, -2, epsilon);
00790   }
00791   else if (source_frame == "a" && target_frame =="c")
00792   {
00793     EXPECT_NEAR(outpose.transform.translation.x, 2, epsilon);
00794     EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon);
00795   }
00796   else if (source_frame == "c" && target_frame =="a")
00797   {
00798     EXPECT_NEAR(outpose.transform.translation.x, -2, epsilon);
00799     EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon);
00800   }
00801   else if (source_frame == "b" && target_frame =="f")
00802   {
00803     EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
00804     EXPECT_NEAR(outpose.transform.translation.y, 1, epsilon);
00805   }
00806   else if (source_frame == "f" && target_frame =="b")
00807   {
00808     EXPECT_NEAR(outpose.transform.translation.x, 1, epsilon);
00809     EXPECT_NEAR(outpose.transform.translation.y, -1, epsilon);
00810   }
00811   else if (source_frame == "c" && target_frame =="f")
00812   {
00813     EXPECT_NEAR(outpose.transform.translation.x, -2, epsilon);
00814     EXPECT_NEAR(outpose.transform.translation.y, 1, epsilon);
00815   }
00816   else if (source_frame == "f" && target_frame =="c")
00817   {
00818     EXPECT_NEAR(outpose.transform.translation.x, 2, epsilon);
00819     EXPECT_NEAR(outpose.transform.translation.y, -1, epsilon);
00820   }
00821   else if (source_frame == "b" && target_frame =="g")
00822   {
00823     EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
00824     EXPECT_NEAR(outpose.transform.translation.y, 2, epsilon);
00825   }
00826   else if (source_frame == "g" && target_frame =="b")
00827   {
00828     EXPECT_NEAR(outpose.transform.translation.x, 1, epsilon);
00829     EXPECT_NEAR(outpose.transform.translation.y, -2, epsilon);
00830   }
00831   else if (source_frame == "c" && target_frame =="g")
00832   {
00833     EXPECT_NEAR(outpose.transform.translation.x, -2, epsilon);
00834     EXPECT_NEAR(outpose.transform.translation.y, 2, epsilon);
00835   }
00836   else if (source_frame == "g" && target_frame =="c")
00837   {
00838     EXPECT_NEAR(outpose.transform.translation.x, 2, epsilon);
00839     EXPECT_NEAR(outpose.transform.translation.y, -2, epsilon);
00840   }
00841   else
00842   {
00843     //printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec());
00844     return false;
00845   }
00846   return true;
00847 }
00848 
00849 /* Check v result return false if test parameters unmet */
00850 bool check_y_result(const geometry_msgs::TransformStamped& outpose, const std::string& source_frame, const std::string& target_frame, const ros::Time& eval_time, double epsilon)
00851 {
00852   //printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec());  
00853   EXPECT_EQ(outpose.header.stamp, eval_time);
00854   EXPECT_EQ(outpose.header.frame_id, source_frame);
00855   EXPECT_EQ(outpose.child_frame_id, target_frame);
00856   EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
00857   EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
00858   EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
00859   EXPECT_NEAR(outpose.transform.rotation.z, 0, epsilon);
00860   EXPECT_NEAR(outpose.transform.rotation.w, 1, epsilon);
00861     
00862   //Zero distance
00863   if (source_frame == target_frame)
00864   {
00865     EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon);
00866   }
00867   else if ((source_frame == "a" && target_frame =="b") ||
00868            (source_frame == "b" && target_frame =="c"))
00869   {
00870     EXPECT_NEAR(outpose.transform.translation.x, 1, epsilon);
00871     EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon);
00872   }
00873   else if ((source_frame == "b" && target_frame =="a") ||
00874            (source_frame == "c" && target_frame =="b"))
00875   {
00876     EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
00877     EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon);
00878   }
00879   else if ((source_frame == "b" && target_frame =="d") ||
00880            (source_frame == "d" && target_frame =="e"))
00881   {
00882     EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon);
00883     EXPECT_NEAR(outpose.transform.translation.y, 1, epsilon);
00884   }
00885   else if ((source_frame == "d" && target_frame =="b") ||
00886            (source_frame == "e" && target_frame =="d"))
00887   {
00888     EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon);
00889     EXPECT_NEAR(outpose.transform.translation.y, -1, epsilon);
00890   }
00891   else if (source_frame == "b" && target_frame =="e")
00892   {
00893     EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon);
00894     EXPECT_NEAR(outpose.transform.translation.y, 2, epsilon);
00895   }
00896   else if (source_frame == "e" && target_frame =="b")
00897   {
00898     EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon);
00899     EXPECT_NEAR(outpose.transform.translation.y, -2, epsilon);
00900   }
00901   else if (source_frame == "a" && target_frame =="c")
00902   {
00903     EXPECT_NEAR(outpose.transform.translation.x, 2, epsilon);
00904     EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon);
00905   }
00906   else if (source_frame == "c" && target_frame =="a")
00907   {
00908     EXPECT_NEAR(outpose.transform.translation.x, -2, epsilon);
00909     EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon);
00910   }
00911   else if (source_frame == "a" && target_frame =="d")
00912   {
00913     EXPECT_NEAR(outpose.transform.translation.x, 1, epsilon);
00914     EXPECT_NEAR(outpose.transform.translation.y, 1, epsilon);
00915   }
00916   else if (source_frame == "d" && target_frame =="a")
00917   {
00918     EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
00919     EXPECT_NEAR(outpose.transform.translation.y, -1, epsilon);
00920   }
00921   else if (source_frame == "c" && target_frame =="d")
00922   {
00923     EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
00924     EXPECT_NEAR(outpose.transform.translation.y, 1, epsilon);
00925   }
00926   else if (source_frame == "d" && target_frame =="c")
00927   {
00928     EXPECT_NEAR(outpose.transform.translation.x, 1, epsilon);
00929     EXPECT_NEAR(outpose.transform.translation.y, -1, epsilon);
00930   }
00931   else if (source_frame == "a" && target_frame =="e")
00932   {
00933     EXPECT_NEAR(outpose.transform.translation.x, 1, epsilon);
00934     EXPECT_NEAR(outpose.transform.translation.y, 2, epsilon);
00935   }
00936   else if (source_frame == "e" && target_frame =="a")
00937   {
00938     EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
00939     EXPECT_NEAR(outpose.transform.translation.y, -2, epsilon);
00940   }
00941   else if (source_frame == "c" && target_frame =="e")
00942   {
00943     EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
00944     EXPECT_NEAR(outpose.transform.translation.y, 2, epsilon);
00945   }
00946   else if (source_frame == "e" && target_frame =="c")
00947   {
00948     EXPECT_NEAR(outpose.transform.translation.x, 1, epsilon);
00949     EXPECT_NEAR(outpose.transform.translation.y, -2, epsilon);
00950   }
00951   else
00952   {
00953     //printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec());
00954     return false;
00955   }
00956   return true;
00957 }
00958 
00959 
00960 TEST(BufferCore_lookupTransform, one_link_configuration)
00961 {
00962   double epsilon = 1e-6;
00963   
00964 
00965 
00966   rostest::Permuter permuter;
00967 
00968   std::vector<ros::Time> times;
00969   times.push_back(ros::Time(1.0));
00970   times.push_back(ros::Time(10.0));
00971   times.push_back(ros::Time(0.0));
00972   ros::Time eval_time;
00973   permuter.addOptionSet(times, &eval_time);
00974 
00975   std::vector<ros::Duration> durations;
00976   durations.push_back(ros::Duration(1.0));
00977   durations.push_back(ros::Duration(0.001));
00978   durations.push_back(ros::Duration(0.1));
00979   ros::Duration interpolation_space;
00980   //  permuter.addOptionSet(durations, &interpolation_space);
00981 
00982   std::vector<std::string> frames;
00983   frames.push_back("1");
00984   frames.push_back("2");
00985   std::string source_frame;
00986   permuter.addOptionSet(frames, &source_frame);
00987   
00988   std::string target_frame;
00989   permuter.addOptionSet(frames, &target_frame);
00990 
00991   while  (permuter.step())
00992   {
00993 
00994     tf2::BufferCore mBC;
00995     setupTree(mBC, "1", eval_time, interpolation_space);
00996 
00997     geometry_msgs::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, eval_time);
00998 
00999     EXPECT_TRUE(check_1_result(outpose, source_frame, target_frame, eval_time, epsilon));
01000   }
01001 }
01002 
01003 
01004 TEST(BufferCore_lookupTransform, v_configuration)
01005 {
01006   double epsilon = 1e-6;
01007   
01008 
01009 
01010   rostest::Permuter permuter;
01011 
01012   std::vector<ros::Time> times;
01013   times.push_back(ros::Time(1.0));
01014   times.push_back(ros::Time(10.0));
01015   times.push_back(ros::Time(0.0));
01016   ros::Time eval_time;
01017   permuter.addOptionSet(times, &eval_time);
01018 
01019   std::vector<ros::Duration> durations;
01020   durations.push_back(ros::Duration(1.0));
01021   durations.push_back(ros::Duration(0.001));
01022   durations.push_back(ros::Duration(0.1));
01023   ros::Duration interpolation_space;
01024   //  permuter.addOptionSet(durations, &interpolation_space);
01025 
01026   std::vector<std::string> frames;
01027   frames.push_back("a");
01028   frames.push_back("b");
01029   frames.push_back("c");
01030   frames.push_back("f");
01031   frames.push_back("g");
01032   std::string source_frame;
01033   permuter.addOptionSet(frames, &source_frame);
01034   
01035   std::string target_frame;
01036   permuter.addOptionSet(frames, &target_frame);
01037 
01038   while  (permuter.step())
01039   {
01040 
01041     tf2::BufferCore mBC;
01042     setupTree(mBC, "v", eval_time, interpolation_space);
01043 
01044     geometry_msgs::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, eval_time);
01045 
01046     EXPECT_TRUE(check_v_result(outpose, source_frame, target_frame, eval_time, epsilon));
01047   }
01048 }
01049 
01050 
01051 TEST(BufferCore_lookupTransform, y_configuration)
01052 {
01053   double epsilon = 1e-6;
01054   
01055 
01056 
01057   rostest::Permuter permuter;
01058 
01059   std::vector<ros::Time> times;
01060   times.push_back(ros::Time(1.0));
01061   times.push_back(ros::Time(10.0));
01062   times.push_back(ros::Time(0.0));
01063   ros::Time eval_time;
01064   permuter.addOptionSet(times, &eval_time);
01065 
01066   std::vector<ros::Duration> durations;
01067   durations.push_back(ros::Duration(1.0));
01068   durations.push_back(ros::Duration(0.001));
01069   durations.push_back(ros::Duration(0.1));
01070   ros::Duration interpolation_space;
01071   //  permuter.addOptionSet(durations, &interpolation_space);
01072 
01073   std::vector<std::string> frames;
01074   frames.push_back("a");
01075   frames.push_back("b");
01076   frames.push_back("c");
01077   frames.push_back("d");
01078   frames.push_back("e");
01079   std::string source_frame;
01080   permuter.addOptionSet(frames, &source_frame);
01081   
01082   std::string target_frame;
01083   permuter.addOptionSet(frames, &target_frame);
01084 
01085   while  (permuter.step())
01086   {
01087 
01088     tf2::BufferCore mBC;
01089     setupTree(mBC, "y", eval_time, interpolation_space);
01090 
01091     geometry_msgs::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, eval_time);
01092 
01093     EXPECT_TRUE(check_y_result(outpose, source_frame, target_frame, eval_time, epsilon));
01094   }
01095 }
01096 
01097 TEST(BufferCore_lookupTransform, multi_configuration)
01098 {
01099   double epsilon = 1e-6;
01100   
01101 
01102 
01103   rostest::Permuter permuter;
01104 
01105   std::vector<ros::Time> times;
01106   times.push_back(ros::Time(1.0));
01107   times.push_back(ros::Time(10.0));
01108   times.push_back(ros::Time(0.0));
01109   ros::Time eval_time;
01110   permuter.addOptionSet(times, &eval_time);
01111 
01112   std::vector<ros::Duration> durations;
01113   durations.push_back(ros::Duration(1.0));
01114   durations.push_back(ros::Duration(0.001));
01115   durations.push_back(ros::Duration(0.1));
01116   ros::Duration interpolation_space;
01117   //  permuter.addOptionSet(durations, &interpolation_space);
01118 
01119   std::vector<std::string> frames;
01120   frames.push_back("1");
01121   frames.push_back("2");
01122   frames.push_back("a");
01123   frames.push_back("b");
01124   frames.push_back("c");
01125   frames.push_back("f");
01126   frames.push_back("g");
01127   std::string source_frame;
01128   permuter.addOptionSet(frames, &source_frame);
01129   
01130   std::string target_frame;
01131   permuter.addOptionSet(frames, &target_frame);
01132 
01133   while  (permuter.step())
01134   {
01135 
01136     tf2::BufferCore mBC;
01137     setupTree(mBC, "1_v", eval_time, interpolation_space);
01138 
01139     if (mBC.canTransform(source_frame, target_frame, eval_time))
01140     {
01141       geometry_msgs::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, eval_time);
01142       
01143       if ((source_frame == "1" || source_frame =="2") && (target_frame =="1" || target_frame == "2"))
01144         EXPECT_TRUE(check_1_result(outpose, source_frame, target_frame, eval_time, epsilon));
01145       else if ((source_frame == "a" || source_frame == "b" || source_frame == "c" || source_frame == "f" || source_frame == "g") && 
01146                (target_frame == "a" || target_frame == "b" || target_frame == "c" || target_frame == "f" || target_frame == "g"))
01147         EXPECT_TRUE(check_v_result(outpose, source_frame, target_frame, eval_time, epsilon));
01148       else
01149         EXPECT_FALSE("Frames unhandled");
01150     }
01151     else
01152       EXPECT_TRUE(((source_frame == "a" || source_frame =="b" || source_frame == "c" || source_frame == "f" || source_frame == "g") && 
01153                    (target_frame == "1" || target_frame == "2") )
01154                   || 
01155                   ((target_frame == "a" || target_frame =="b" || target_frame == "c" || target_frame == "f" || target_frame == "g") && 
01156                    (source_frame == "1" || source_frame == "2"))
01157                   );
01158       
01159   }      
01160 }
01161 
01162 #define CHECK_QUATERNION_NEAR(_q1, _x, _y, _z, _w, _epsilon)                 \
01163            {                                                                                                             \
01164            btQuaternion q1(_q1.x, _q1.y, _q1.z, _q1.w);                          \
01165        btQuaternion q2(_x, _y, _z, _w);                                      \
01166        double angle = q1.angle(q2);                                          \
01167            EXPECT_TRUE(fabs(angle) < _epsilon || fabs(angle - M_PI) < _epsilon); \
01168            }
01169 
01170 #define CHECK_TRANSFORMS_NEAR(_out, _expected, _eps)                                                                                                                                                                                                                        \
01171         EXPECT_NEAR(_out.transform.translation.x, _expected.getOrigin().x(), epsilon);                                                                                                                                                          \
01172         EXPECT_NEAR(_out.transform.translation.y, _expected.getOrigin().y(), epsilon);                                                                                                                                                                  \
01173         EXPECT_NEAR(_out.transform.translation.z, _expected.getOrigin().z(), epsilon);                                                                                                                                                          \
01174         CHECK_QUATERNION_NEAR(_out.transform.rotation, _expected.getRotation().x(), _expected.getRotation().y(), _expected.getRotation().z(), _expected.getRotation().w(), _eps);
01175 
01176 
01177 // Simple test with compound transform
01178 TEST(BufferCore_lookupTransform, compound_xfm_configuration)
01179 {
01180         /*
01181          * Frames
01182          *
01183          * root->a
01184          *
01185          * root->b->c->d
01186          *
01187          */
01188 
01189         double epsilon = 2e-5; // Larger epsilon for interpolation values
01190 
01191     tf2::BufferCore mBC;
01192 
01193     geometry_msgs::TransformStamped tsa;
01194     tsa.header.frame_id = "root";
01195     tsa.child_frame_id  = "a";
01196     tsa.transform.translation.x = 1.0;
01197     tsa.transform.translation.y = 1.0;
01198     tsa.transform.translation.z = 1.0;
01199     btQuaternion q1;
01200     q1.setEuler(0.25, .5, .75);
01201     tsa.transform.rotation.x = q1.x();
01202     tsa.transform.rotation.y = q1.y();
01203     tsa.transform.rotation.z = q1.z();
01204     tsa.transform.rotation.w = q1.w();
01205     EXPECT_TRUE(mBC.setTransform(tsa, "authority"));
01206 
01207     geometry_msgs::TransformStamped tsb;
01208     tsb.header.frame_id = "root";
01209     tsb.child_frame_id  = "b";
01210     tsb.transform.translation.x = -1.0;
01211     tsb.transform.translation.y =  0.0;
01212     tsb.transform.translation.z = -1.0;
01213     btQuaternion q2;
01214     q2.setEuler(1.0, 0.25, 0.5);
01215     tsb.transform.rotation.x = q2.x();
01216     tsb.transform.rotation.y = q2.y();
01217     tsb.transform.rotation.z = q2.z();
01218     tsb.transform.rotation.w = q2.w();
01219     EXPECT_TRUE(mBC.setTransform(tsb, "authority"));
01220 
01221     geometry_msgs::TransformStamped tsc;
01222     tsc.header.frame_id = "b";
01223     tsc.child_frame_id  = "c";
01224     tsc.transform.translation.x =  0.0;
01225     tsc.transform.translation.y =  2.0;
01226     tsc.transform.translation.z =  0.5;
01227     btQuaternion q3;
01228     q3.setEuler(0.25, .75, 1.25);
01229     tsc.transform.rotation.x = q3.x();
01230     tsc.transform.rotation.y = q3.y();
01231     tsc.transform.rotation.z = q3.z();
01232     tsc.transform.rotation.w = q3.w();
01233     EXPECT_TRUE(mBC.setTransform(tsc, "authority"));
01234 
01235     geometry_msgs::TransformStamped tsd;
01236     tsd.header.frame_id = "c";
01237     tsd.child_frame_id  = "d";
01238     tsd.transform.translation.x =  0.5;
01239     tsd.transform.translation.y =  -1;
01240     tsd.transform.translation.z =  1.5;
01241     btQuaternion q4;
01242     q4.setEuler(-0.5, 1.0, -.75);
01243     tsd.transform.rotation.x = q4.x();
01244     tsd.transform.rotation.y = q4.y();
01245     tsd.transform.rotation.z = q4.z();
01246     tsd.transform.rotation.w = q4.w();
01247     EXPECT_TRUE(mBC.setTransform(tsd, "authority"));
01248 
01249     btTransform ta, tb, tc, td, expected_ab, expected_bc, expected_cb, expected_ac, expected_ba, expected_ca, expected_ad, expected_da, expected_bd, expected_db, expected_rootd, expected_rootc;
01250     ta.setOrigin(btVector3(1.0,  1.0,  1.0));
01251     ta.setRotation(q1);
01252     tb.setOrigin(btVector3(-1.0, 0.0, -1.0));
01253     tb.setRotation(q2);
01254     tc.setOrigin(btVector3(0.0, 2.0, 0.5));
01255     tc.setRotation(q3);
01256     td.setOrigin(btVector3(0.5, -1, 1.5));
01257     td.setRotation(q4);
01258 
01259 
01260     expected_ab = ta.inverse() * tb;
01261     expected_ac = ta.inverse() * tb * tc;
01262     expected_ad = ta.inverse() * tb * tc * td;
01263     expected_cb = tc.inverse();
01264     expected_bc = tc;
01265     expected_bd = tc * td;
01266     expected_db = expected_bd.inverse();
01267     expected_ba = tb.inverse() * ta;
01268     expected_ca = tc.inverse() * tb.inverse() * ta;
01269     expected_da = td.inverse() * tc.inverse() * tb.inverse() * ta;
01270     expected_rootd = tb * tc * td;
01271     expected_rootc = tb * tc;
01272 
01273     // root -> b -> c
01274     geometry_msgs::TransformStamped out_rootc = mBC.lookupTransform("root", "c", ros::Time());
01275     CHECK_TRANSFORMS_NEAR(out_rootc, expected_rootc, epsilon);
01276 
01277     // root -> b -> c -> d
01278     geometry_msgs::TransformStamped out_rootd = mBC.lookupTransform("root", "d", ros::Time());
01279     CHECK_TRANSFORMS_NEAR(out_rootd, expected_rootd, epsilon);
01280 
01281     // a <- root -> b
01282     geometry_msgs::TransformStamped out_ab = mBC.lookupTransform("a", "b", ros::Time());
01283     CHECK_TRANSFORMS_NEAR(out_ab, expected_ab, epsilon);
01284 
01285     geometry_msgs::TransformStamped out_ba = mBC.lookupTransform("b", "a", ros::Time());
01286     CHECK_TRANSFORMS_NEAR(out_ba, expected_ba, epsilon);
01287 
01288     // a <- root -> b -> c
01289     geometry_msgs::TransformStamped out_ac = mBC.lookupTransform("a", "c", ros::Time());
01290     CHECK_TRANSFORMS_NEAR(out_ac, expected_ac, epsilon);
01291 
01292     geometry_msgs::TransformStamped out_ca = mBC.lookupTransform("c", "a", ros::Time());
01293     CHECK_TRANSFORMS_NEAR(out_ca, expected_ca, epsilon);
01294 
01295     // a <- root -> b -> c -> d
01296     geometry_msgs::TransformStamped out_ad = mBC.lookupTransform("a", "d", ros::Time());
01297     CHECK_TRANSFORMS_NEAR(out_ad, expected_ad, epsilon);
01298 
01299     geometry_msgs::TransformStamped out_da = mBC.lookupTransform("d", "a", ros::Time());
01300     CHECK_TRANSFORMS_NEAR(out_da, expected_da, epsilon);
01301 
01302     // b -> c
01303     geometry_msgs::TransformStamped out_cb = mBC.lookupTransform("c", "b", ros::Time());
01304     CHECK_TRANSFORMS_NEAR(out_cb, expected_cb, epsilon);
01305 
01306     geometry_msgs::TransformStamped out_bc = mBC.lookupTransform("b", "c", ros::Time());
01307     CHECK_TRANSFORMS_NEAR(out_bc, expected_bc, epsilon);
01308 
01309     // b -> c -> d
01310     geometry_msgs::TransformStamped out_bd = mBC.lookupTransform("b", "d", ros::Time());
01311     CHECK_TRANSFORMS_NEAR(out_bd, expected_bd, epsilon);
01312 
01313     geometry_msgs::TransformStamped out_db = mBC.lookupTransform("d", "b", ros::Time());
01314     CHECK_TRANSFORMS_NEAR(out_db, expected_db, epsilon);
01315 }
01316 
01317 // Time varying transforms, testing interpolation
01318 TEST(BufferCore_lookupTransform, helix_configuration)
01319 {
01320         double epsilon = 2e-5; // Larger epsilon for interpolation values
01321 
01322     tf2::BufferCore mBC;
01323 
01324     ros::Time     t0        = ros::Time() + ros::Duration(10);
01325     ros::Duration step      = ros::Duration(0.05);
01326     ros::Duration half_step = ros::Duration(0.025);
01327     ros::Time     t1        = t0 + ros::Duration(5.0);
01328 
01329     /*
01330      * a->b->c
01331      *
01332      * b.z = vel * (t - t0)
01333      * c.x = cos(theta * (t - t0))
01334      * c.y = sin(theta * (t - t0))
01335      *
01336      * a->d
01337      *
01338      * d.z = 2 * cos(theta * (t - t0))
01339      * a->d transforms are at half-step between a->b->c transforms
01340      */
01341 
01342     double theta = 0.25;
01343     double vel   = 1.0;
01344 
01345     for (ros::Time t = t0; t <= t1; t += step)
01346     {
01347         ros::Time t2 = t + half_step;
01348         double dt  = (t - t0).toSec();
01349         double dt2 = (t2 - t0).toSec();
01350 
01351         geometry_msgs::TransformStamped ts;
01352         ts.header.frame_id = "a";
01353         ts.header.stamp    = t;
01354         ts.child_frame_id  = "b";
01355         ts.transform.translation.z = vel * dt;
01356         ts.transform.rotation.w = 1.0;
01357         EXPECT_TRUE(mBC.setTransform(ts, "authority"));
01358 
01359         geometry_msgs::TransformStamped ts2;
01360         ts2.header.frame_id = "b";
01361         ts2.header.stamp    = t;
01362         ts2.child_frame_id  = "c";
01363         ts2.transform.translation.x = cos(theta * dt);
01364         ts2.transform.translation.y = sin(theta * dt);
01365         btQuaternion q;
01366         q.setEuler(0,0,theta*dt);
01367         ts2.transform.rotation.z = q.z();
01368         ts2.transform.rotation.w = q.w();
01369         EXPECT_TRUE(mBC.setTransform(ts2, "authority"));
01370 
01371         geometry_msgs::TransformStamped ts3;
01372         ts3.header.frame_id = "a";
01373         ts3.header.stamp    = t2;
01374         ts3.child_frame_id  = "d";
01375         ts3.transform.translation.z = cos(theta * dt2);
01376         ts3.transform.rotation.w = 1.0;
01377         EXPECT_TRUE(mBC.setTransform(ts3, "authority"));
01378     }
01379 
01380 
01381     for (ros::Time t = t0 + half_step; t < t1; t += step)
01382     {
01383         ros::Time t2 = t + half_step;
01384         double dt  = (t - t0).toSec();
01385         double dt2 = (t2 - t0).toSec();
01386 
01387         geometry_msgs::TransformStamped out_ab = mBC.lookupTransform("a", "b", t);
01388         EXPECT_NEAR(out_ab.transform.translation.z, vel * dt, epsilon);
01389 
01390         geometry_msgs::TransformStamped out_ac = mBC.lookupTransform("a", "c", t);
01391         EXPECT_NEAR(out_ac.transform.translation.x, cos(theta * dt), epsilon);
01392         EXPECT_NEAR(out_ac.transform.translation.y, sin(theta * dt), epsilon);
01393         EXPECT_NEAR(out_ac.transform.translation.z, vel * dt,            epsilon);
01394         btQuaternion q;
01395         q.setEuler(0,0,theta*dt);
01396         CHECK_QUATERNION_NEAR(out_ac.transform.rotation, 0, 0, q.z(), q.w(), epsilon);
01397 
01398         geometry_msgs::TransformStamped out_ad = mBC.lookupTransform("a", "d", t);
01399         EXPECT_NEAR(out_ad.transform.translation.z, cos(theta * dt), epsilon);
01400 
01401         geometry_msgs::TransformStamped out_cd = mBC.lookupTransform("c", "d", t2);
01402         EXPECT_NEAR(out_cd.transform.translation.x, -1,                                       epsilon);
01403         EXPECT_NEAR(out_cd.transform.translation.y,  0,                                       epsilon);
01404         EXPECT_NEAR(out_cd.transform.translation.z, cos(theta * dt2) - vel * dt2, epsilon);
01405         btQuaternion mq;
01406         mq.setEuler(0,0,-theta*dt2);
01407         CHECK_QUATERNION_NEAR(out_cd.transform.rotation, 0, 0, mq.z(), mq.w(), epsilon);
01408     }
01409 
01410     // Advanced API
01411     for (ros::Time t = t0 + half_step; t < t1; t += (step + step))
01412     {
01413         ros::Time t2 = t + step;
01414         double dt  = (t - t0).toSec();
01415         double dt2 = (t2 - t0).toSec();
01416 
01417         geometry_msgs::TransformStamped out_cd2 = mBC.lookupTransform("c", t, "d", t2, "a");
01418         EXPECT_NEAR(out_cd2.transform.translation.x, -1,                                      epsilon);
01419         EXPECT_NEAR(out_cd2.transform.translation.y,  0,                                      epsilon);
01420         EXPECT_NEAR(out_cd2.transform.translation.z, cos(theta * dt2) - vel * dt, epsilon);
01421         btQuaternion mq2;
01422         mq2.setEuler(0,0,-theta*dt);
01423         CHECK_QUATERNION_NEAR(out_cd2.transform.rotation, 0, 0, mq2.z(), mq2.w(), epsilon);
01424     }
01425 }
01426 
01427 
01428 TEST(BufferCore_lookupTransform, ring_45_configuration)
01429 {
01430   double epsilon = 1e-6;
01431   rostest::Permuter permuter;
01432 
01433   std::vector<ros::Time> times;
01434   times.push_back(ros::Time(1.0));
01435   times.push_back(ros::Time(10.0));
01436   times.push_back(ros::Time(0.0));
01437   ros::Time eval_time;
01438   permuter.addOptionSet(times, &eval_time);
01439 
01440   std::vector<ros::Duration> durations;
01441   durations.push_back(ros::Duration(1.0));
01442   durations.push_back(ros::Duration(0.001));
01443   durations.push_back(ros::Duration(0.1));
01444   ros::Duration interpolation_space;
01445   //  permuter.addOptionSet(durations, &interpolation_space);
01446 
01447   std::vector<std::string> frames;
01448   frames.push_back("a");
01449   frames.push_back("b");
01450   frames.push_back("c");
01451   frames.push_back("d");
01452   frames.push_back("e");
01453   frames.push_back("f");
01454   frames.push_back("g");
01455   frames.push_back("h");
01456   frames.push_back("i");
01457   /*  frames.push_back("inverse_b");
01458   frames.push_back("inverse_c");
01459   frames.push_back("inverse_d");
01460   frames.push_back("inverse_e");
01461   frames.push_back("inverse_f");
01462   frames.push_back("inverse_g");
01463   frames.push_back("inverse_h");
01464   frames.push_back("inverse_i");*/
01465   std::string source_frame;
01466   permuter.addOptionSet(frames, &source_frame);
01467   
01468   std::string target_frame;
01469   permuter.addOptionSet(frames, &target_frame);
01470 
01471   while  (permuter.step())
01472   {
01473 
01474     tf2::BufferCore mBC;
01475     setupTree(mBC, "ring_45", eval_time, interpolation_space);
01476 
01477     geometry_msgs::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, eval_time);
01478 
01479 
01480     //printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec());  
01481     EXPECT_EQ(outpose.header.stamp, eval_time);
01482     EXPECT_EQ(outpose.header.frame_id, source_frame);
01483     EXPECT_EQ(outpose.child_frame_id, target_frame);
01484 
01485 
01486     
01487     //Zero distance or all the way
01488     if (source_frame == target_frame               ||
01489         (source_frame == "a" && target_frame == "i") ||
01490         (source_frame == "i" && target_frame == "a") ||
01491         (source_frame == "a" && target_frame == "inverse_i") ||
01492         (source_frame == "inverse_i" && target_frame == "a") )
01493     {
01494       //printf ("here %s %s\n", source_frame.c_str(), target_frame.c_str());
01495       EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon);
01496       EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon);
01497       EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
01498       EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
01499       EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
01500       EXPECT_NEAR(outpose.transform.rotation.z, 0, epsilon);
01501       EXPECT_NEAR(fabs(outpose.transform.rotation.w), 1, epsilon);
01502     }
01503     // Chaining 1
01504     else if ((source_frame == "a" && target_frame =="b") ||
01505              (source_frame == "b" && target_frame =="c") ||
01506              (source_frame == "c" && target_frame =="d") ||
01507              (source_frame == "d" && target_frame =="e") ||
01508              (source_frame == "e" && target_frame =="f") ||
01509              (source_frame == "f" && target_frame =="g") ||
01510              (source_frame == "g" && target_frame =="h") ||
01511              (source_frame == "h" && target_frame =="i")
01512              )
01513     {
01514       EXPECT_NEAR(outpose.transform.translation.x, sqrt(2)/2 - 1, epsilon);
01515       EXPECT_NEAR(outpose.transform.translation.y, sqrt(2)/2 , epsilon);
01516       EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
01517       EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
01518       EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
01519       EXPECT_NEAR(outpose.transform.rotation.z, sin(M_PI/8), epsilon);
01520       EXPECT_NEAR(outpose.transform.rotation.w, cos(M_PI/8), epsilon);
01521     }
01522     // Inverse Chaining 1
01523     else if ((source_frame == "b" && target_frame =="a") ||
01524              (source_frame == "c" && target_frame =="b") ||
01525              (source_frame == "d" && target_frame =="c") ||
01526              (source_frame == "e" && target_frame =="d") ||
01527              (source_frame == "f" && target_frame =="e") ||
01528              (source_frame == "g" && target_frame =="f") ||
01529              (source_frame == "h" && target_frame =="g") ||
01530              (source_frame == "i" && target_frame =="h")
01531              )
01532     {
01533       EXPECT_NEAR(outpose.transform.translation.x, sqrt(2)/2 - 1, epsilon);
01534       EXPECT_NEAR(outpose.transform.translation.y, -sqrt(2)/2 , epsilon);
01535       EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
01536       EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
01537       EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
01538       EXPECT_NEAR(outpose.transform.rotation.z, sin(-M_PI/8), epsilon);
01539       EXPECT_NEAR(outpose.transform.rotation.w, cos(-M_PI/8), epsilon);
01540     }
01541     // Chaining 2
01542     else if ((source_frame == "a" && target_frame =="c") ||
01543              (source_frame == "b" && target_frame =="d") ||
01544              (source_frame == "c" && target_frame =="e") ||
01545              (source_frame == "d" && target_frame =="f") ||
01546              (source_frame == "e" && target_frame =="g") ||
01547              (source_frame == "f" && target_frame =="h") ||
01548              (source_frame == "g" && target_frame =="i")
01549              )
01550     {
01551       EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
01552       EXPECT_NEAR(outpose.transform.translation.y, 1 , epsilon);
01553       EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
01554       EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
01555       EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
01556       EXPECT_NEAR(outpose.transform.rotation.z, sin(M_PI/4), epsilon);
01557       EXPECT_NEAR(outpose.transform.rotation.w, cos(M_PI/4), epsilon);
01558     }
01559     // Inverse Chaining 2
01560     else if ((source_frame == "c" && target_frame =="a") ||
01561              (source_frame == "d" && target_frame =="b") ||
01562              (source_frame == "e" && target_frame =="c") ||
01563              (source_frame == "f" && target_frame =="d") ||
01564              (source_frame == "g" && target_frame =="e") ||
01565              (source_frame == "h" && target_frame =="f") ||
01566              (source_frame == "i" && target_frame =="g")
01567              )
01568     {
01569       EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
01570       EXPECT_NEAR(outpose.transform.translation.y, -1 , epsilon);
01571       EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
01572       EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
01573       EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
01574       EXPECT_NEAR(outpose.transform.rotation.z, sin(-M_PI/4), epsilon);
01575       EXPECT_NEAR(outpose.transform.rotation.w, cos(-M_PI/4), epsilon);
01576     }
01577     // Chaining 3
01578     else if ((source_frame == "a" && target_frame =="d") ||
01579              (source_frame == "b" && target_frame =="e") ||
01580              (source_frame == "c" && target_frame =="f") ||
01581              (source_frame == "d" && target_frame =="g") ||
01582              (source_frame == "e" && target_frame =="h") ||
01583              (source_frame == "f" && target_frame =="i")
01584              )
01585     {
01586       EXPECT_NEAR(outpose.transform.translation.x, -1 - sqrt(2)/2, epsilon);
01587       EXPECT_NEAR(outpose.transform.translation.y, sqrt(2)/2 , epsilon);
01588       EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
01589       EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
01590       EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
01591       EXPECT_NEAR(outpose.transform.rotation.z, sin(M_PI*3/8), epsilon);
01592       EXPECT_NEAR(outpose.transform.rotation.w, cos(M_PI*3/8), epsilon);
01593     }
01594     // Inverse Chaining 3
01595     else if ((target_frame == "a" && source_frame =="d") ||
01596              (target_frame == "b" && source_frame =="e") ||
01597              (target_frame == "c" && source_frame =="f") ||
01598              (target_frame == "d" && source_frame =="g") ||
01599              (target_frame == "e" && source_frame =="h") ||
01600              (target_frame == "f" && source_frame =="i")
01601              )
01602     {
01603       EXPECT_NEAR(outpose.transform.translation.x, -1 - sqrt(2)/2, epsilon);
01604       EXPECT_NEAR(outpose.transform.translation.y, - sqrt(2)/2 , epsilon);
01605       EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
01606       EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
01607       EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
01608       EXPECT_NEAR(outpose.transform.rotation.z, sin(-M_PI*3/8), epsilon);
01609       EXPECT_NEAR(outpose.transform.rotation.w, cos(-M_PI*3/8), epsilon);
01610     }
01611     // Chaining 4
01612     else if ((source_frame == "a" && target_frame =="e") ||
01613              (source_frame == "b" && target_frame =="f") ||
01614              (source_frame == "c" && target_frame =="g") ||
01615              (source_frame == "d" && target_frame =="h") ||
01616              (source_frame == "e" && target_frame =="i")
01617              )
01618     {
01619       EXPECT_NEAR(outpose.transform.translation.x, -2, epsilon);
01620       EXPECT_NEAR(outpose.transform.translation.y, 0 , epsilon);
01621       EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
01622       EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
01623       EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
01624       EXPECT_NEAR(outpose.transform.rotation.z, sin(M_PI/2), epsilon);
01625       EXPECT_NEAR(outpose.transform.rotation.w, cos(M_PI/2), epsilon);
01626     }
01627     // Inverse Chaining 4
01628     else if ((target_frame == "a" && source_frame =="e") ||
01629              (target_frame == "b" && source_frame =="f") ||
01630              (target_frame == "c" && source_frame =="g") ||
01631              (target_frame == "d" && source_frame =="h") ||
01632              (target_frame == "e" && source_frame =="i")
01633              )
01634     {
01635       EXPECT_NEAR(outpose.transform.translation.x, -2, epsilon);
01636       EXPECT_NEAR(outpose.transform.translation.y, 0 , epsilon);
01637       EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
01638       EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
01639       EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
01640       EXPECT_NEAR(outpose.transform.rotation.z, sin(-M_PI/2), epsilon);
01641       EXPECT_NEAR(outpose.transform.rotation.w, cos(-M_PI/2), epsilon);
01642     }
01643     // Chaining 5
01644     else if ((source_frame == "a" && target_frame =="f") ||
01645              (source_frame == "b" && target_frame =="g") ||
01646              (source_frame == "c" && target_frame =="h") ||
01647              (source_frame == "d" && target_frame =="i")
01648              )
01649     {
01650       EXPECT_NEAR(outpose.transform.translation.x, -1 - sqrt(2) /2, epsilon);
01651       EXPECT_NEAR(outpose.transform.translation.y, - sqrt(2) /2 , epsilon);
01652       EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
01653       EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
01654       EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
01655       EXPECT_NEAR(outpose.transform.rotation.z, sin(M_PI*5/8), epsilon);
01656       EXPECT_NEAR(outpose.transform.rotation.w, cos(M_PI*5/8), epsilon);
01657     }
01658     // Inverse Chaining 5
01659     else if ((target_frame == "a" && source_frame =="f") ||
01660              (target_frame == "b" && source_frame =="g") ||
01661              (target_frame == "c" && source_frame =="h") ||
01662              (target_frame == "d" && source_frame =="i")
01663              )
01664     {
01665       EXPECT_NEAR(outpose.transform.translation.x, -1 - sqrt(2)/2, epsilon);
01666       EXPECT_NEAR(outpose.transform.translation.y, sqrt(2)/2 , epsilon);
01667       EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
01668       EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
01669       EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
01670       EXPECT_NEAR(outpose.transform.rotation.z, sin(-M_PI*5/8), epsilon);
01671       EXPECT_NEAR(outpose.transform.rotation.w, cos(-M_PI*5/8), epsilon);
01672     }
01673     // Chaining 6
01674     else if ((source_frame == "a" && target_frame =="g") ||
01675              (source_frame == "b" && target_frame =="h") ||
01676              (source_frame == "c" && target_frame =="i")
01677              )
01678     {
01679       EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
01680       EXPECT_NEAR(outpose.transform.translation.y, -1 , epsilon);
01681       EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
01682       EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
01683       EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
01684       EXPECT_NEAR(outpose.transform.rotation.z, sin(M_PI*6/8), epsilon);
01685       EXPECT_NEAR(outpose.transform.rotation.w, cos(M_PI*6/8), epsilon);
01686     }
01687     // Inverse Chaining 6
01688     else if ((target_frame == "a" && source_frame =="g") ||
01689              (target_frame == "b" && source_frame =="h") ||
01690              (target_frame == "c" && source_frame =="i")
01691              )
01692     {
01693       EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
01694       EXPECT_NEAR(outpose.transform.translation.y, 1 , epsilon);
01695       EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
01696       EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
01697       EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
01698       EXPECT_NEAR(outpose.transform.rotation.z, sin(-M_PI*6/8), epsilon);
01699       EXPECT_NEAR(outpose.transform.rotation.w, cos(-M_PI*6/8), epsilon);
01700     }
01701     // Chaining 7
01702     else if ((source_frame == "a" && target_frame =="h") ||
01703              (source_frame == "b" && target_frame =="i")
01704              )
01705     {
01706       EXPECT_NEAR(outpose.transform.translation.x, sqrt(2)/2 - 1, epsilon);
01707       EXPECT_NEAR(outpose.transform.translation.y, -sqrt(2)/2 , epsilon);
01708       EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
01709       EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
01710       EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
01711       EXPECT_NEAR(outpose.transform.rotation.z, sin(M_PI*7/8), epsilon);
01712       EXPECT_NEAR(outpose.transform.rotation.w, cos(M_PI*7/8), epsilon);
01713     }
01714     // Inverse Chaining 7
01715     else if ((target_frame == "a" && source_frame =="h") ||
01716              (target_frame == "b" && source_frame =="i")
01717              )
01718     {
01719       EXPECT_NEAR(outpose.transform.translation.x, sqrt(2)/2 - 1, epsilon);
01720       EXPECT_NEAR(outpose.transform.translation.y, sqrt(2)/2 , epsilon);
01721       EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
01722       EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
01723       EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
01724       EXPECT_NEAR(outpose.transform.rotation.z, sin(-M_PI*7/8), epsilon);
01725       EXPECT_NEAR(outpose.transform.rotation.w, cos(-M_PI*7/8), epsilon);
01726     }
01727     else
01728     {
01729       EXPECT_FALSE("Ring_45 testing Shouldn't get here");
01730       printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec());
01731     }
01732     
01733   }
01734 }
01735 
01736 TEST(BufferCore_lookupTransform, invalid_arguments)
01737 {
01738   tf2::BufferCore mBC;
01739   
01740   setupTree(mBC, "i", ros::Time(1.0));
01741   
01742   EXPECT_NO_THROW(mBC.lookupTransform("b", "a", ros::Time()));
01743 
01744   //Empty frame_id
01745   EXPECT_THROW(mBC.lookupTransform("", "a", ros::Time()), tf2::InvalidArgumentException);
01746   EXPECT_THROW(mBC.lookupTransform("b", "", ros::Time()), tf2::InvalidArgumentException);
01747 
01748   //frame_id with /
01749   EXPECT_THROW(mBC.lookupTransform("/b", "a", ros::Time()), tf2::InvalidArgumentException);
01750   EXPECT_THROW(mBC.lookupTransform("b", "/a", ros::Time()), tf2::InvalidArgumentException);
01751 
01752 };
01753 
01754 TEST(BufferCore_canTransform, invalid_arguments)
01755 {
01756   tf2::BufferCore mBC;
01757 
01758   setupTree(mBC, "i", ros::Time(1.0));
01759   
01760   EXPECT_TRUE(mBC.canTransform("b", "a", ros::Time()));
01761   
01762   
01763   //Empty frame_id
01764   EXPECT_FALSE(mBC.canTransform("", "a", ros::Time()));
01765   EXPECT_FALSE(mBC.canTransform("b", "", ros::Time()));
01766 
01767   //frame_id with /
01768   EXPECT_FALSE(mBC.canTransform("/b", "a", ros::Time()));
01769   EXPECT_FALSE(mBC.canTransform("b", "/a", ros::Time()));
01770 
01771 };
01772 
01773 struct TransformableHelper
01774 {
01775   TransformableHelper()
01776   : called(false)
01777   {}
01778 
01779   void callback(tf2::TransformableRequestHandle request_handle, const std::string& target_frame, const std::string& source_frame,
01780           ros::Time time, tf2::TransformableResult result)
01781   {
01782     called = true;
01783   }
01784 
01785   bool called;
01786 };
01787 
01788 TEST(BufferCore_transformableCallbacks, alreadyTransformable)
01789 {
01790   tf2::BufferCore b;
01791   TransformableHelper h;
01792 
01793   geometry_msgs::TransformStamped t;
01794   t.header.stamp = ros::Time(1);
01795   t.header.frame_id = "a";
01796   t.child_frame_id = "b";
01797   t.transform.rotation.w = 1.0;
01798   b.setTransform(t, "me");
01799 
01800   tf2::TransformableCallbackHandle cb_handle = b.addTransformableCallback(boost::bind(&TransformableHelper::callback, &h, _1, _2, _3, _4, _5));
01801   EXPECT_EQ(b.addTransformableRequest(cb_handle, "a", "b", ros::Time(1)), 0U);
01802 }
01803 
01804 TEST(BufferCore_transformableCallbacks, waitForNewTransform)
01805 {
01806   tf2::BufferCore b;
01807   TransformableHelper h;
01808   tf2::TransformableCallbackHandle cb_handle = b.addTransformableCallback(boost::bind(&TransformableHelper::callback, &h, _1, _2, _3, _4, _5));
01809   EXPECT_GT(b.addTransformableRequest(cb_handle, "a", "b", ros::Time(10)), 0U);
01810 
01811   geometry_msgs::TransformStamped t;
01812   for (uint32_t i = 1; i <= 10; ++i)
01813   {
01814     t.header.stamp = ros::Time(i);
01815     t.header.frame_id = "a";
01816     t.child_frame_id = "b";
01817     t.transform.rotation.w = 1.0;
01818     b.setTransform(t, "me");
01819 
01820     if (i < 10)
01821     {
01822       ASSERT_FALSE(h.called);
01823     }
01824     else
01825     {
01826       ASSERT_TRUE(h.called);
01827     }
01828   }
01829 }
01830 
01831 TEST(BufferCore_transformableCallbacks, waitForOldTransform)
01832 {
01833   tf2::BufferCore b;
01834   TransformableHelper h;
01835   tf2::TransformableCallbackHandle cb_handle = b.addTransformableCallback(boost::bind(&TransformableHelper::callback, &h, _1, _2, _3, _4, _5));
01836   EXPECT_GT(b.addTransformableRequest(cb_handle, "a", "b", ros::Time(1)), 0U);
01837 
01838   geometry_msgs::TransformStamped t;
01839   for (uint32_t i = 10; i > 0; --i)
01840   {
01841     t.header.stamp = ros::Time(i);
01842     t.header.frame_id = "a";
01843     t.child_frame_id = "b";
01844     t.transform.rotation.w = 1.0;
01845     b.setTransform(t, "me");
01846 
01847     if (i > 1)
01848     {
01849       ASSERT_FALSE(h.called);
01850     }
01851     else
01852     {
01853       ASSERT_TRUE(h.called);
01854     }
01855   }
01856 }
01857 
01858 /*
01859 TEST(tf, Exceptions)
01860 {
01861 
01862  tf::Transformer mTR(true);
01863 
01864  
01865  Stamped<btTransform> outpose;
01866 
01867  //connectivity when no data
01868  EXPECT_FALSE(mTR.canTransform("parent", "me", ros::Time().fromNSec(10000000)));
01869  try 
01870  {
01871    mTR.transformPose("parent",Stamped<Pose>(btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(10000000) , "me"), outpose);
01872    EXPECT_FALSE("ConnectivityException Not Thrown");   
01873  }
01874  catch ( tf::LookupException &ex)
01875  {
01876    EXPECT_TRUE("Lookupgh Exception Caught");
01877  }
01878  catch (tf::TransformException& ex)
01879  {
01880    printf("%s\n",ex.what());
01881    EXPECT_FALSE("Other Exception Caught");
01882  }
01883  
01884  mTR.setTransform( StampedTransform(btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(100000), "parent", "me"));
01885 
01886  //Extrapolation not valid with one value
01887  EXPECT_FALSE(mTR.canTransform("parent", "me", ros::Time().fromNSec(200000)));
01888  try 
01889  {
01890    mTR.transformPose("parent",Stamped<Pose>(btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(200000) , "me"), outpose);
01891    EXPECT_TRUE("ExtrapolationException Not Thrown");
01892  }
01893  catch ( tf::ExtrapolationException &ex)
01894  {
01895    EXPECT_TRUE("Extrapolation Exception Caught");
01896  }
01897  catch (tf::TransformException& ex)
01898  {
01899    printf("%s\n",ex.what());
01900    EXPECT_FALSE("Other Exception Caught");
01901  }
01902  
01903 
01904  mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(300000), "parent", "me"));
01905 
01906  //NO Extration when Interpolating
01907  //inverse list
01908  EXPECT_TRUE(mTR.canTransform("parent", "me", ros::Time().fromNSec(200000)));
01909  try 
01910  {
01911    mTR.transformPose("parent",Stamped<Pose>(btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(200000) , "me"), outpose);
01912    EXPECT_TRUE("ExtrapolationException Not Thrown");
01913  }
01914  catch ( tf::ExtrapolationException &ex)
01915  {
01916    EXPECT_FALSE("Extrapolation Exception Caught");
01917  }
01918  catch (tf::TransformException& ex)
01919  {
01920    printf("%s\n",ex.what());
01921    EXPECT_FALSE("Other Exception Caught");
01922  }
01923 
01924 
01925 
01926  //forward list
01927  EXPECT_TRUE(mTR.canTransform("me", "parent", ros::Time().fromNSec(200000)));
01928  try 
01929  {
01930    mTR.transformPose("me",Stamped<Pose>(btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(200000) , "parent"), outpose);
01931    EXPECT_TRUE("ExtrapolationException Not Thrown");
01932  }
01933  catch ( tf::ExtrapolationException &ex)
01934  {
01935    EXPECT_FALSE("Extrapolation Exception Caught");
01936  }
01937  catch (tf::TransformException& ex)
01938  {
01939    printf("%s\n",ex.what());
01940    EXPECT_FALSE("Other Exception Caught");
01941  }
01942   
01943 
01944  //Extrapolating backwards
01945  //inverse list
01946  EXPECT_FALSE(mTR.canTransform("parent", "me", ros::Time().fromNSec(1000)));
01947  try 
01948  {
01949    mTR.transformPose("parent",Stamped<Pose> (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(1000) , "me"), outpose);
01950    EXPECT_FALSE("ExtrapolationException Not Thrown");
01951  }
01952  catch ( tf::ExtrapolationException &ex)
01953  {
01954    EXPECT_TRUE("Extrapolation Exception Caught");
01955  }
01956  catch (tf::TransformException& ex)
01957  {
01958    printf("%s\n",ex.what());
01959    EXPECT_FALSE("Other Exception Caught");
01960  }
01961  //forwards list
01962  EXPECT_FALSE(mTR.canTransform("me", "parent", ros::Time().fromNSec(1000)));
01963  try 
01964  {
01965    mTR.transformPose("me",Stamped<Pose> (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(1000) , "parent"), outpose);
01966    EXPECT_FALSE("ExtrapolationException Not Thrown");
01967  }
01968  catch ( tf::ExtrapolationException &ex)
01969  {
01970    EXPECT_TRUE("Extrapolation Exception Caught");
01971  }
01972  catch (tf::TransformException& ex)
01973  {
01974    printf("%s\n",ex.what());
01975    EXPECT_FALSE("Other Exception Caught");
01976  }
01977   
01978 
01979 
01980  // Test extrapolation inverse and forward linkages FORWARD
01981 
01982  //inverse list
01983  EXPECT_FALSE(mTR.canTransform("parent", "me", ros::Time().fromNSec(350000)));
01984  try 
01985  {
01986    mTR.transformPose("parent", Stamped<Pose> (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(350000) , "me"), outpose);
01987    EXPECT_FALSE("ExtrapolationException Not Thrown");
01988  }
01989  catch ( tf::ExtrapolationException &ex)
01990  {
01991    EXPECT_TRUE("Extrapolation Exception Caught");
01992  }
01993  catch (tf::TransformException& ex)
01994  {
01995    printf("%s\n",ex.what());
01996    EXPECT_FALSE("Other Exception Caught");
01997  }
01998 
01999  //forward list
02000  EXPECT_FALSE(mTR.canTransform("parent", "me", ros::Time().fromNSec(350000)));
02001  try 
02002  {
02003    mTR.transformPose("me", Stamped<Pose> (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(350000) , "parent"), outpose);
02004    EXPECT_FALSE("ExtrapolationException Not Thrown");
02005  }
02006  catch ( tf::ExtrapolationException &ex)
02007  {
02008    EXPECT_TRUE("Extrapolation Exception Caught");
02009  }
02010  catch (tf::TransformException& ex)
02011  {
02012    printf("%s\n",ex.what());
02013    EXPECT_FALSE("Other Exception Caught");
02014  }
02015   
02016 
02017 
02018 
02019 }
02020 
02021 
02022 
02023 TEST(tf, NoExtrapolationExceptionFromParent)
02024 {
02025   tf::Transformer mTR(true, ros::Duration().fromNSec(1000000));
02026   
02027 
02028 
02029   mTR.setTransform(  StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(1000), "parent", "a"));
02030   mTR.setTransform(  StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(10000),  "parent", "a"));
02031   
02032   
02033   mTR.setTransform(  StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(1000),  "parent", "b"));
02034   mTR.setTransform(  StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(10000),  "parent", "b"));
02035 
02036   mTR.setTransform(  StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(1000),  "parent's parent", "parent"));
02037   mTR.setTransform(  StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(1000),  "parent's parent's parent", "parent's parent"));
02038 
02039   mTR.setTransform(  StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(10000),  "parent's parent", "parent"));
02040   mTR.setTransform(  StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(10000),  "parent's parent's parent", "parent's parent"));
02041 
02042   Stamped<Point> output;
02043 
02044   try
02045   {
02046     mTR.transformPoint( "b", Stamped<Point>(Point(1,1,1), ros::Time().fromNSec(2000), "a"), output);
02047   }
02048   catch (ExtrapolationException &ex)
02049   {
02050     EXPECT_FALSE("Shouldn't have gotten this exception");
02051   }
02052 
02053 
02054 
02055 };
02056 
02057 
02058 
02059 TEST(tf, ExtrapolationFromOneValue)
02060 {
02061   tf::Transformer mTR(true, ros::Duration().fromNSec(1000000));
02062   
02063 
02064 
02065   mTR.setTransform(  StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(1000),  "parent", "a"));
02066 
02067   mTR.setTransform(  StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(1000),  "parent's parent", "parent"));
02068 
02069 
02070   Stamped<Point> output;
02071 
02072   bool excepted = false;
02073   //Past time
02074   try
02075   {
02076     mTR.transformPoint( "parent", Stamped<Point>(Point(1,1,1), ros::Time().fromNSec(10), "a"), output);
02077   }
02078   catch (ExtrapolationException &ex)
02079   {
02080     excepted = true;
02081   }
02082   
02083   EXPECT_TRUE(excepted);
02084 
02085   excepted = false;
02086   //Future one element
02087   try
02088   {
02089     mTR.transformPoint( "parent", Stamped<Point>(Point(1,1,1), ros::Time().fromNSec(100000), "a"), output);
02090   }
02091   catch (ExtrapolationException &ex)
02092   {
02093     excepted = true;
02094   }
02095   
02096   EXPECT_TRUE(excepted);
02097 
02098   //Past multi link
02099   excepted = false;
02100   try
02101   {
02102     mTR.transformPoint( "parent's parent", Stamped<Point>(Point(1,1,1), ros::Time().fromNSec(1), "a"), output);
02103   }
02104   catch (ExtrapolationException &ex)
02105   {
02106     excepted = true;
02107   }
02108   
02109   EXPECT_TRUE(excepted);
02110 
02111   //Future case multi link
02112   excepted = false;
02113   try
02114   {
02115     mTR.transformPoint( "parent's parent", Stamped<Point>(Point(1,1,1), ros::Time().fromNSec(10000), "a"), output);
02116   }
02117   catch (ExtrapolationException &ex)
02118   {
02119     excepted = true;
02120   }
02121   
02122   EXPECT_TRUE(excepted);
02123 
02124   mTR.setTransform(  StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(20000),  "parent", "a"));
02125 
02126   excepted = false;
02127   try
02128   {
02129     mTR.transformPoint( "parent", Stamped<Point>(Point(1,1,1), ros::Time().fromNSec(10000), "a"), output);
02130   }
02131   catch (ExtrapolationException &ex)
02132   {
02133     excepted = true;
02134   }
02135   
02136   EXPECT_FALSE(excepted);
02137 
02138 };
02139 
02140 
02141 
02142 TEST(tf, getLatestCommonTime)
02143 {
02144   tf::Transformer mTR(true);
02145   mTR.setTransform(  StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(1000),  "parent", "a"));
02146   mTR.setTransform(  StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(2000),  "parent's parent", "parent"));
02147   
02148   //simple case
02149   ros::Time t;
02150   mTR.getLatestCommonTime("a", "parent's parent", t, NULL);
02151   EXPECT_EQ(t, ros::Time().fromNSec(1000));
02152 
02153   //no connection
02154   EXPECT_EQ(tf::LOOKUP_ERROR, mTR.getLatestCommonTime("a", "not valid", t, NULL));
02155   EXPECT_EQ(t, ros::Time());
02156 
02157   //testing with update
02158   mTR.setTransform(  StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(3000),  "parent", "a"));
02159   mTR.getLatestCommonTime("a", "parent's parent",t, NULL);
02160   EXPECT_EQ(t, ros::Time().fromNSec(2000));
02161 
02162   //longer chain
02163   mTR.setTransform(  StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(4000),  "parent", "b"));
02164   mTR.setTransform(  StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(3000),  "b", "c"));
02165   mTR.setTransform(  StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(9000),  "c", "d"));
02166   mTR.setTransform(  StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(5000),  "f", "e"));
02167 
02168   //shared parent
02169   mTR.getLatestCommonTime("a", "b",t, NULL);
02170   EXPECT_EQ(t, ros::Time().fromNSec(3000));
02171 
02172   //two degrees
02173   mTR.getLatestCommonTime("a", "c", t, NULL);
02174   EXPECT_EQ(t, ros::Time().fromNSec(3000));
02175   //reversed
02176   mTR.getLatestCommonTime("c", "a", t, NULL);
02177   EXPECT_EQ(t, ros::Time().fromNSec(3000));
02178 
02179   //three degrees
02180   mTR.getLatestCommonTime("a", "d", t, NULL);
02181   EXPECT_EQ(t, ros::Time().fromNSec(3000));
02182   //reversed
02183   mTR.getLatestCommonTime("d", "a", t, NULL);
02184   EXPECT_EQ(t, ros::Time().fromNSec(3000));
02185 
02186   //disconnected tree
02187   mTR.getLatestCommonTime("e", "f", t, NULL);
02188   EXPECT_EQ(t, ros::Time().fromNSec(5000));
02189   //reversed order
02190   mTR.getLatestCommonTime("f", "e", t, NULL);
02191   EXPECT_EQ(t, ros::Time().fromNSec(5000));
02192 
02193 
02194   mTR.setExtrapolationLimit(ros::Duration().fromNSec(20000));
02195 
02196   //check timestamps resulting
02197   tf::Stamped<tf::Point> output, output2;
02198   try
02199   {
02200     mTR.transformPoint( "parent", Stamped<Point>(Point(1,1,1), ros::Time(), "b"), output);
02201     mTR.transformPoint( "a", ros::Time(),Stamped<Point>(Point(1,1,1), ros::Time(), "b"), "c",  output2);
02202   }
02203   catch (tf::TransformException &ex)
02204   {
02205     printf("%s\n", ex.what());
02206     EXPECT_FALSE("Shouldn't get this Exception");
02207   }
02208 
02209   EXPECT_EQ(output.stamp_, ros::Time().fromNSec(4000));
02210   EXPECT_EQ(output2.stamp_, ros::Time().fromNSec(3000));
02211 
02212 
02213   //zero length lookup zero time
02214   ros::Time now1 = ros::Time::now();
02215   ros::Time time_output;
02216   mTR.getLatestCommonTime("a", "a", time_output, NULL);
02217   EXPECT_LE(now1.toSec(), time_output.toSec());
02218   EXPECT_LE(time_output.toSec(), ros::Time::now().toSec());
02219 
02220 
02221 }
02222 
02223 TEST(tf, RepeatedTimes)
02224 {
02225   Transformer mTR;
02226   mTR.setTransform(  StampedTransform (btTransform(btQuaternion(1,0,0), btVector3(0,0,0)), ros::Time().fromNSec(4000),  "parent", "b"));
02227   mTR.setTransform(  StampedTransform (btTransform(btQuaternion(1,1,0), btVector3(0,0,0)), ros::Time().fromNSec(4000),  "parent", "b"));
02228 
02229   tf::StampedTransform  output;
02230   try{
02231     mTR.lookupTransform("parent", "b" , ros::Time().fromNSec(4000), output);
02232     EXPECT_TRUE(!std::isnan(output.getOrigin().x()));
02233     EXPECT_TRUE(!std::isnan(output.getOrigin().y()));
02234     EXPECT_TRUE(!std::isnan(output.getOrigin().z()));
02235     EXPECT_TRUE(!std::isnan(output.getRotation().x()));
02236     EXPECT_TRUE(!std::isnan(output.getRotation().y()));
02237     EXPECT_TRUE(!std::isnan(output.getRotation().z()));
02238     EXPECT_TRUE(!std::isnan(output.getRotation().w()));
02239   }
02240   catch (...)
02241   {
02242     EXPECT_FALSE("Excetion improperly thrown");
02243   }
02244   
02245 
02246 }
02247 
02248 TEST(tf, frameExists)
02249 {
02250   Transformer mTR;
02251 
02252   // test with fully qualified name
02253   EXPECT_FALSE(mTR.frameExists("/b"));;
02254   EXPECT_FALSE(mTR.frameExists("/parent"));
02255   EXPECT_FALSE(mTR.frameExists("/other"));
02256   EXPECT_FALSE(mTR.frameExists("/frame"));
02257 
02258   //test with resolveping
02259   EXPECT_FALSE(mTR.frameExists("b"));;
02260   EXPECT_FALSE(mTR.frameExists("parent"));
02261   EXPECT_FALSE(mTR.frameExists("other"));
02262   EXPECT_FALSE(mTR.frameExists("frame"));
02263 
02264   mTR.setTransform(  StampedTransform (btTransform(btQuaternion(1,0,0), btVector3(0,0,0)), ros::Time().fromNSec(4000),  "/parent", "/b"));
02265 
02266   // test with fully qualified name
02267   EXPECT_TRUE(mTR.frameExists("/b"));
02268   EXPECT_TRUE(mTR.frameExists("/parent"));
02269   EXPECT_FALSE(mTR.frameExists("/other"));
02270   EXPECT_FALSE(mTR.frameExists("/frame"));
02271 
02272   //Test with resolveping
02273   EXPECT_TRUE(mTR.frameExists("b"));
02274   EXPECT_TRUE(mTR.frameExists("parent"));
02275   EXPECT_FALSE(mTR.frameExists("other"));
02276   EXPECT_FALSE(mTR.frameExists("frame"));
02277 
02278   mTR.setTransform(  StampedTransform (btTransform(btQuaternion(1,1,0), btVector3(0,0,0)), ros::Time().fromNSec(4000),  "/frame", "/other"));
02279 
02280   // test with fully qualified name
02281   EXPECT_TRUE(mTR.frameExists("/b"));
02282   EXPECT_TRUE(mTR.frameExists("/parent"));
02283   EXPECT_TRUE(mTR.frameExists("/other"));
02284   EXPECT_TRUE(mTR.frameExists("/frame"));
02285   
02286   //Test with resolveping
02287   EXPECT_TRUE(mTR.frameExists("b"));
02288   EXPECT_TRUE(mTR.frameExists("parent"));
02289   EXPECT_TRUE(mTR.frameExists("other"));
02290   EXPECT_TRUE(mTR.frameExists("frame"));
02291 
02292 }
02293 
02294 TEST(tf, resolve)
02295 {
02296   //no prefix
02297   EXPECT_STREQ("/id", tf::resolve("","id").c_str());
02298   //prefix w/o /
02299   EXPECT_STREQ("/asdf/id", tf::resolve("asdf","id").c_str());
02300   //prefix w /
02301   EXPECT_STREQ("/asdf/id", tf::resolve("/asdf","id").c_str());
02302   // frame_id w / -> no prefix
02303   EXPECT_STREQ("/id", tf::resolve("asdf","/id").c_str());
02304   // frame_id w / -> no prefix
02305   EXPECT_STREQ("/id", tf::resolve("/asdf","/id").c_str());
02306 
02307 }
02308 
02309 TEST(tf, canTransform)
02310 {
02311   Transformer mTR;
02312 
02313   //confirm zero length list disconnected will return true
02314   EXPECT_TRUE(mTR.canTransform("some_frame","some_frame", ros::Time()));
02315   EXPECT_TRUE(mTR.canTransform("some_frame","some_frame", ros::Time::now()));
02316 
02317   //Create a two link tree between times 10 and 20
02318   for (int i = 10; i < 20; i++)
02319   {
02320     mTR.setTransform(  StampedTransform (btTransform(btQuaternion(1,0,0), btVector3(0,0,0)), ros::Time().fromSec(i),  "parent", "child"));
02321     mTR.setTransform(  StampedTransform (btTransform(btQuaternion(1,0,0), btVector3(0,0,0)), ros::Time().fromSec(i),  "parent", "other_child"));
02322   }
02323 
02324   // four different timestamps related to tf state
02325   ros::Time zero_time = ros::Time().fromSec(0);
02326   ros::Time old_time = ros::Time().fromSec(5);
02327   ros::Time valid_time = ros::Time().fromSec(15);
02328   ros::Time future_time = ros::Time().fromSec(25);
02329 
02330 
02331   //confirm zero length list disconnected will return true
02332   EXPECT_TRUE(mTR.canTransform("some_frame","some_frame", zero_time));
02333   EXPECT_TRUE(mTR.canTransform("some_frame","some_frame", old_time));
02334   EXPECT_TRUE(mTR.canTransform("some_frame","some_frame", valid_time));
02335   EXPECT_TRUE(mTR.canTransform("some_frame","some_frame", future_time));
02336 
02337   // Basic API Tests
02338 
02339   //Valid data should pass
02340   EXPECT_TRUE(mTR.canTransform("child", "parent", valid_time));
02341   EXPECT_TRUE(mTR.canTransform("child", "other_child", valid_time));
02342 
02343   //zero data should pass
02344   EXPECT_TRUE(mTR.canTransform("child", "parent", zero_time));
02345   EXPECT_TRUE(mTR.canTransform("child", "other_child", zero_time));
02346 
02347   //Old data should fail
02348   EXPECT_FALSE(mTR.canTransform("child", "parent", old_time));
02349   EXPECT_FALSE(mTR.canTransform("child", "other_child", old_time));
02350 
02351   //Future data should fail
02352   EXPECT_FALSE(mTR.canTransform("child", "parent", future_time));
02353   EXPECT_FALSE(mTR.canTransform("child", "other_child", future_time));
02354 
02355   //Same Frame should pass for all times
02356   EXPECT_TRUE(mTR.canTransform("child", "child", zero_time));
02357   EXPECT_TRUE(mTR.canTransform("child", "child", old_time));
02358   EXPECT_TRUE(mTR.canTransform("child", "child", valid_time));
02359   EXPECT_TRUE(mTR.canTransform("child", "child", future_time));
02360 
02361   // Advanced API Tests
02362 
02363   // Source = Fixed
02364   //zero data in fixed frame should pass
02365   EXPECT_TRUE(mTR.canTransform("child", zero_time, "parent", valid_time, "child"));
02366   EXPECT_TRUE(mTR.canTransform("child", zero_time, "other_child", valid_time, "child"));
02367   //Old data in fixed frame should pass
02368   EXPECT_TRUE(mTR.canTransform("child", old_time, "parent", valid_time, "child"));
02369   EXPECT_TRUE(mTR.canTransform("child", old_time, "other_child", valid_time, "child"));
02370   //valid data in fixed frame should pass
02371   EXPECT_TRUE(mTR.canTransform("child", valid_time, "parent", valid_time, "child"));
02372   EXPECT_TRUE(mTR.canTransform("child", valid_time, "other_child", valid_time, "child"));
02373   //future data in fixed frame should pass
02374   EXPECT_TRUE(mTR.canTransform("child", future_time, "parent", valid_time, "child"));
02375   EXPECT_TRUE(mTR.canTransform("child", future_time, "other_child", valid_time, "child"));
02376 
02377   //transforming through fixed into the past
02378   EXPECT_FALSE(mTR.canTransform("child", valid_time, "parent", old_time, "child"));
02379   EXPECT_FALSE(mTR.canTransform("child", valid_time, "other_child", old_time, "child"));
02380   //transforming through fixed into the future
02381   EXPECT_FALSE(mTR.canTransform("child", valid_time, "parent", future_time, "child"));
02382   EXPECT_FALSE(mTR.canTransform("child", valid_time, "other_child", future_time, "child"));
02383 
02384   // Target = Fixed
02385   //zero data in fixed frame should pass
02386   EXPECT_TRUE(mTR.canTransform("child", zero_time, "parent", valid_time, "parent"));
02387   //Old data in fixed frame should pass
02388   EXPECT_FALSE(mTR.canTransform("child", old_time, "parent", valid_time, "parent"));
02389   //valid data in fixed frame should pass
02390   EXPECT_TRUE(mTR.canTransform("child", valid_time, "parent", valid_time, "parent"));
02391   //future data in fixed frame should pass
02392   EXPECT_FALSE(mTR.canTransform("child", future_time, "parent", valid_time, "parent"));
02393 
02394   //transforming through fixed into the zero
02395   EXPECT_TRUE(mTR.canTransform("child", valid_time, "parent", zero_time, "parent"));
02396   //transforming through fixed into the past
02397   EXPECT_TRUE(mTR.canTransform("child", valid_time, "parent", old_time, "parent"));
02398   //transforming through fixed into the valid
02399   EXPECT_TRUE(mTR.canTransform("child", valid_time, "parent", valid_time, "parent"));
02400   //transforming through fixed into the future
02401   EXPECT_TRUE(mTR.canTransform("child", valid_time, "parent", future_time, "parent"));
02402 
02403 }
02404 
02405 TEST(tf, lookupTransform)
02406 {
02407   Transformer mTR;
02408   //Create a two link tree between times 10 and 20
02409   for (int i = 10; i < 20; i++)
02410   {
02411     mTR.setTransform(  StampedTransform (btTransform(btQuaternion(1,0,0), btVector3(0,0,0)), ros::Time().fromSec(i),  "parent", "child"));
02412     mTR.setTransform(  StampedTransform (btTransform(btQuaternion(1,0,0), btVector3(0,0,0)), ros::Time().fromSec(i),  "parent", "other_child"));
02413   }
02414 
02415   // four different timestamps related to tf state
02416   ros::Time zero_time = ros::Time().fromSec(0);
02417   ros::Time old_time = ros::Time().fromSec(5);
02418   ros::Time valid_time = ros::Time().fromSec(15);
02419   ros::Time future_time = ros::Time().fromSec(25);
02420 
02421   //output
02422   tf::StampedTransform output;
02423 
02424   // Basic API Tests
02425 
02426   try
02427   {
02428     //confirm zero length list disconnected will return true
02429     mTR.lookupTransform("some_frame","some_frame", zero_time, output);
02430     mTR.lookupTransform("some_frame","some_frame", old_time, output);
02431     mTR.lookupTransform("some_frame","some_frame", valid_time, output);
02432     mTR.lookupTransform("some_frame","some_frame", future_time, output);
02433     mTR.lookupTransform("child","child", future_time, output);
02434     mTR.lookupTransform("other_child","other_child", future_time, output);
02435 
02436     //Valid data should pass
02437     mTR.lookupTransform("child", "parent", valid_time, output);
02438     mTR.lookupTransform("child", "other_child", valid_time, output);
02439     
02440     //zero data should pass
02441     mTR.lookupTransform("child", "parent", zero_time, output);
02442     mTR.lookupTransform("child", "other_child", zero_time, output);
02443   }
02444   catch (tf::TransformException &ex)
02445   {
02446     printf("Exception improperly thrown: %s", ex.what());
02447     EXPECT_FALSE("Exception thrown");
02448   }
02449   try
02450   {
02451     //Old data should fail
02452     mTR.lookupTransform("child", "parent", old_time, output);
02453     EXPECT_FALSE("Exception should have been thrown");
02454   }
02455   catch (tf::TransformException)
02456   {
02457     EXPECT_TRUE("Exception Thrown Correctly");
02458   }
02459   try {
02460     //Future data should fail
02461     mTR.lookupTransform("child", "parent", future_time, output);
02462     EXPECT_FALSE("Exception should have been thrown");
02463   }
02464   catch (tf::TransformException)
02465   {
02466     EXPECT_TRUE("Exception Thrown Correctly");
02467   }
02468     
02469   try {
02470     //Same Frame should pass for all times
02471     mTR.lookupTransform("child", "child", zero_time, output);
02472     mTR.lookupTransform("child", "child", old_time, output);
02473     mTR.lookupTransform("child", "child", valid_time, output);
02474     mTR.lookupTransform("child", "child", future_time, output);
02475     
02476     // Advanced API Tests
02477     
02478     // Source = Fixed
02479     //zero data in fixed frame should pass
02480     mTR.lookupTransform("child", zero_time, "parent", valid_time, "child", output);
02481     mTR.lookupTransform("child", zero_time, "other_child", valid_time, "child", output);
02482     //Old data in fixed frame should pass
02483     mTR.lookupTransform("child", old_time, "parent", valid_time, "child", output);
02484     mTR.lookupTransform("child", old_time, "other_child", valid_time, "child", output);
02485     //valid data in fixed frame should pass
02486     mTR.lookupTransform("child", valid_time, "parent", valid_time, "child", output);
02487     mTR.lookupTransform("child", valid_time, "other_child", valid_time, "child", output);
02488     //future data in fixed frame should pass
02489     mTR.lookupTransform("child", future_time, "parent", valid_time, "child", output);
02490     mTR.lookupTransform("child", future_time, "other_child", valid_time, "child", output);
02491   }
02492   catch (tf::TransformException &ex)
02493   {
02494     printf("Exception improperly thrown: %s", ex.what());
02495     EXPECT_FALSE("Exception incorrectly thrown");
02496   }
02497 
02498   try {
02499     //transforming through fixed into the past
02500     mTR.lookupTransform("child", valid_time, "parent", old_time, "child", output);
02501     EXPECT_FALSE("Exception should have been thrown");
02502   }
02503   catch (tf::TransformException)
02504   {
02505     EXPECT_TRUE("Exception Thrown Correctly");
02506   }
02507 
02508   try {
02509     //transforming through fixed into the future
02510     mTR.lookupTransform("child", valid_time, "parent", future_time, "child", output);
02511     EXPECT_FALSE("Exception should have been thrown");
02512   }
02513   catch (tf::TransformException)
02514   {
02515     EXPECT_TRUE("Exception Thrown Correctly");
02516   }
02517 
02518   try {
02519     // Target = Fixed
02520     //zero data in fixed frame should pass
02521     mTR.lookupTransform("child", zero_time, "parent", valid_time, "parent", output);
02522     //valid data in fixed frame should pass
02523     mTR.lookupTransform("child", valid_time, "parent", valid_time, "parent", output);
02524   }
02525   catch (tf::TransformException &ex)
02526   {
02527     printf("Exception improperly thrown: %s", ex.what());
02528     EXPECT_FALSE("Exception incorrectly thrown");
02529   }
02530 
02531   try {
02532   //Old data in fixed frame should pass
02533   mTR.lookupTransform("child", old_time, "parent", valid_time, "parent", output);
02534       EXPECT_FALSE("Exception should have been thrown");
02535   }
02536   catch (tf::TransformException)
02537   {
02538     EXPECT_TRUE("Exception Thrown Correctly");
02539   }
02540   try {
02541     //future data in fixed frame should pass
02542     mTR.lookupTransform("child", future_time, "parent", valid_time, "parent", output);
02543     EXPECT_FALSE("Exception should have been thrown");
02544   }
02545   catch (tf::TransformException)
02546   {
02547     EXPECT_TRUE("Exception Thrown Correctly");
02548   }
02549 
02550   try {
02551     //transforming through fixed into the zero
02552     mTR.lookupTransform("child", valid_time, "parent", zero_time, "parent", output);
02553     //transforming through fixed into the past
02554     mTR.lookupTransform("child", valid_time, "parent", old_time, "parent", output);
02555     //transforming through fixed into the valid
02556     mTR.lookupTransform("child", valid_time, "parent", valid_time, "parent", output);
02557     //transforming through fixed into the future
02558     mTR.lookupTransform("child", valid_time, "parent", future_time, "parent", output);
02559   }
02560   catch (tf::TransformException &ex)
02561   {
02562     printf("Exception improperly thrown: %s", ex.what());
02563     EXPECT_FALSE("Exception improperly thrown");
02564   }
02565   
02566 
02567   //make sure zero goes to now for zero length
02568   try
02569   {
02570     ros::Time now1 = ros::Time::now();
02571 
02572     mTR.lookupTransform("a", "a", ros::Time(),output);
02573     EXPECT_LE(now1.toSec(), output.stamp_.toSec());
02574     EXPECT_LE(output.stamp_.toSec(), ros::Time::now().toSec());
02575   }
02576   catch (tf::TransformException &ex)
02577   {
02578     printf("Exception improperly thrown: %s", ex.what());
02579     EXPECT_FALSE("Exception improperly thrown");
02580   }
02581   
02582 }
02583 
02584 
02585 TEST(tf, getFrameStrings)
02586 {
02587   Transformer mTR;
02588 
02589 
02590   mTR.setTransform(  StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(4000),  "/parent", "/b"));
02591   std::vector <std::string> frames_string;
02592   mTR.getFrameStrings(frames_string);
02593   ASSERT_EQ(frames_string.size(), (unsigned)2);
02594   EXPECT_STREQ(frames_string[0].c_str(), std::string("/b").c_str());
02595   EXPECT_STREQ(frames_string[1].c_str(), std::string("/parent").c_str());
02596 
02597 
02598   mTR.setTransform(  StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(4000),  "/frame", "/other"));
02599   
02600   mTR.getFrameStrings(frames_string);
02601   ASSERT_EQ(frames_string.size(), (unsigned)4);
02602   EXPECT_STREQ(frames_string[0].c_str(), std::string("/b").c_str());
02603   EXPECT_STREQ(frames_string[1].c_str(), std::string("/parent").c_str());
02604   EXPECT_STREQ(frames_string[2].c_str(), std::string("/other").c_str());
02605   EXPECT_STREQ(frames_string[3].c_str(), std::string("/frame").c_str());
02606 
02607 }
02608 
02609 bool expectInvalidQuaternion(tf::Quaternion q)
02610 {
02611   try
02612   {
02613     tf::assertQuaternionValid(q);
02614     printf("this should have thrown\n");
02615     return false;
02616   }
02617   catch (tf::InvalidArgument &ex)  
02618   {
02619     return true;
02620   }
02621   catch  (...)
02622   {
02623     printf("A different type of exception was expected\n");
02624     return false;
02625   }
02626   return false;
02627 }
02628 
02629 bool expectValidQuaternion(tf::Quaternion q)
02630 {
02631   try
02632   {
02633     tf::assertQuaternionValid(q);
02634   }
02635   catch (tf::TransformException &ex)  
02636   {
02637     return false;
02638   }
02639   return true;
02640 }
02641 
02642 bool expectInvalidQuaternion(geometry_msgs::Quaternion q)
02643 {
02644   try
02645   {
02646     tf::assertQuaternionValid(q);
02647     printf("this should have thrown\n");
02648     return false;
02649   }
02650   catch (tf::InvalidArgument &ex)  
02651   {
02652     return true;
02653   }
02654   catch  (...)
02655   {
02656     printf("A different type of exception was expected\n");
02657     return false;
02658   }
02659   return false;
02660 }
02661 
02662 bool expectValidQuaternion(geometry_msgs::Quaternion q)
02663 {
02664   try
02665   {
02666     tf::assertQuaternionValid(q);
02667   }
02668   catch (tf::TransformException &ex)  
02669   {
02670     return false;
02671   }
02672   return true;
02673 }
02674 
02675 
02676 TEST(tf, assertQuaternionValid)
02677 {
02678   tf::Quaternion q(1,0,0,0);
02679   EXPECT_TRUE(expectValidQuaternion(q));
02680   q.setX(0);
02681   EXPECT_TRUE(expectInvalidQuaternion(q));
02682   q.setY(1);
02683   EXPECT_TRUE(expectValidQuaternion(q));
02684   q.setZ(1);
02685   EXPECT_TRUE(expectInvalidQuaternion(q));
02686   q.setY(0);
02687   EXPECT_TRUE(expectValidQuaternion(q));
02688   q.setW(1);
02689   EXPECT_TRUE(expectInvalidQuaternion(q));
02690   q.setZ(0);
02691   EXPECT_TRUE(expectValidQuaternion(q));
02692   q.setZ(sqrt(2.0)/2.0);
02693   EXPECT_TRUE(expectInvalidQuaternion(q));
02694   q.setW(sqrt(2.0)/2.0);
02695   EXPECT_TRUE(expectValidQuaternion(q));
02696 
02697   q.setZ(sqrt(2.0)/2.0 + 0.01);
02698   EXPECT_TRUE(expectInvalidQuaternion(q));
02699 
02700   q.setZ(sqrt(2.0)/2.0 - 0.01);
02701   EXPECT_TRUE(expectInvalidQuaternion(q));
02702 
02703   EXPECT_THROW(tf::assertQuaternionValid(q), tf::InvalidArgument);
02704   //    Waiting for gtest 1.1 or later
02705   //  EXPECT_NO_THROW(tf::assertQuaternionValid(q));
02706   //q.setX(0);
02707   //EXPECT_THROW(tf::assertQuaternionValid(q), tf::InvalidArgument);
02708   //q.setY(1);
02709   //EXPECT_NO_THROW(tf::assertQuaternionValid(q));
02710   
02711 }
02712 TEST(tf, assertQuaternionMsgValid)
02713 {
02714   geometry_msgs::Quaternion q;
02715   q.x = 1;//others zeroed to start
02716 
02717   EXPECT_TRUE(expectValidQuaternion(q));
02718   q.x = 0;
02719   EXPECT_TRUE(expectInvalidQuaternion(q));
02720   q.y = 1;
02721   EXPECT_TRUE(expectValidQuaternion(q));
02722   q.z = 1;
02723   EXPECT_TRUE(expectInvalidQuaternion(q));
02724   q.y = 0;
02725   EXPECT_TRUE(expectValidQuaternion(q));
02726   q.w = 1;
02727   EXPECT_TRUE(expectInvalidQuaternion(q));
02728   q.z = 0;
02729   EXPECT_TRUE(expectValidQuaternion(q));
02730   q.z = sqrt(2.0)/2.0;
02731   EXPECT_TRUE(expectInvalidQuaternion(q));
02732   q.w = sqrt(2.0)/2.0;
02733   EXPECT_TRUE(expectValidQuaternion(q));
02734 
02735   q.z = sqrt(2.0)/2.0 + 0.01;
02736   EXPECT_TRUE(expectInvalidQuaternion(q));
02737 
02738   q.z = sqrt(2.0)/2.0 - 0.01;
02739   EXPECT_TRUE(expectInvalidQuaternion(q));
02740 
02741 
02742   //    Waiting for gtest 1.1 or later
02743   //  EXPECT_NO_THROW(tf::assertQuaternionValid(q));
02744   //q.x = 0);
02745   //EXPECT_THROW(tf::assertQuaternionValid(q), tf::InvalidArgument);
02746   //q.y = 1);
02747   //EXPECT_NO_THROW(tf::assertQuaternionValid(q));
02748 
02749 }
02750 
02751 
02752 TEST(tf2_stamped, OperatorEqualEqual)
02753 {
02754   btTransform transform0, transform1, transform0a;
02755   transform0.setIdentity();
02756   transform0a.setIdentity();
02757   transform1.setIdentity();
02758   transform1.setOrigin(btVector3(1, 0, 0));
02759   tf2::StampedTransform stamped_transform_reference(transform0a, ros::Time(), "frame_id", "child_frame_id");
02760   tf2::StampedTransform stamped_transform0A(transform0, ros::Time(), "frame_id", "child_frame_id");
02761   EXPECT_TRUE(stamped_transform0A == stamped_transform_reference); // Equal
02762   tf2::StampedTransform stamped_transform0B(transform0, ros::Time(), "frame_id_not_equal", "child_frame_id");
02763   EXPECT_FALSE(stamped_transform0B == stamped_transform_reference); // Different Frame id
02764   tf2::StampedTransform stamped_transform0C(transform0, ros::Time(1.0), "frame_id", "child_frame_id");
02765   EXPECT_FALSE(stamped_transform0C == stamped_transform_reference); // Different Time
02766   tf2::StampedTransform stamped_transform0D(transform0, ros::Time(1.0), "frame_id_not_equal", "child_frame_id");
02767   EXPECT_FALSE(stamped_transform0D == stamped_transform_reference); // Different frame id and time
02768   tf2::StampedTransform stamped_transform0E(transform1, ros::Time(), "frame_id_not_equal", "child_frame_id");
02769   EXPECT_FALSE(stamped_transform0E == stamped_transform_reference); // Different transform, frame id
02770   tf2::StampedTransform stamped_transform0F(transform1, ros::Time(1.0), "frame_id", "child_frame_id");
02771   EXPECT_FALSE(stamped_transform0F == stamped_transform_reference); // Different transform, time
02772   tf2::StampedTransform stamped_transform0G(transform1, ros::Time(1.0), "frame_id_not_equal", "child_frame_id");
02773   EXPECT_FALSE(stamped_transform0G == stamped_transform_reference); // Different transform, frame id and time
02774   tf2::StampedTransform stamped_transform0H(transform1, ros::Time(), "frame_id", "child_frame_id");
02775   EXPECT_FALSE(stamped_transform0H == stamped_transform_reference); // Different transform
02776 
02777 
02778   //Different child_frame_id
02779   tf2::StampedTransform stamped_transform1A(transform0, ros::Time(), "frame_id", "child_frame_id2");
02780   EXPECT_FALSE(stamped_transform1A == stamped_transform_reference); // Equal
02781   tf2::StampedTransform stamped_transform1B(transform0, ros::Time(), "frame_id_not_equal", "child_frame_id2");
02782   EXPECT_FALSE(stamped_transform1B == stamped_transform_reference); // Different Frame id
02783   tf2::StampedTransform stamped_transform1C(transform0, ros::Time(1.0), "frame_id", "child_frame_id2");
02784   EXPECT_FALSE(stamped_transform1C == stamped_transform_reference); // Different Time
02785   tf2::StampedTransform stamped_transform1D(transform0, ros::Time(1.0), "frame_id_not_equal", "child_frame_id2");
02786   EXPECT_FALSE(stamped_transform1D == stamped_transform_reference); // Different frame id and time
02787   tf2::StampedTransform stamped_transform1E(transform1, ros::Time(), "frame_id_not_equal", "child_frame_id2");
02788   EXPECT_FALSE(stamped_transform1E == stamped_transform_reference); // Different transform, frame id
02789   tf2::StampedTransform stamped_transform1F(transform1, ros::Time(1.0), "frame_id", "child_frame_id2");
02790   EXPECT_FALSE(stamped_transform1F == stamped_transform_reference); // Different transform, time
02791   tf2::StampedTransform stamped_transform1G(transform1, ros::Time(1.0), "frame_id_not_equal", "child_frame_id2");
02792   EXPECT_FALSE(stamped_transform1G == stamped_transform_reference); // Different transform, frame id and time
02793   tf2::StampedTransform stamped_transform1H(transform1, ros::Time(), "frame_id", "child_frame_id2");
02794   EXPECT_FALSE(stamped_transform1H == stamped_transform_reference); // Different transform
02795 
02796 }
02797 
02798 TEST(tf2_stamped, OperatorEqual)
02799 {
02800   btTransform pose0, pose1, pose0a;
02801   pose0.setIdentity();
02802   pose1.setIdentity();
02803   pose1.setOrigin(btVector3(1, 0, 0));
02804   tf2::Stamped<btTransform> stamped_pose0(pose0, ros::Time(), "frame_id");
02805   tf2::Stamped<btTransform> stamped_pose1(pose1, ros::Time(1.0), "frame_id_not_equal");
02806   EXPECT_FALSE(stamped_pose1 == stamped_pose0);
02807   stamped_pose1 = stamped_pose0;
02808   EXPECT_TRUE(stamped_pose1 == stamped_pose0);
02809 
02810 }
02811   */
02812 int main(int argc, char **argv){
02813   testing::InitGoogleTest(&argc, argv);
02814   ros::Time::init(); //needed for ros::TIme::now()
02815   ros::init(argc, argv, "tf_unittest");
02816   return RUN_ALL_TESTS();
02817 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends Defines


test_tf2
Author(s): Tully Foote, Eitan Marder-Eppstein
autogenerated on Mon Feb 11 2013 02:14:30