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


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Thu Jun 6 2019 18:45:32