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


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Mon Oct 6 2014 00:12:04