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(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   
01570 
01571   // Check assertion of extra string
01572   std::string error_str;
01573   EXPECT_FALSE(mTR.waitForTransform("parent", "me", ros::Time().fromNSec(10000000), ros::Duration().fromSec(1.0), ros::Duration().fromSec(0.01), &error_str));
01574   EXPECT_STREQ(tf2_ros::threading_error.c_str(), error_str.c_str());
01575 
01576   // check that it doesn't segfault if NULL
01577   EXPECT_FALSE(mTR.waitForTransform("parent", "me", ros::Time().fromNSec(10000000), ros::Duration().fromSec(1.0), ros::Duration().fromSec(0.01)));
01578 
01579   
01580 
01581   //A seperate thread is required to use the blocking call for normal usage
01582   // This isn't actually using it, but it will not affect this direct usage case.  
01583   mTR.setUsingDedicatedThread(true);  
01584   // make sure timeout is resonably lengthed
01585 
01586   ros::Duration timeout = ros::Duration().fromSec(1.0);
01587   ros::Duration poll_freq = ros::Duration().fromSec(0.1);
01588   double eps = 0.2;
01589 
01590   // Default polling freq
01591   ros::Time start_time = ros::Time::now();
01592   EXPECT_FALSE(mTR.waitForTransform("parent", "me", ros::Time().fromNSec(10000000), timeout));
01593   ros::Time stop_time = ros::Time::now();
01594   EXPECT_TRUE(fabs(((stop_time-start_time)-timeout).toSec()) < eps);
01595 
01596   // 10Hz polling
01597   start_time = ros::Time::now();
01598   EXPECT_FALSE(mTR.waitForTransform("parent", "me", ros::Time().fromNSec(10000000), timeout, poll_freq));
01599   stop_time = ros::Time::now();
01600   EXPECT_TRUE(fabs(((stop_time-start_time)-timeout).toSec()) < eps);
01601   
01602 
01603   //Now it should be able to transform
01604   mTR.setTransform( StampedTransform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(10000000),  "parent", "me"));
01605   
01606   start_time = ros::Time::now();
01607   EXPECT_TRUE(mTR.waitForTransform("parent", "me", ros::Time().fromNSec(10000000),timeout));
01608   stop_time = ros::Time::now();
01609   EXPECT_TRUE(fabs(((stop_time-start_time)).toSec()) < eps);
01610 
01611 
01612   start_time = ros::Time::now();
01613   EXPECT_TRUE(mTR.waitForTransform("parent", "me", ros::Time().fromNSec(10000000),timeout, poll_freq));
01614   stop_time = ros::Time::now();
01615   EXPECT_TRUE(fabs(((stop_time-start_time)).toSec()) < eps);
01616 }
01617 
01618 
01619 TEST(tf, Exceptions)
01620 {
01621 
01622  tf::Transformer mTR(true);
01623 
01624  
01625  Stamped<tf::Transform> outpose;
01626 
01627  //connectivity when no data
01628  EXPECT_FALSE(mTR.canTransform("parent", "me", ros::Time().fromNSec(10000000)));
01629  try 
01630  {
01631    mTR.transformPose("parent",Stamped<Pose>(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(10000000) , "me"), outpose);
01632    EXPECT_FALSE("ConnectivityException Not Thrown");   
01633  }
01634  catch ( tf::LookupException &ex)
01635  {
01636    EXPECT_TRUE("Lookupgh Exception Caught");
01637  }
01638  catch (tf::TransformException& ex)
01639  {
01640    printf("%s\n",ex.what());
01641    EXPECT_FALSE("Other Exception Caught");
01642  }
01643  
01644  mTR.setTransform( StampedTransform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(100000), "parent", "me"));
01645 
01646  //Extrapolation not valid with one value
01647  EXPECT_FALSE(mTR.canTransform("parent", "me", ros::Time().fromNSec(200000)));
01648  try 
01649  {
01650    mTR.transformPose("parent",Stamped<Pose>(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(200000) , "me"), outpose);
01651    EXPECT_TRUE("ExtrapolationException Not Thrown");
01652  }
01653  catch ( tf::ExtrapolationException &ex)
01654  {
01655    EXPECT_TRUE("Extrapolation Exception Caught");
01656  }
01657  catch (tf::TransformException& ex)
01658  {
01659    printf("%s\n",ex.what());
01660    EXPECT_FALSE("Other Exception Caught");
01661  }
01662  
01663 
01664  mTR.setTransform( StampedTransform (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(300000), "parent", "me"));
01665 
01666  //NO Extration when Interpolating
01667  //inverse list
01668  EXPECT_TRUE(mTR.canTransform("parent", "me", ros::Time().fromNSec(200000)));
01669  try 
01670  {
01671    mTR.transformPose("parent",Stamped<Pose>(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(200000) , "me"), outpose);
01672    EXPECT_TRUE("ExtrapolationException Not Thrown");
01673  }
01674  catch ( tf::ExtrapolationException &ex)
01675  {
01676    EXPECT_FALSE("Extrapolation Exception Caught");
01677  }
01678  catch (tf::TransformException& ex)
01679  {
01680    printf("%s\n",ex.what());
01681    EXPECT_FALSE("Other Exception Caught");
01682  }
01683 
01684 
01685 
01686  //forward list
01687  EXPECT_TRUE(mTR.canTransform("me", "parent", ros::Time().fromNSec(200000)));
01688  try 
01689  {
01690    mTR.transformPose("me",Stamped<Pose>(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(200000) , "parent"), outpose);
01691    EXPECT_TRUE("ExtrapolationException Not Thrown");
01692  }
01693  catch ( tf::ExtrapolationException &ex)
01694  {
01695    EXPECT_FALSE("Extrapolation Exception Caught");
01696  }
01697  catch (tf::TransformException& ex)
01698  {
01699    printf("%s\n",ex.what());
01700    EXPECT_FALSE("Other Exception Caught");
01701  }
01702   
01703 
01704  //Extrapolating backwards
01705  //inverse list
01706  EXPECT_FALSE(mTR.canTransform("parent", "me", ros::Time().fromNSec(1000)));
01707  try 
01708  {
01709    mTR.transformPose("parent",Stamped<Pose> (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(1000) , "me"), outpose);
01710    EXPECT_FALSE("ExtrapolationException Not Thrown");
01711  }
01712  catch ( tf::ExtrapolationException &ex)
01713  {
01714    EXPECT_TRUE("Extrapolation Exception Caught");
01715  }
01716  catch (tf::TransformException& ex)
01717  {
01718    printf("%s\n",ex.what());
01719    EXPECT_FALSE("Other Exception Caught");
01720  }
01721  //forwards list
01722  EXPECT_FALSE(mTR.canTransform("me", "parent", ros::Time().fromNSec(1000)));
01723  try 
01724  {
01725    mTR.transformPose("me",Stamped<Pose> (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(1000) , "parent"), outpose);
01726    EXPECT_FALSE("ExtrapolationException Not Thrown");
01727  }
01728  catch ( tf::ExtrapolationException &ex)
01729  {
01730    EXPECT_TRUE("Extrapolation Exception Caught");
01731  }
01732  catch (tf::TransformException& ex)
01733  {
01734    printf("%s\n",ex.what());
01735    EXPECT_FALSE("Other Exception Caught");
01736  }
01737   
01738 
01739 
01740  // Test extrapolation inverse and forward linkages FORWARD
01741 
01742  //inverse list
01743  EXPECT_FALSE(mTR.canTransform("parent", "me", ros::Time().fromNSec(350000)));
01744  try 
01745  {
01746    mTR.transformPose("parent", Stamped<Pose> (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(350000) , "me"), outpose);
01747    EXPECT_FALSE("ExtrapolationException Not Thrown");
01748  }
01749  catch ( tf::ExtrapolationException &ex)
01750  {
01751    EXPECT_TRUE("Extrapolation Exception Caught");
01752  }
01753  catch (tf::TransformException& ex)
01754  {
01755    printf("%s\n",ex.what());
01756    EXPECT_FALSE("Other Exception Caught");
01757  }
01758 
01759  //forward list
01760  EXPECT_FALSE(mTR.canTransform("parent", "me", ros::Time().fromNSec(350000)));
01761  try 
01762  {
01763    mTR.transformPose("me", Stamped<Pose> (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(350000) , "parent"), outpose);
01764    EXPECT_FALSE("ExtrapolationException Not Thrown");
01765  }
01766  catch ( tf::ExtrapolationException &ex)
01767  {
01768    EXPECT_TRUE("Extrapolation Exception Caught");
01769  }
01770  catch (tf::TransformException& ex)
01771  {
01772    printf("%s\n",ex.what());
01773    EXPECT_FALSE("Other Exception Caught");
01774  }
01775   
01776 
01777 
01778 
01779 }
01780 
01781 
01782 
01783 TEST(tf, NoExtrapolationExceptionFromParent)
01784 {
01785   tf::Transformer mTR(true, ros::Duration().fromNSec(1000000));
01786   
01787 
01788 
01789   mTR.setTransform(  StampedTransform (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(1000), "parent", "a"));
01790   mTR.setTransform(  StampedTransform (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(10000),  "parent", "a"));
01791   
01792   
01793   mTR.setTransform(  StampedTransform (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(1000),  "parent", "b"));
01794   mTR.setTransform(  StampedTransform (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(10000),  "parent", "b"));
01795 
01796   mTR.setTransform(  StampedTransform (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(1000),  "parent's parent", "parent"));
01797   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"));
01798 
01799   mTR.setTransform(  StampedTransform (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(10000),  "parent's parent", "parent"));
01800   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"));
01801 
01802   Stamped<Point> output;
01803 
01804   try
01805   {
01806     mTR.transformPoint( "b", Stamped<Point>(Point(1,1,1), ros::Time().fromNSec(2000), "a"), output);
01807   }
01808   catch (ExtrapolationException &ex)
01809   {
01810     EXPECT_FALSE("Shouldn't have gotten this exception");
01811   }
01812 
01813 
01814 
01815 };
01816 
01817 
01818 
01819 TEST(tf, ExtrapolationFromOneValue)
01820 {
01821   tf::Transformer mTR(true, ros::Duration().fromNSec(1000000));
01822   
01823 
01824 
01825   mTR.setTransform(  StampedTransform (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(1000),  "parent", "a"));
01826 
01827   mTR.setTransform(  StampedTransform (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(1000),  "parent's parent", "parent"));
01828 
01829 
01830   Stamped<Point> output;
01831 
01832   bool excepted = false;
01833   //Past time
01834   try
01835   {
01836     mTR.transformPoint( "parent", Stamped<Point>(Point(1,1,1), ros::Time().fromNSec(10), "a"), output);
01837   }
01838   catch (ExtrapolationException &ex)
01839   {
01840     excepted = true;
01841   }
01842   
01843   EXPECT_TRUE(excepted);
01844 
01845   excepted = false;
01846   //Future one element
01847   try
01848   {
01849     mTR.transformPoint( "parent", Stamped<Point>(Point(1,1,1), ros::Time().fromNSec(100000), "a"), output);
01850   }
01851   catch (ExtrapolationException &ex)
01852   {
01853     excepted = true;
01854   }
01855   
01856   EXPECT_TRUE(excepted);
01857 
01858   //Past multi link
01859   excepted = false;
01860   try
01861   {
01862     mTR.transformPoint( "parent's parent", Stamped<Point>(Point(1,1,1), ros::Time().fromNSec(1), "a"), output);
01863   }
01864   catch (ExtrapolationException &ex)
01865   {
01866     excepted = true;
01867   }
01868   
01869   EXPECT_TRUE(excepted);
01870 
01871   //Future case multi link
01872   excepted = false;
01873   try
01874   {
01875     mTR.transformPoint( "parent's parent", Stamped<Point>(Point(1,1,1), ros::Time().fromNSec(10000), "a"), output);
01876   }
01877   catch (ExtrapolationException &ex)
01878   {
01879     excepted = true;
01880   }
01881   
01882   EXPECT_TRUE(excepted);
01883 
01884   mTR.setTransform(  StampedTransform (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(20000),  "parent", "a"));
01885 
01886   excepted = false;
01887   try
01888   {
01889     mTR.transformPoint( "parent", Stamped<Point>(Point(1,1,1), ros::Time().fromNSec(10000), "a"), output);
01890   }
01891   catch (ExtrapolationException &ex)
01892   {
01893     excepted = true;
01894   }
01895   
01896   EXPECT_FALSE(excepted);
01897 
01898 };
01899 
01900 
01901 
01902 
01903 TEST(tf, RepeatedTimes)
01904 {
01905   Transformer mTR;
01906   tf::Quaternion qt1, qt2;
01907   qt1.setRPY(1,0,0);
01908   qt2.setRPY(1,1,0);
01909   mTR.setTransform(  StampedTransform (tf::Transform(qt1, tf::Vector3(0,0,0)), ros::Time().fromNSec(4000),  "parent", "b"));
01910   mTR.setTransform(  StampedTransform (tf::Transform(qt2, tf::Vector3(0,0,0)), ros::Time().fromNSec(4000),  "parent", "b"));
01911 
01912   tf::StampedTransform  output;
01913   try{
01914     mTR.lookupTransform("parent", "b" , ros::Time().fromNSec(4000), output);
01915     EXPECT_TRUE(!std::isnan(output.getOrigin().x()));
01916     EXPECT_TRUE(!std::isnan(output.getOrigin().y()));
01917     EXPECT_TRUE(!std::isnan(output.getOrigin().z()));
01918     EXPECT_TRUE(!std::isnan(output.getRotation().x()));
01919     EXPECT_TRUE(!std::isnan(output.getRotation().y()));
01920     EXPECT_TRUE(!std::isnan(output.getRotation().z()));
01921     EXPECT_TRUE(!std::isnan(output.getRotation().w()));
01922   }
01923   catch (...)
01924   {
01925     EXPECT_FALSE("Excetion improperly thrown");
01926   }
01927   
01928 
01929 }
01930 
01931 TEST(tf, frameExists)
01932 {
01933   Transformer mTR;
01934 
01935   // test with fully qualified name
01936   EXPECT_FALSE(mTR.frameExists("/b"));;
01937   EXPECT_FALSE(mTR.frameExists("/parent"));
01938   EXPECT_FALSE(mTR.frameExists("/other"));
01939   EXPECT_FALSE(mTR.frameExists("/frame"));
01940 
01941   //test with resolveping
01942   EXPECT_FALSE(mTR.frameExists("b"));;
01943   EXPECT_FALSE(mTR.frameExists("parent"));
01944   EXPECT_FALSE(mTR.frameExists("other"));
01945   EXPECT_FALSE(mTR.frameExists("frame"));
01946 
01947   tf::Quaternion qt1;
01948   qt1.setRPY(1,0,0);
01949   mTR.setTransform(  StampedTransform (tf::Transform(qt1, tf::Vector3(0,0,0)), ros::Time().fromNSec(4000),  "/parent", "/b"));
01950 
01951 
01952   /* Not applicable anymore with tf2
01953   // test with fully qualified name
01954   EXPECT_TRUE(mTR.frameExists("/b"));
01955   EXPECT_TRUE(mTR.frameExists("/parent"));
01956   EXPECT_FALSE(mTR.frameExists("/other"));
01957   EXPECT_FALSE(mTR.frameExists("/frame"));
01958   */
01959 
01960   //Test with resolveping
01961   EXPECT_TRUE(mTR.frameExists("b"));
01962   EXPECT_TRUE(mTR.frameExists("parent"));
01963   EXPECT_FALSE(mTR.frameExists("other"));
01964   EXPECT_FALSE(mTR.frameExists("frame"));
01965 
01966   tf::Quaternion qt2;
01967   qt2.setRPY(1,1,0);
01968   mTR.setTransform(  StampedTransform (tf::Transform(qt2, tf::Vector3(0,0,0)), ros::Time().fromNSec(4000),  "/frame", "/other"));
01969 
01970   /* Not applicable anymore with tf2
01971   // test with fully qualified name
01972   EXPECT_TRUE(mTR.frameExists("/b"));
01973   EXPECT_TRUE(mTR.frameExists("/parent"));
01974   EXPECT_TRUE(mTR.frameExists("/other"));
01975   EXPECT_TRUE(mTR.frameExists("/frame"));
01976   */
01977   
01978   //Test with resolveping
01979   EXPECT_TRUE(mTR.frameExists("b"));
01980   EXPECT_TRUE(mTR.frameExists("parent"));
01981   EXPECT_TRUE(mTR.frameExists("other"));
01982   EXPECT_TRUE(mTR.frameExists("frame"));
01983 
01984 }
01985 
01986 
01987 TEST(tf, canTransform)
01988 {
01989   Transformer mTR;
01990 
01991   //confirm zero length list disconnected will return true
01992   EXPECT_TRUE(mTR.canTransform("some_frame","some_frame", ros::Time()));
01993   EXPECT_TRUE(mTR.canTransform("some_frame","some_frame", ros::Time::now()));
01994 
01995   //Create a two link tree between times 10 and 20
01996   for (int i = 10; i < 20; i++)
01997   {
01998     tf::Quaternion qt;
01999     qt.setRPY(1,0,0);
02000     mTR.setTransform(  StampedTransform (tf::Transform(qt, tf::Vector3(0,0,0)), ros::Time().fromSec(i),  "parent", "child"));
02001     mTR.setTransform(  StampedTransform (tf::Transform(qt, tf::Vector3(0,0,0)), ros::Time().fromSec(i),  "parent", "other_child"));
02002   }
02003 
02004   // four different timestamps related to tf state
02005   ros::Time zero_time = ros::Time().fromSec(0);
02006   ros::Time old_time = ros::Time().fromSec(5);
02007   ros::Time valid_time = ros::Time().fromSec(15);
02008   ros::Time future_time = ros::Time().fromSec(25);
02009 
02010 
02011   //confirm zero length list disconnected will return true
02012   EXPECT_TRUE(mTR.canTransform("some_frame","some_frame", zero_time));
02013   EXPECT_TRUE(mTR.canTransform("some_frame","some_frame", old_time));
02014   EXPECT_TRUE(mTR.canTransform("some_frame","some_frame", valid_time));
02015   EXPECT_TRUE(mTR.canTransform("some_frame","some_frame", future_time));
02016 
02017   // Basic API Tests
02018 
02019   //Valid data should pass
02020   EXPECT_TRUE(mTR.canTransform("child", "parent", valid_time));
02021   EXPECT_TRUE(mTR.canTransform("child", "other_child", valid_time));
02022 
02023   //zero data should pass
02024   EXPECT_TRUE(mTR.canTransform("child", "parent", zero_time));
02025   EXPECT_TRUE(mTR.canTransform("child", "other_child", zero_time));
02026 
02027   //Old data should fail
02028   EXPECT_FALSE(mTR.canTransform("child", "parent", old_time));
02029   EXPECT_FALSE(mTR.canTransform("child", "other_child", old_time));
02030 
02031   //Future data should fail
02032   EXPECT_FALSE(mTR.canTransform("child", "parent", future_time));
02033   EXPECT_FALSE(mTR.canTransform("child", "other_child", future_time));
02034 
02035   //Same Frame should pass for all times
02036   EXPECT_TRUE(mTR.canTransform("child", "child", zero_time));
02037   EXPECT_TRUE(mTR.canTransform("child", "child", old_time));
02038   EXPECT_TRUE(mTR.canTransform("child", "child", valid_time));
02039   EXPECT_TRUE(mTR.canTransform("child", "child", future_time));
02040 
02041   // Advanced API Tests
02042 
02043   // Source = Fixed
02044   //zero data in fixed frame should pass
02045   EXPECT_TRUE(mTR.canTransform("child", zero_time, "parent", valid_time, "child"));
02046   EXPECT_TRUE(mTR.canTransform("child", zero_time, "other_child", valid_time, "child"));
02047   //Old data in fixed frame should pass
02048   EXPECT_TRUE(mTR.canTransform("child", old_time, "parent", valid_time, "child"));
02049   EXPECT_TRUE(mTR.canTransform("child", old_time, "other_child", valid_time, "child"));
02050   //valid data in fixed frame should pass
02051   EXPECT_TRUE(mTR.canTransform("child", valid_time, "parent", valid_time, "child"));
02052   EXPECT_TRUE(mTR.canTransform("child", valid_time, "other_child", valid_time, "child"));
02053   //future data in fixed frame should pass
02054   EXPECT_TRUE(mTR.canTransform("child", future_time, "parent", valid_time, "child"));
02055   EXPECT_TRUE(mTR.canTransform("child", future_time, "other_child", valid_time, "child"));
02056 
02057   //transforming through fixed into the past
02058   EXPECT_FALSE(mTR.canTransform("child", valid_time, "parent", old_time, "child"));
02059   EXPECT_FALSE(mTR.canTransform("child", valid_time, "other_child", old_time, "child"));
02060   //transforming through fixed into the future
02061   EXPECT_FALSE(mTR.canTransform("child", valid_time, "parent", future_time, "child"));
02062   EXPECT_FALSE(mTR.canTransform("child", valid_time, "other_child", future_time, "child"));
02063 
02064   // Target = Fixed
02065   //zero data in fixed frame should pass
02066   EXPECT_TRUE(mTR.canTransform("child", zero_time, "parent", valid_time, "parent"));
02067   //Old data in fixed frame should pass
02068   EXPECT_FALSE(mTR.canTransform("child", old_time, "parent", valid_time, "parent"));
02069   //valid data in fixed frame should pass
02070   EXPECT_TRUE(mTR.canTransform("child", valid_time, "parent", valid_time, "parent"));
02071   //future data in fixed frame should pass
02072   EXPECT_FALSE(mTR.canTransform("child", future_time, "parent", valid_time, "parent"));
02073 
02074   //transforming through fixed into the zero
02075   EXPECT_TRUE(mTR.canTransform("child", valid_time, "parent", zero_time, "parent"));
02076   //transforming through fixed into the past
02077   EXPECT_TRUE(mTR.canTransform("child", valid_time, "parent", old_time, "parent"));
02078   //transforming through fixed into the valid
02079   EXPECT_TRUE(mTR.canTransform("child", valid_time, "parent", valid_time, "parent"));
02080   //transforming through fixed into the future
02081   EXPECT_TRUE(mTR.canTransform("child", valid_time, "parent", future_time, "parent"));
02082 
02083 }
02084 
02085 TEST(tf, lookupTransform)
02086 {
02087   Transformer mTR;
02088   //Create a two link tree between times 10 and 20
02089   for (int i = 10; i < 20; i++)
02090   {
02091     tf::Quaternion qt;
02092     qt.setRPY(1,0,0);
02093     mTR.setTransform(  StampedTransform (tf::Transform(qt, tf::Vector3(0,0,0)), ros::Time().fromSec(i),  "parent", "child"));
02094     mTR.setTransform(  StampedTransform (tf::Transform(qt, tf::Vector3(0,0,0)), ros::Time().fromSec(i),  "parent", "other_child"));
02095   }
02096 
02097   // four different timestamps related to tf state
02098   ros::Time zero_time = ros::Time().fromSec(0);
02099   ros::Time old_time = ros::Time().fromSec(5);
02100   ros::Time valid_time = ros::Time().fromSec(15);
02101   ros::Time future_time = ros::Time().fromSec(25);
02102 
02103   //output
02104   tf::StampedTransform output;
02105 
02106   // Basic API Tests
02107 
02108   try
02109   {
02110     //confirm zero length list disconnected will return true
02111     mTR.lookupTransform("some_frame1","some_frame1", zero_time, output);
02112     mTR.lookupTransform("some_frame2","some_frame2", old_time, output);
02113     mTR.lookupTransform("some_frame3","some_frame3", valid_time, output);
02114     mTR.lookupTransform("some_frame4","some_frame4", future_time, output);
02115     mTR.lookupTransform("child","child", future_time, output);
02116     mTR.lookupTransform("other_child","other_child", future_time, output);
02117 
02118     //Valid data should pass
02119     mTR.lookupTransform("child", "parent", valid_time, output);
02120     mTR.lookupTransform("child", "other_child", valid_time, output);
02121     
02122     //zero data should pass
02123     mTR.lookupTransform("child", "parent", zero_time, output);
02124     mTR.lookupTransform("child", "other_child", zero_time, output);
02125   }
02126   catch (tf::TransformException &ex)
02127   {
02128     printf("Exception improperly thrown: %s", ex.what());
02129     EXPECT_FALSE("Exception thrown");
02130   }
02131   try
02132   {
02133     //Old data should fail
02134     mTR.lookupTransform("child", "parent", old_time, output);
02135     EXPECT_FALSE("Exception should have been thrown");
02136   }
02137   catch (tf::TransformException)
02138   {
02139     EXPECT_TRUE("Exception Thrown Correctly");
02140   }
02141   try {
02142     //Future data should fail
02143     mTR.lookupTransform("child", "parent", future_time, output);
02144     EXPECT_FALSE("Exception should have been thrown");
02145   }
02146   catch (tf::TransformException)
02147   {
02148     EXPECT_TRUE("Exception Thrown Correctly");
02149   }
02150     
02151   try {
02152     //Same Frame should pass for all times
02153     mTR.lookupTransform("child", "child", zero_time, output);
02154     mTR.lookupTransform("child", "child", old_time, output);
02155     mTR.lookupTransform("child", "child", valid_time, output);
02156     mTR.lookupTransform("child", "child", future_time, output);
02157     
02158     // Advanced API Tests
02159     
02160     // Source = Fixed
02161     //zero data in fixed frame should pass
02162     mTR.lookupTransform("child", zero_time, "parent", valid_time, "child", output);
02163     mTR.lookupTransform("child", zero_time, "other_child", valid_time, "child", output);
02164     //Old data in fixed frame should pass
02165     mTR.lookupTransform("child", old_time, "parent", valid_time, "child", output);
02166     mTR.lookupTransform("child", old_time, "other_child", valid_time, "child", output);
02167     //valid data in fixed frame should pass
02168     mTR.lookupTransform("child", valid_time, "parent", valid_time, "child", output);
02169     mTR.lookupTransform("child", valid_time, "other_child", valid_time, "child", output);
02170     //future data in fixed frame should pass
02171     mTR.lookupTransform("child", future_time, "parent", valid_time, "child", output);
02172     mTR.lookupTransform("child", future_time, "other_child", valid_time, "child", output);
02173   }
02174   catch (tf::TransformException &ex)
02175   {
02176     printf("Exception improperly thrown: %s", ex.what());
02177     EXPECT_FALSE("Exception incorrectly thrown");
02178   }
02179 
02180   try {
02181     //transforming through fixed into the past
02182     mTR.lookupTransform("child", valid_time, "parent", old_time, "child", output);
02183     EXPECT_FALSE("Exception should have been thrown");
02184   }
02185   catch (tf::TransformException)
02186   {
02187     EXPECT_TRUE("Exception Thrown Correctly");
02188   }
02189 
02190   try {
02191     //transforming through fixed into the future
02192     mTR.lookupTransform("child", valid_time, "parent", future_time, "child", output);
02193     EXPECT_FALSE("Exception should have been thrown");
02194   }
02195   catch (tf::TransformException)
02196   {
02197     EXPECT_TRUE("Exception Thrown Correctly");
02198   }
02199 
02200   try {
02201     // Target = Fixed
02202     //zero data in fixed frame should pass
02203     mTR.lookupTransform("child", zero_time, "parent", valid_time, "parent", output);
02204     //valid data in fixed frame should pass
02205     mTR.lookupTransform("child", valid_time, "parent", valid_time, "parent", output);
02206   }
02207   catch (tf::TransformException &ex)
02208   {
02209     printf("Exception improperly thrown: %s", ex.what());
02210     EXPECT_FALSE("Exception incorrectly thrown");
02211   }
02212 
02213   try {
02214   //Old data in fixed frame should pass
02215   mTR.lookupTransform("child", old_time, "parent", valid_time, "parent", output);
02216       EXPECT_FALSE("Exception should have been thrown");
02217   }
02218   catch (tf::TransformException)
02219   {
02220     EXPECT_TRUE("Exception Thrown Correctly");
02221   }
02222   try {
02223     //future data in fixed frame should pass
02224     mTR.lookupTransform("child", future_time, "parent", valid_time, "parent", output);
02225     EXPECT_FALSE("Exception should have been thrown");
02226   }
02227   catch (tf::TransformException)
02228   {
02229     EXPECT_TRUE("Exception Thrown Correctly");
02230   }
02231 
02232   try {
02233     //transforming through fixed into the zero
02234     mTR.lookupTransform("child", valid_time, "parent", zero_time, "parent", output);
02235     //transforming through fixed into the past
02236     mTR.lookupTransform("child", valid_time, "parent", old_time, "parent", output);
02237     //transforming through fixed into the valid
02238     mTR.lookupTransform("child", valid_time, "parent", valid_time, "parent", output);
02239     //transforming through fixed into the future
02240     mTR.lookupTransform("child", valid_time, "parent", future_time, "parent", output);
02241   }
02242   catch (tf::TransformException &ex)
02243   {
02244     printf("Exception improperly thrown: %s", ex.what());
02245     EXPECT_FALSE("Exception improperly thrown");
02246   }
02247   
02248 
02249   //zero time goes to latest known value for frames
02250   try
02251   {
02252   double epsilon = 1e-6;
02253     mTR.lookupTransform("a", "a", ros::Time(),output);
02254     EXPECT_NEAR(ros::Time().toSec(), output.stamp_.toSec(), epsilon);
02255     mTR.lookupTransform("child", "child", ros::Time().fromSec(15),output);
02256     EXPECT_NEAR(15.0, output.stamp_.toSec(), epsilon);
02257     mTR.lookupTransform("child", "child", ros::Time(),output);
02258     EXPECT_NEAR(19.0, output.stamp_.toSec(), epsilon);
02259   }
02260   catch (tf::TransformException &ex)
02261   {
02262     printf("Exception improperly thrown: %s", ex.what());
02263     EXPECT_FALSE("Exception improperly thrown");
02264   }
02265   
02266 }
02267 
02268 
02269 TEST(tf, getFrameStrings)
02270 {
02271   Transformer mTR;
02272 
02273   tf::Quaternion qt1, qt2;
02274   qt1.setRPY(1,0,0);
02275   qt2.setRPY(1,1,0);
02276   mTR.setTransform(  StampedTransform (tf::Transform(qt1, tf::Vector3(0,0,0)), ros::Time().fromNSec(4000),  "/parent", "/b"));
02277   std::vector <std::string> frames_string;
02278   mTR.getFrameStrings(frames_string);
02279   ASSERT_EQ(frames_string.size(), (unsigned)2);
02280   EXPECT_STREQ(frames_string[0].c_str(), std::string("b").c_str());
02281   EXPECT_STREQ(frames_string[1].c_str(), std::string("parent").c_str());
02282 
02283 
02284   mTR.setTransform(  StampedTransform (tf::Transform(qt2, tf::Vector3(0,0,0)), ros::Time().fromNSec(4000),  "/frame", "/other"));
02285   
02286   mTR.getFrameStrings(frames_string);
02287   ASSERT_EQ(frames_string.size(), (unsigned)4);
02288   EXPECT_STREQ(frames_string[0].c_str(), std::string("b").c_str());
02289   EXPECT_STREQ(frames_string[1].c_str(), std::string("parent").c_str());
02290   EXPECT_STREQ(frames_string[2].c_str(), std::string("other").c_str());
02291   EXPECT_STREQ(frames_string[3].c_str(), std::string("frame").c_str());
02292 
02293 }
02294 
02295 bool expectInvalidQuaternion(tf::Quaternion q)
02296 {
02297   try
02298   {
02299     tf::assertQuaternionValid(q);
02300     printf("this should have thrown\n");
02301     return false;
02302   }
02303   catch (tf::InvalidArgument &ex)  
02304   {
02305     return true;
02306   }
02307   catch  (...)
02308   {
02309     printf("A different type of exception was expected\n");
02310     return false;
02311   }
02312   return false;
02313 }
02314 
02315 bool expectValidQuaternion(tf::Quaternion q)
02316 {
02317   try
02318   {
02319     tf::assertQuaternionValid(q);
02320   }
02321   catch (tf::TransformException &ex)  
02322   {
02323     return false;
02324   }
02325   return true;
02326 }
02327 
02328 bool expectInvalidQuaternion(geometry_msgs::Quaternion q)
02329 {
02330   try
02331   {
02332     tf::assertQuaternionValid(q);
02333     printf("this should have thrown\n");
02334     return false;
02335   }
02336   catch (tf::InvalidArgument &ex)  
02337   {
02338     return true;
02339   }
02340   catch  (...)
02341   {
02342     printf("A different type of exception was expected\n");
02343     return false;
02344   }
02345   return false;
02346 }
02347 
02348 bool expectValidQuaternion(geometry_msgs::Quaternion q)
02349 {
02350   try
02351   {
02352     tf::assertQuaternionValid(q);
02353   }
02354   catch (tf::TransformException &ex)  
02355   {
02356     return false;
02357   }
02358   return true;
02359 }
02360 
02361 
02362 TEST(tf, assertQuaternionValid)
02363 {
02364   tf::Quaternion q(1,0,0,0);
02365   EXPECT_TRUE(expectValidQuaternion(q));
02366   q.setX(0);
02367   EXPECT_TRUE(expectInvalidQuaternion(q));
02368   q.setY(1);
02369   EXPECT_TRUE(expectValidQuaternion(q));
02370   q.setZ(1);
02371   EXPECT_TRUE(expectInvalidQuaternion(q));
02372   q.setY(0);
02373   EXPECT_TRUE(expectValidQuaternion(q));
02374   q.setW(1);
02375   EXPECT_TRUE(expectInvalidQuaternion(q));
02376   q.setZ(0);
02377   EXPECT_TRUE(expectValidQuaternion(q));
02378   q.setZ(sqrt(2.0)/2.0);
02379   EXPECT_TRUE(expectInvalidQuaternion(q));
02380   q.setW(sqrt(2.0)/2.0);
02381   EXPECT_TRUE(expectValidQuaternion(q));
02382 
02383   q.setZ(sqrt(2.0)/2.0 + 0.01);
02384   EXPECT_TRUE(expectInvalidQuaternion(q));
02385 
02386   q.setZ(sqrt(2.0)/2.0 - 0.01);
02387   EXPECT_TRUE(expectInvalidQuaternion(q));
02388 
02389 
02390   /*    Waiting for gtest 1.1 or later
02391     EXPECT_NO_THROW(tf::assertQuaternionValid(q));
02392   q.setX(0);
02393   EXPECT_THROW(tf::assertQuaternionValid(q), tf::InvalidArgument);
02394   q.setY(1);
02395   EXPECT_NO_THROW(tf::assertQuaternionValid(q));
02396   */
02397 }
02398 TEST(tf, assertQuaternionMsgValid)
02399 {
02400   geometry_msgs::Quaternion q;
02401   q.x = 1;//others zeroed to start
02402 
02403   EXPECT_TRUE(expectValidQuaternion(q));
02404   q.x = 0;
02405   EXPECT_TRUE(expectInvalidQuaternion(q));
02406   q.y = 1;
02407   EXPECT_TRUE(expectValidQuaternion(q));
02408   q.z = 1;
02409   EXPECT_TRUE(expectInvalidQuaternion(q));
02410   q.y = 0;
02411   EXPECT_TRUE(expectValidQuaternion(q));
02412   q.w = 1;
02413   EXPECT_TRUE(expectInvalidQuaternion(q));
02414   q.z = 0;
02415   EXPECT_TRUE(expectValidQuaternion(q));
02416   q.z = sqrt(2.0)/2.0;
02417   EXPECT_TRUE(expectInvalidQuaternion(q));
02418   q.w = sqrt(2.0)/2.0;
02419   EXPECT_TRUE(expectValidQuaternion(q));
02420 
02421   q.z = sqrt(2.0)/2.0 + 0.01;
02422   EXPECT_TRUE(expectInvalidQuaternion(q));
02423 
02424   q.z = sqrt(2.0)/2.0 - 0.01;
02425   EXPECT_TRUE(expectInvalidQuaternion(q));
02426 
02427 
02428   /*    Waiting for gtest 1.1 or later
02429     EXPECT_NO_THROW(tf::assertQuaternionValid(q));
02430   q.x = 0);
02431   EXPECT_THROW(tf::assertQuaternionValid(q), tf::InvalidArgument);
02432   q.y = 1);
02433   EXPECT_NO_THROW(tf::assertQuaternionValid(q));
02434   */
02435 }
02436 
02437 TEST(data, StampedOperatorEqualEqual)
02438 {
02439   tf::Pose pose0, pose1, pose0a;
02440   pose0.setIdentity();
02441   pose0a.setIdentity();
02442   pose1.setIdentity();
02443   pose1.setOrigin(tf::Vector3(1, 0, 0));
02444   tf::Stamped<tf::Pose> stamped_pose_reference(pose0a, ros::Time(), "frame_id");
02445   tf::Stamped<tf::Pose> stamped_pose0A(pose0, ros::Time(), "frame_id");
02446   EXPECT_TRUE(stamped_pose0A == stamped_pose_reference); // Equal
02447   tf::Stamped<tf::Pose> stamped_pose0B(pose0, ros::Time(), "frame_id_not_equal");
02448   EXPECT_FALSE(stamped_pose0B == stamped_pose_reference); // Different Frame id
02449   tf::Stamped<tf::Pose> stamped_pose0C(pose0, ros::Time(1.0), "frame_id");
02450   EXPECT_FALSE(stamped_pose0C == stamped_pose_reference); // Different Time
02451   tf::Stamped<tf::Pose> stamped_pose0D(pose0, ros::Time(1.0), "frame_id_not_equal");
02452   EXPECT_FALSE(stamped_pose0D == stamped_pose_reference); // Different frame id and time
02453   tf::Stamped<tf::Pose> stamped_pose0E(pose1, ros::Time(), "frame_id_not_equal");
02454   EXPECT_FALSE(stamped_pose0E == stamped_pose_reference); // Different pose, frame id
02455   tf::Stamped<tf::Pose> stamped_pose0F(pose1, ros::Time(1.0), "frame_id");
02456   EXPECT_FALSE(stamped_pose0F == stamped_pose_reference); // Different pose, time
02457   tf::Stamped<tf::Pose> stamped_pose0G(pose1, ros::Time(1.0), "frame_id_not_equal");
02458   EXPECT_FALSE(stamped_pose0G == stamped_pose_reference); // Different pose, frame id and time
02459   tf::Stamped<tf::Pose> stamped_pose0H(pose1, ros::Time(), "frame_id");
02460   EXPECT_FALSE(stamped_pose0H == stamped_pose_reference); // Different pose
02461 
02462 }
02463 
02464 TEST(data, StampedTransformOperatorEqualEqual)
02465 {
02466   tf::Transform transform0, transform1, transform0a;
02467   transform0.setIdentity();
02468   transform0a.setIdentity();
02469   transform1.setIdentity();
02470   transform1.setOrigin(tf::Vector3(1, 0, 0));
02471   tf::StampedTransform stamped_transform_reference(transform0a, ros::Time(), "frame_id", "child_frame_id");
02472   tf::StampedTransform stamped_transform0A(transform0, ros::Time(), "frame_id", "child_frame_id");
02473   EXPECT_TRUE(stamped_transform0A == stamped_transform_reference); // Equal
02474   tf::StampedTransform stamped_transform0B(transform0, ros::Time(), "frame_id_not_equal", "child_frame_id");
02475   EXPECT_FALSE(stamped_transform0B == stamped_transform_reference); // Different Frame id
02476   tf::StampedTransform stamped_transform0C(transform0, ros::Time(1.0), "frame_id", "child_frame_id");
02477   EXPECT_FALSE(stamped_transform0C == stamped_transform_reference); // Different Time
02478   tf::StampedTransform stamped_transform0D(transform0, ros::Time(1.0), "frame_id_not_equal", "child_frame_id");
02479   EXPECT_FALSE(stamped_transform0D == stamped_transform_reference); // Different frame id and time
02480   tf::StampedTransform stamped_transform0E(transform1, ros::Time(), "frame_id_not_equal", "child_frame_id");
02481   EXPECT_FALSE(stamped_transform0E == stamped_transform_reference); // Different transform, frame id
02482   tf::StampedTransform stamped_transform0F(transform1, ros::Time(1.0), "frame_id", "child_frame_id");
02483   EXPECT_FALSE(stamped_transform0F == stamped_transform_reference); // Different transform, time
02484   tf::StampedTransform stamped_transform0G(transform1, ros::Time(1.0), "frame_id_not_equal", "child_frame_id");
02485   EXPECT_FALSE(stamped_transform0G == stamped_transform_reference); // Different transform, frame id and time
02486   tf::StampedTransform stamped_transform0H(transform1, ros::Time(), "frame_id", "child_frame_id");
02487   EXPECT_FALSE(stamped_transform0H == stamped_transform_reference); // Different transform
02488 
02489 
02490   //Different child_frame_id
02491   tf::StampedTransform stamped_transform1A(transform0, ros::Time(), "frame_id", "child_frame_id2");
02492   EXPECT_FALSE(stamped_transform1A == stamped_transform_reference); // Equal
02493   tf::StampedTransform stamped_transform1B(transform0, ros::Time(), "frame_id_not_equal", "child_frame_id2");
02494   EXPECT_FALSE(stamped_transform1B == stamped_transform_reference); // Different Frame id
02495   tf::StampedTransform stamped_transform1C(transform0, ros::Time(1.0), "frame_id", "child_frame_id2");
02496   EXPECT_FALSE(stamped_transform1C == stamped_transform_reference); // Different Time
02497   tf::StampedTransform stamped_transform1D(transform0, ros::Time(1.0), "frame_id_not_equal", "child_frame_id2");
02498   EXPECT_FALSE(stamped_transform1D == stamped_transform_reference); // Different frame id and time
02499   tf::StampedTransform stamped_transform1E(transform1, ros::Time(), "frame_id_not_equal", "child_frame_id2");
02500   EXPECT_FALSE(stamped_transform1E == stamped_transform_reference); // Different transform, frame id
02501   tf::StampedTransform stamped_transform1F(transform1, ros::Time(1.0), "frame_id", "child_frame_id2");
02502   EXPECT_FALSE(stamped_transform1F == stamped_transform_reference); // Different transform, time
02503   tf::StampedTransform stamped_transform1G(transform1, ros::Time(1.0), "frame_id_not_equal", "child_frame_id2");
02504   EXPECT_FALSE(stamped_transform1G == stamped_transform_reference); // Different transform, frame id and time
02505   tf::StampedTransform stamped_transform1H(transform1, ros::Time(), "frame_id", "child_frame_id2");
02506   EXPECT_FALSE(stamped_transform1H == stamped_transform_reference); // Different transform
02507 
02508 }
02509 
02510 TEST(data, StampedOperatorEqual)
02511 {
02512  tf::Pose pose0, pose1, pose0a;
02513   pose0.setIdentity();
02514   pose1.setIdentity();
02515   pose1.setOrigin(tf::Vector3(1, 0, 0));
02516   tf::Stamped<tf::Pose> stamped_pose0(pose0, ros::Time(), "frame_id");
02517   tf::Stamped<tf::Pose> stamped_pose1(pose1, ros::Time(1.0), "frame_id_not_equal");
02518   EXPECT_FALSE(stamped_pose1 == stamped_pose0);
02519   stamped_pose1 = stamped_pose0;
02520   EXPECT_TRUE(stamped_pose1 == stamped_pose0);
02521 
02522 }
02523 
02524 int main(int argc, char **argv){
02525   testing::InitGoogleTest(&argc, argv);
02526   ros::Time::init(); //needed for ros::TIme::now()
02527   ros::init(argc, argv, "tf_unittest");
02528   return RUN_ALL_TESTS();
02529 }


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