00001 #include <gtest/gtest.h>
00002 #include <tf/transform_listener.h>
00003 #include <sys/time.h>
00004
00005 #include "tf/LinearMath/Vector3.h"
00006
00007 using namespace tf;
00008
00009
00010
00011 class TransformTwistLinearTest : public ::testing::Test {
00012 protected:
00013
00014
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(tf::Transform(tf::createIdentityQuaternion(), tf::Vector3(x, y, z)), ros::Time(t), "foo", "bar"));
00029 tf_.setTransform(StampedTransform(tf::Transform(tf::createIdentityQuaternion(), tf::Vector3(1,0,0)), ros::Time(t), "foo", "stationary_offset_child"));
00030 tf_.setTransform(StampedTransform(tf::Transform(tf::createIdentityQuaternion(), tf::Vector3(0,0,1)), ros::Time(t), "stationary_offset_parent", "foo"));
00031 }
00032
00033
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
00047 }
00048
00049
00050
00051
00052 virtual void SetUp() {
00053
00054
00055 }
00056
00057 virtual void TearDown() {
00058
00059
00060 }
00061
00062
00063
00064 tf::TransformListener tf_;
00065
00066 geometry_msgs::TwistStamped tw_;
00067
00068 };
00069
00070
00071 class TransformTwistAngularTest : public ::testing::Test {
00072 protected:
00073
00074
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(tf::Transform(tf::createQuaternionFromRPY(x, y, z), tf::Vector3(0,0,0)), ros::Time(t), "foo", "bar"));
00089 tf_.setTransform(StampedTransform(tf::Transform(tf::createIdentityQuaternion(), tf::Vector3(1,0,0)), ros::Time(t), "foo", "stationary_offset_child"));
00090 tf_.setTransform(StampedTransform(tf::Transform(tf::createIdentityQuaternion(), tf::Vector3(0,0,-1)), ros::Time(t), "stationary_offset_parent", "foo"));
00091 }
00092
00093
00094
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
00110 }
00111
00112
00113
00114
00115 virtual void SetUp() {
00116
00117
00118 }
00119
00120 virtual void TearDown() {
00121
00122
00123 }
00124
00125
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
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
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
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
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
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
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
00289
00290
00291
00292
00293
00294
00295
00296
00297
00298
00299
00300
00301
00302
00303
00304
00305
00306
00307
00308
00309
00310
00311
00312
00313
00314
00315
00316
00317
00318
00319
00320
00321
00322
00323
00324
00325
00326
00327
00328
00329
00330
00331
00332
00333
00334
00335
00336
00337
00338
00339
00340
00341
00342
00343
00344
00345
00346
00347
00348
00349
00350
00351
00352
00353
00354
00355
00356
00357
00358
00359
00360
00361
00362
00363
00364
00365
00366
00367
00368
00369
00370
00371
00372
00373
00374
00375
00376
00377
00378
00379
00380
00381
00382
00383
00384
00385
00386
00387
00388
00389
00390
00391
00392
00393
00394
00395
00396
00397
00398
00399
00400
00401
00402
00403
00404
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