tf: tf: velocity_test.cpp Source File

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