tf: tf: transform_twist_test.cpp Source File

00001 #include <gtest/gtest.h>
00002 #include <tf/transform_listener.h>
00003 #include <sys/time.h>
00004 
00005 #include "LinearMath/btVector3.h"
00006 
00007 using namespace tf;
00008 
00009 
00010 // The fixture for testing class Foo.
00011 class TransformTwistLinearTest : public ::testing::Test {
00012 protected:
00013   // You can remove any or all of the following functions if its body
00014   // is empty.
00015 
00016   TransformTwistLinearTest() {
00017     double x = -.1;
00018     double y = 0;
00019     double z = 0;
00020     for (double t = 0.1; t <= 6; t += 0.1)
00021     {
00022       if      (t < 1) x += 1;
00023       else if (t < 2) y += 1;
00024       else if (t < 3) x -= 1;
00025       else if (t < 4) y -= 1;
00026       else if (t < 5) z += 1;
00027       else            z -= 1;
00028       tf_.setTransform(StampedTransform(btTransform(tf::createIdentityQuaternion(), btVector3(x, y, z)), ros::Time(t), "foo", "bar"));
00029       tf_.setTransform(StampedTransform(btTransform(tf::createIdentityQuaternion(), btVector3(1,0,0)), ros::Time(t), "foo", "stationary_offset_child"));
00030       tf_.setTransform(StampedTransform(btTransform(tf::createIdentityQuaternion(), btVector3(0,0,1)), ros::Time(t), "stationary_offset_parent", "foo"));
00031     }
00032 
00033     // You can do set-up work for each test here.
00034     tw_.header.stamp = ros::Time();
00035     tw_.header.frame_id = "bar";
00036     tw_.twist.linear.x =  1;
00037     tw_.twist.linear.y =  2;
00038     tw_.twist.linear.z =  3;
00039     tw_.twist.angular.x =  0;
00040     tw_.twist.angular.y =  0;
00041     tw_.twist.angular.z =  0;
00042   
00043   }
00044 
00045   virtual ~TransformTwistLinearTest() {
00046     // You can do clean-up work that doesn't throw exceptions here.
00047   }
00048 
00049   // If the constructor and destructor are not enough for setting up
00050   // and cleaning up each test, you can define the following methods:
00051 
00052   virtual void SetUp() {
00053     // Code here will be called immediately after the constructor (right
00054     // before each test).
00055   }
00056 
00057   virtual void TearDown() {
00058     // Code here will be called immediately after each test (right
00059     // before the destructor).
00060   }
00061 
00062   // Objects declared here can be used by all tests in the test case for LinearVelocity.
00063 
00064   tf::TransformListener tf_;
00065 
00066   geometry_msgs::TwistStamped tw_;
00067 
00068 };
00069 
00070 // The fixture for testing class Foo.
00071 class TransformTwistAngularTest : public ::testing::Test {
00072 protected:
00073   // You can remove any or all of the following functions if its body
00074   // is empty.
00075 
00076   TransformTwistAngularTest() {
00077     double x = -.1;
00078     double y = 0;
00079     double z = 0;
00080     for (double t = 0.1; t < 6; t += 0.1)
00081     {
00082       if      (t < 1) x += .1;
00083       else if (t < 2) x -= .1;
00084       else if (t < 3) y += .1;
00085       else if (t < 4) y -= .1;
00086       else if (t < 5) z += .1;
00087       else            z -= .1;
00088       tf_.setTransform(StampedTransform(btTransform(tf::createQuaternionFromRPY(x, y, z), btVector3(0,0,0)), ros::Time(t), "foo", "bar"));
00089       tf_.setTransform(StampedTransform(btTransform(tf::createIdentityQuaternion(), btVector3(1,0,0)), ros::Time(t), "foo", "stationary_offset_child"));
00090       tf_.setTransform(StampedTransform(btTransform(tf::createIdentityQuaternion(), btVector3(0,0,-1)), ros::Time(t), "stationary_offset_parent", "foo"));
00091     }
00092 
00093     // You can do set-up work for each test here.
00094     // You can do set-up work for each test here.
00095     tw_x.header.stamp = ros::Time();
00096     tw_x.header.frame_id = "bar";
00097     tw_x.twist.angular.x =  1;
00098 
00099     tw_y.header.stamp = ros::Time();
00100     tw_y.header.frame_id = "bar";
00101     tw_y.twist.angular.y =  1;
00102 
00103     tw_z.header.stamp = ros::Time();
00104     tw_z.header.frame_id = "bar";
00105     tw_z.twist.angular.z =  1;
00106   }
00107 
00108   virtual ~TransformTwistAngularTest() {
00109     // You can do clean-up work that doesn't throw exceptions here.
00110   }
00111 
00112   // If the constructor and destructor are not enough for setting up
00113   // and cleaning up each test, you can define the following methods:
00114 
00115   virtual void SetUp() {
00116     // Code here will be called immediately after the constructor (right
00117     // before each test).
00118   }
00119 
00120   virtual void TearDown() {
00121     // Code here will be called immediately after each test (right
00122     // before the destructor).
00123   }
00124 
00125   // Objects declared here can be used by all tests in the test case for AngularVelocity.
00126 
00127   tf::TransformListener tf_;
00128   geometry_msgs::TwistStamped tw_x, tw_y, tw_z;
00129 };
00130 
00131 TEST_F(TransformTwistLinearTest, LinearVelocityToThreeFrames)
00132 {
00133   geometry_msgs::TwistStamped tw;
00134   geometry_msgs::TwistStamped tw_in =tw_;
00135 
00136   std::vector<std::string> offset_frames;
00137 
00138   offset_frames.push_back("foo");
00139   offset_frames.push_back("stationary_offset_child");
00140   offset_frames.push_back("stationary_offset_parent");
00141 
00142 
00143   for (std::vector<std::string>::iterator it = offset_frames.begin(); it != offset_frames.end(); ++it)
00144   {
00145     try
00146     {
00147       tw_in.header.stamp = ros::Time(0.5);
00148       tf_.transformTwist(*it, tw_in, tw);
00149       EXPECT_FLOAT_EQ(11.0, tw.twist.linear.x);
00150       EXPECT_FLOAT_EQ(2.0 , tw.twist.linear.y);
00151       EXPECT_FLOAT_EQ(3.0, tw.twist.linear.z);
00152       EXPECT_FLOAT_EQ(0.0, tw.twist.angular.x);
00153       EXPECT_FLOAT_EQ(0.0, tw.twist.angular.y);
00154       EXPECT_FLOAT_EQ(0.0, tw.twist.angular.z);
00155   
00156       tw_in.header.stamp = ros::Time(1.5);
00157       tf_.transformTwist(*it, tw_in, tw);
00158       EXPECT_FLOAT_EQ(1.0, tw.twist.linear.x);
00159       EXPECT_FLOAT_EQ(12.0, tw.twist.linear.y);
00160       EXPECT_FLOAT_EQ(3.0, tw.twist.linear.z);
00161       EXPECT_FLOAT_EQ(0.0, tw.twist.angular.x);
00162       EXPECT_FLOAT_EQ(0.0, tw.twist.angular.y);
00163       EXPECT_FLOAT_EQ(0.0, tw.twist.angular.z);
00164 
00165       tw_in.header.stamp = ros::Time(2.5);
00166       tf_.transformTwist(*it, tw_in, tw);
00167       EXPECT_FLOAT_EQ(-9.0, tw.twist.linear.x);
00168       EXPECT_FLOAT_EQ(2.0, tw.twist.linear.y);
00169       EXPECT_FLOAT_EQ(3.0, tw.twist.linear.z);
00170       EXPECT_FLOAT_EQ(tw.twist.angular.x, 0.0);
00171       EXPECT_FLOAT_EQ(tw.twist.angular.y, 0.0);
00172       EXPECT_FLOAT_EQ(tw.twist.angular.z, 0.0);
00173 
00174       tw_in.header.stamp = ros::Time(3.5);
00175       tf_.transformTwist(*it, tw_in, tw);
00176       EXPECT_FLOAT_EQ(tw.twist.linear.x, 1.0);
00177       EXPECT_FLOAT_EQ(tw.twist.linear.y, -8.0);
00178       EXPECT_FLOAT_EQ(tw.twist.linear.z, 3.0);
00179       EXPECT_FLOAT_EQ(tw.twist.angular.x, 0.0);
00180       EXPECT_FLOAT_EQ(tw.twist.angular.y, 0.0);
00181       EXPECT_FLOAT_EQ(tw.twist.angular.z, 0.0);
00182 
00183       tw_in.header.stamp = ros::Time(4.5);
00184       tf_.transformTwist(*it, tw_in, tw);
00185       EXPECT_FLOAT_EQ(tw.twist.linear.x, 1.0);
00186       EXPECT_FLOAT_EQ(tw.twist.linear.y, 2.0);
00187       EXPECT_FLOAT_EQ(tw.twist.linear.z, 13.0);
00188       EXPECT_FLOAT_EQ(tw.twist.angular.x, 0.0);
00189       EXPECT_FLOAT_EQ(tw.twist.angular.y, 0.0);
00190       EXPECT_FLOAT_EQ(tw.twist.angular.z, 0.0);
00191 
00192       tw_in.header.stamp = ros::Time(5.5);
00193       tf_.transformTwist(*it, tw_in, tw);
00194       EXPECT_FLOAT_EQ(tw.twist.linear.x, 1.0);
00195       EXPECT_FLOAT_EQ(tw.twist.linear.y, 2.0);
00196       EXPECT_FLOAT_EQ(tw.twist.linear.z, -7.0);
00197       EXPECT_FLOAT_EQ(tw.twist.angular.x, 0.0);
00198       EXPECT_FLOAT_EQ(tw.twist.angular.y, 0.0);
00199       EXPECT_FLOAT_EQ(tw.twist.angular.z, 0.0);
00200     }
00201     catch(tf::TransformException &ex)
00202     {
00203       EXPECT_STREQ("", ex.what());
00204     }
00205   }
00206 };
00207 
00208 TEST_F(TransformTwistAngularTest, AngularVelocityAlone)
00209 {
00210   double epsilon = 1e-14;
00211   geometry_msgs::TwistStamped tw;
00212   geometry_msgs::TwistStamped tw_in;
00213 
00214   try
00215   {
00216     //tf_.lookupVelocity("foo", "bar", ros::Time(0.5), ros::Duration(0.1), tw);
00217     tw_in =tw_x;
00218     tw_in.header.stamp = ros::Time(0.5);
00219     tf_.transformTwist("foo", tw_in, tw);
00220     EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon);
00221     EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon);
00222     EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon);
00223     EXPECT_NEAR(tw.twist.angular.x, 2.0, epsilon);
00224     EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon);
00225     EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon);
00226   
00227     //tf_.lookupVelocity("foo", "bar", ros::Time(1.5), ros::Duration(0.1), tw);
00228     tw_in =tw_x;
00229     tw_in.header.stamp = ros::Time(1.5);
00230     tf_.transformTwist("foo", tw_in, tw);
00231     EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon);
00232     EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon);
00233     EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon);
00234     EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon);
00235     EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon);
00236     EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon);
00237 
00238     //    tf_.lookupVelocity("foo", "bar", ros::Time(2.5), ros::Duration(0.1), tw);
00239     tw_in =tw_y;
00240     tw_in.header.stamp = ros::Time(2.5);
00241     tf_.transformTwist("foo", tw_in, tw);
00242     EXPECT_NEAR(tw.twist.linear.x,  0.0, epsilon);
00243     EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon);
00244     EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon);
00245     EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon);
00246     EXPECT_NEAR(tw.twist.angular.y, 2.0, epsilon);
00247     EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon);
00248 
00249     //tf_.lookupVelocity("foo", "bar", ros::Time(3.5), ros::Duration(0.1), tw);
00250     tw_in =tw_y;
00251     tw_in.header.stamp = ros::Time(3.5);
00252     tf_.transformTwist("foo", tw_in, tw);
00253     EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon);
00254     EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon);
00255     EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon);
00256     EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon);
00257     EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon);
00258     EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon);
00259 
00260     //tf_.lookupVelocity("foo", "bar", ros::Time(4.5), ros::Duration(0.1), tw);
00261     tw_in =tw_z;
00262     tw_in.header.stamp = ros::Time(4.5);
00263     tf_.transformTwist("foo", tw_in, tw);
00264     EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon);
00265     EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon);
00266     EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon);
00267     EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon);
00268     EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon);
00269     EXPECT_NEAR(tw.twist.angular.z, 2.0, epsilon);
00270 
00271     //    tf_.lookupVelocity("foo", "bar", ros::Time(5.5), ros::Duration(0.1), tw);
00272     tw_in =tw_z;
00273     tw_in.header.stamp = ros::Time(5.5);
00274     tf_.transformTwist("foo", tw_in, tw);
00275     EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon);
00276     EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon);
00277     EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon);
00278     EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon);
00279     EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon);
00280     EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon);
00281   }
00282   catch(tf::TransformException &ex)
00283   {
00284     EXPECT_STREQ("", ex.what());
00285   }
00286 };
00287 /*
00288 TEST_F(TransformTwistAngularTest, AngularVelocityOffsetChildFrameInX)
00289 {
00290   double epsilon = 1e-14;
00291   geometry_msgs::TwistStamped tw;
00292   try
00293   {
00294     tf_.lookupVelocity("stationary_offset_child", "bar", ros::Time(0.5), ros::Duration(0.1), tw);
00295     EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon);
00296     EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon);
00297     EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon);
00298     EXPECT_NEAR(tw.twist.angular.x, 1.0, epsilon);
00299     EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon);
00300     EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon);
00301   
00302     tf_.lookupVelocity("stationary_offset_child", "bar", ros::Time(1.5), ros::Duration(0.1), tw);
00303     EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon);
00304     EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon);
00305     EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon);
00306     EXPECT_NEAR(tw.twist.angular.x, -1.0, epsilon);
00307     EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon);
00308     EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon);
00309 
00310     tf_.lookupVelocity("stationary_offset_child", "bar", ros::Time(2.5), ros::Duration(0.1), tw);
00311     EXPECT_NEAR(tw.twist.linear.x,  0.0, epsilon);
00312     EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon);
00313     EXPECT_NEAR(tw.twist.linear.z, -1.0, epsilon);
00314     EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon);
00315     EXPECT_NEAR(tw.twist.angular.y, 1.0, epsilon);
00316     EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon);
00317 
00318     tf_.lookupVelocity("stationary_offset_child", "bar", ros::Time(3.5), ros::Duration(0.1), tw);
00319     EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon);
00320     EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon);
00321     EXPECT_NEAR(tw.twist.linear.z, 1.0, epsilon);
00322     EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon);
00323     EXPECT_NEAR(tw.twist.angular.y, -1.0, epsilon);
00324     EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon);
00325 
00326     tf_.lookupVelocity("stationary_offset_child", "bar", ros::Time(4.5), ros::Duration(0.1), tw);
00327     EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon);
00328     EXPECT_NEAR(tw.twist.linear.y, 1.0, epsilon);
00329     EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon);
00330     EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon);
00331     EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon);
00332     EXPECT_NEAR(tw.twist.angular.z, 1.0, epsilon);
00333 
00334     tf_.lookupVelocity("stationary_offset_child", "bar", ros::Time(5.5), ros::Duration(0.1), tw);
00335     EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon);
00336     EXPECT_NEAR(tw.twist.linear.y, -1.0, epsilon);
00337     EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon);
00338     EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon);
00339     EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon);
00340     EXPECT_NEAR(tw.twist.angular.z, -1.0, epsilon);
00341   }
00342   catch(tf::TransformException &ex)
00343   {
00344     EXPECT_STREQ("", ex.what());
00345   }
00346 };
00347 
00348 TEST_F(TransformTwistAngularTest, AngularVelocityOffsetParentFrameInZ)
00349 {
00350   double epsilon = 1e-14;
00351   geometry_msgs::TwistStamped tw;
00352   try
00353   {
00354     tf_.lookupVelocity("stationary_offset_parent", "bar", ros::Time(0.5), ros::Duration(0.1), tw);
00355     EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon);
00356     EXPECT_NEAR(tw.twist.linear.y, -1.0, epsilon);
00357     EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon);
00358     EXPECT_NEAR(tw.twist.angular.x, 1.0, epsilon);
00359     EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon);
00360     EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon);
00361   
00362     tf_.lookupVelocity("stationary_offset_parent", "bar", ros::Time(1.5), ros::Duration(0.1), tw);
00363     EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon);
00364     EXPECT_NEAR(tw.twist.linear.y, 1.0, epsilon);
00365     EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon);
00366     EXPECT_NEAR(tw.twist.angular.x, -1.0, epsilon);
00367     EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon);
00368     EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon);
00369 
00370     tf_.lookupVelocity("stationary_offset_parent", "bar", ros::Time(2.5), ros::Duration(0.1), tw);
00371     EXPECT_NEAR(tw.twist.linear.x, 1.0, epsilon);
00372     EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon);
00373     EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon);
00374     EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon);
00375     EXPECT_NEAR(tw.twist.angular.y, 1.0, epsilon);
00376     EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon);
00377 
00378     tf_.lookupVelocity("stationary_offset_parent", "bar", ros::Time(3.5), ros::Duration(0.1), tw);
00379     EXPECT_NEAR(tw.twist.linear.x, -1.0, epsilon);
00380     EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon);
00381     EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon);
00382     EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon);
00383     EXPECT_NEAR(tw.twist.angular.y, -1.0, epsilon);
00384     EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon);
00385 
00386     tf_.lookupVelocity("stationary_offset_parent", "bar", ros::Time(4.5), ros::Duration(0.1), tw);
00387     EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon);
00388     EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon);
00389     EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon);
00390     EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon);
00391     EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon);
00392     EXPECT_NEAR(tw.twist.angular.z, 1.0, epsilon);
00393 
00394     tf_.lookupVelocity("stationary_offset_parent", "bar", ros::Time(5.5), ros::Duration(0.1), tw);
00395     EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon);
00396     EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon);
00397     EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon);
00398     EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon);
00399     EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon);
00400     EXPECT_NEAR(tw.twist.angular.z, -1.0, epsilon);
00401   }
00402   catch(tf::TransformException &ex)
00403   {
00404     EXPECT_STREQ("", ex.what());
00405   }
00406 };
00407 */
00408 
00409 int main(int argc, char **argv){
00410   testing::InitGoogleTest(&argc, argv);
00411   ros::init(argc, argv, "testing_transform_twist");
00412   return RUN_ALL_TESTS();
00413 }
00414