00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00033 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
00034 #include <tf2_ros/transform_listener.h>
00035 #include <ros/ros.h>
00036 #include <gtest/gtest.h>
00037 #include <tf2_ros/buffer.h>
00038 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
00039
00040 tf2_ros::Buffer* tf_buffer;
00041 geometry_msgs::TransformStamped t;
00042 static const double EPS = 1e-3;
00043
00044
00045 TEST(TfGeometry, Frame)
00046 {
00047 geometry_msgs::PoseStamped v1;
00048 v1.pose.position.x = 1;
00049 v1.pose.position.y = 2;
00050 v1.pose.position.z = 3;
00051 v1.pose.orientation.x = 1;
00052 v1.header.stamp = ros::Time(2);
00053 v1.header.frame_id = "A";
00054
00055
00056 geometry_msgs::PoseStamped v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0));
00057 EXPECT_NEAR(v_simple.pose.position.x, -9, EPS);
00058 EXPECT_NEAR(v_simple.pose.position.y, 18, EPS);
00059 EXPECT_NEAR(v_simple.pose.position.z, 27, EPS);
00060 EXPECT_NEAR(v_simple.pose.orientation.x, 0.0, EPS);
00061 EXPECT_NEAR(v_simple.pose.orientation.y, 0.0, EPS);
00062 EXPECT_NEAR(v_simple.pose.orientation.z, 0.0, EPS);
00063 EXPECT_NEAR(v_simple.pose.orientation.w, 1.0, EPS);
00064
00065
00066 geometry_msgs::PoseStamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0),
00067 "A", ros::Duration(3.0));
00068 EXPECT_NEAR(v_advanced.pose.position.x, -9, EPS);
00069 EXPECT_NEAR(v_advanced.pose.position.y, 18, EPS);
00070 EXPECT_NEAR(v_advanced.pose.position.z, 27, EPS);
00071 EXPECT_NEAR(v_advanced.pose.orientation.x, 0.0, EPS);
00072 EXPECT_NEAR(v_advanced.pose.orientation.y, 0.0, EPS);
00073 EXPECT_NEAR(v_advanced.pose.orientation.z, 0.0, EPS);
00074 EXPECT_NEAR(v_advanced.pose.orientation.w, 1.0, EPS);
00075 }
00076
00077 TEST(TfGeometry, PoseWithCovarianceStamped)
00078 {
00079 geometry_msgs::PoseWithCovarianceStamped v1;
00080 v1.pose.pose.position.x = 1;
00081 v1.pose.pose.position.y = 2;
00082 v1.pose.pose.position.z = 3;
00083 v1.pose.pose.orientation.x = 1;
00084 v1.header.stamp = ros::Time(2);
00085 v1.header.frame_id = "A";
00086 v1.pose.covariance[0] = 1;
00087 v1.pose.covariance[7] = 1;
00088 v1.pose.covariance[14] = 1;
00089 v1.pose.covariance[21] = 1;
00090 v1.pose.covariance[28] = 1;
00091 v1.pose.covariance[35] = 1;
00092
00093
00094 const geometry_msgs::PoseWithCovarianceStamped v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0));
00095 EXPECT_NEAR(v_simple.pose.pose.position.x, -9, EPS);
00096 EXPECT_NEAR(v_simple.pose.pose.position.y, 18, EPS);
00097 EXPECT_NEAR(v_simple.pose.pose.position.z, 27, EPS);
00098 EXPECT_NEAR(v_simple.pose.pose.orientation.x, 0.0, EPS);
00099 EXPECT_NEAR(v_simple.pose.pose.orientation.y, 0.0, EPS);
00100 EXPECT_NEAR(v_simple.pose.pose.orientation.z, 0.0, EPS);
00101 EXPECT_NEAR(v_simple.pose.pose.orientation.w, 1.0, EPS);
00102
00103
00104 EXPECT_NEAR(v_simple.pose.covariance[0], 1.0, EPS);
00105 EXPECT_NEAR(v_simple.pose.covariance[7], 1.0, EPS);
00106 EXPECT_NEAR(v_simple.pose.covariance[14], 1.0, EPS);
00107 EXPECT_NEAR(v_simple.pose.covariance[21], 1.0, EPS);
00108 EXPECT_NEAR(v_simple.pose.covariance[28], 1.0, EPS);
00109 EXPECT_NEAR(v_simple.pose.covariance[35], 1.0, EPS);
00110
00111
00112 const geometry_msgs::PoseWithCovarianceStamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0),
00113 "A", ros::Duration(3.0));
00114 EXPECT_NEAR(v_advanced.pose.pose.position.x, -9, EPS);
00115 EXPECT_NEAR(v_advanced.pose.pose.position.y, 18, EPS);
00116 EXPECT_NEAR(v_advanced.pose.pose.position.z, 27, EPS);
00117 EXPECT_NEAR(v_advanced.pose.pose.orientation.x, 0.0, EPS);
00118 EXPECT_NEAR(v_advanced.pose.pose.orientation.y, 0.0, EPS);
00119 EXPECT_NEAR(v_advanced.pose.pose.orientation.z, 0.0, EPS);
00120 EXPECT_NEAR(v_advanced.pose.pose.orientation.w, 1.0, EPS);
00121
00122
00123 EXPECT_NEAR(v_advanced.pose.covariance[0], 1.0, EPS);
00124 EXPECT_NEAR(v_advanced.pose.covariance[7], 1.0, EPS);
00125 EXPECT_NEAR(v_advanced.pose.covariance[14], 1.0, EPS);
00126 EXPECT_NEAR(v_advanced.pose.covariance[21], 1.0, EPS);
00127 EXPECT_NEAR(v_advanced.pose.covariance[28], 1.0, EPS);
00128 EXPECT_NEAR(v_advanced.pose.covariance[35], 1.0, EPS);
00129
00132
00133 geometry_msgs::TransformStamped t_rot;
00134 t_rot.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(1,0,0), M_PI/2));
00135 t_rot.header.stamp = ros::Time(2.0);
00136 t_rot.header.frame_id = "A";
00137 t_rot.child_frame_id = "rotated";
00138 tf_buffer->setTransform(t_rot, "rotation_test");
00139
00140
00141 v1.pose.covariance[1] = 1;
00142 v1.pose.covariance[6] = 1;
00143 v1.pose.covariance[12] = 1;
00144
00145
00146 const geometry_msgs::PoseWithCovarianceStamped v_rotated = tf_buffer->transform(v1, "rotated", ros::Duration(2.0));
00147
00148
00149 EXPECT_NEAR(v_rotated.pose.covariance[0], 1.0, EPS);
00150 EXPECT_NEAR(v_rotated.pose.covariance[1], 0.0, EPS);
00151 EXPECT_NEAR(v_rotated.pose.covariance[2],-1.0, EPS);
00152 EXPECT_NEAR(v_rotated.pose.covariance[6], 1.0, EPS);
00153 EXPECT_NEAR(v_rotated.pose.covariance[7], 1.0, EPS);
00154 EXPECT_NEAR(v_rotated.pose.covariance[8], 0.0, EPS);
00155 EXPECT_NEAR(v_rotated.pose.covariance[12],-1.0, EPS);
00156 EXPECT_NEAR(v_rotated.pose.covariance[13], 0.0, EPS);
00157 EXPECT_NEAR(v_rotated.pose.covariance[14], 1.0, EPS);
00158
00159
00160 tf_buffer->setTransform(t, "test");
00161 }
00162
00163 TEST(TfGeometry, Transform)
00164 {
00165 geometry_msgs::TransformStamped v1;
00166 v1.transform.translation.x = 1;
00167 v1.transform.translation.y = 2;
00168 v1.transform.translation.z = 3;
00169 v1.transform.rotation.x = 1;
00170 v1.header.stamp = ros::Time(2);
00171 v1.header.frame_id = "A";
00172
00173
00174 geometry_msgs::TransformStamped v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0));
00175 EXPECT_NEAR(v_simple.transform.translation.x, -9, EPS);
00176 EXPECT_NEAR(v_simple.transform.translation.y, 18, EPS);
00177 EXPECT_NEAR(v_simple.transform.translation.z, 27, EPS);
00178 EXPECT_NEAR(v_simple.transform.rotation.x, 0.0, EPS);
00179 EXPECT_NEAR(v_simple.transform.rotation.y, 0.0, EPS);
00180 EXPECT_NEAR(v_simple.transform.rotation.z, 0.0, EPS);
00181 EXPECT_NEAR(v_simple.transform.rotation.w, 1.0, EPS);
00182
00183
00184
00185 geometry_msgs::TransformStamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0),
00186 "A", ros::Duration(3.0));
00187 EXPECT_NEAR(v_advanced.transform.translation.x, -9, EPS);
00188 EXPECT_NEAR(v_advanced.transform.translation.y, 18, EPS);
00189 EXPECT_NEAR(v_advanced.transform.translation.z, 27, EPS);
00190 EXPECT_NEAR(v_advanced.transform.rotation.x, 0.0, EPS);
00191 EXPECT_NEAR(v_advanced.transform.rotation.y, 0.0, EPS);
00192 EXPECT_NEAR(v_advanced.transform.rotation.z, 0.0, EPS);
00193 EXPECT_NEAR(v_advanced.transform.rotation.w, 1.0, EPS);
00194 }
00195
00196 TEST(TfGeometry, Vector)
00197 {
00198 geometry_msgs::Vector3Stamped v1, res;
00199 v1.vector.x = 1;
00200 v1.vector.y = 2;
00201 v1.vector.z = 3;
00202 v1.header.stamp = ros::Time(2.0);
00203 v1.header.frame_id = "A";
00204
00205
00206 geometry_msgs::Vector3Stamped v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0));
00207 EXPECT_NEAR(v_simple.vector.x, 1, EPS);
00208 EXPECT_NEAR(v_simple.vector.y, -2, EPS);
00209 EXPECT_NEAR(v_simple.vector.z, -3, EPS);
00210
00211
00212 geometry_msgs::Vector3Stamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0),
00213 "A", ros::Duration(3.0));
00214 EXPECT_NEAR(v_advanced.vector.x, 1, EPS);
00215 EXPECT_NEAR(v_advanced.vector.y, -2, EPS);
00216 EXPECT_NEAR(v_advanced.vector.z, -3, EPS);
00217 }
00218
00219
00220 TEST(TfGeometry, Point)
00221 {
00222 geometry_msgs::PointStamped v1, res;
00223 v1.point.x = 1;
00224 v1.point.y = 2;
00225 v1.point.z = 3;
00226 v1.header.stamp = ros::Time(2.0);
00227 v1.header.frame_id = "A";
00228
00229
00230 geometry_msgs::PointStamped v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0));
00231 EXPECT_NEAR(v_simple.point.x, -9, EPS);
00232 EXPECT_NEAR(v_simple.point.y, 18, EPS);
00233 EXPECT_NEAR(v_simple.point.z, 27, EPS);
00234
00235
00236 geometry_msgs::PointStamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0),
00237 "A", ros::Duration(3.0));
00238 EXPECT_NEAR(v_advanced.point.x, -9, EPS);
00239 EXPECT_NEAR(v_advanced.point.y, 18, EPS);
00240 EXPECT_NEAR(v_advanced.point.z, 27, EPS);
00241 }
00242
00243 TEST(TfGeometry, doTransformPoint)
00244 {
00245 geometry_msgs::Point v1, res;
00246 v1.x = 2;
00247 v1.y = 1;
00248 v1.z = 3;
00249
00250 geometry_msgs::TransformStamped trafo;
00251 trafo.transform.translation.x = -1;
00252 trafo.transform.translation.y = 2;
00253 trafo.transform.translation.z = -3;
00254 trafo.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0,0,1), -M_PI / 2.0));
00255
00256 tf2::doTransform(v1, res, trafo);
00257
00258 EXPECT_NEAR(res.x, 0, EPS);
00259 EXPECT_NEAR(res.y, 0, EPS);
00260 EXPECT_NEAR(res.z, 0, EPS);
00261 }
00262
00263 TEST(TfGeometry, doTransformQuaterion)
00264 {
00265 geometry_msgs::Quaternion v1, res;
00266 v1.w = 1;
00267
00268 geometry_msgs::TransformStamped trafo;
00269 trafo.transform.translation.x = -1;
00270 trafo.transform.translation.y = 2;
00271 trafo.transform.translation.z = -3;
00272 trafo.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0,0,1), -M_PI / 2.0));
00273
00274 tf2::doTransform(v1, res, trafo);
00275
00276 EXPECT_NEAR(res.x, trafo.transform.rotation.x, EPS);
00277 EXPECT_NEAR(res.y, trafo.transform.rotation.y, EPS);
00278 EXPECT_NEAR(res.z, trafo.transform.rotation.z, EPS);
00279 EXPECT_NEAR(res.w, trafo.transform.rotation.w, EPS);
00280 }
00281
00282 TEST(TfGeometry, doTransformPose)
00283 {
00284 geometry_msgs::Pose v1, res;
00285 v1.position.x = 2;
00286 v1.position.y = 1;
00287 v1.position.z = 3;
00288 v1.orientation.w = 1;
00289
00290 geometry_msgs::TransformStamped trafo;
00291 trafo.transform.translation.x = -1;
00292 trafo.transform.translation.y = 2;
00293 trafo.transform.translation.z = -3;
00294 trafo.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0,0,1), -M_PI / 2.0));
00295
00296 tf2::doTransform(v1, res, trafo);
00297
00298 EXPECT_NEAR(res.position.x, 0, EPS);
00299 EXPECT_NEAR(res.position.y, 0, EPS);
00300 EXPECT_NEAR(res.position.z, 0, EPS);
00301
00302 EXPECT_NEAR(res.orientation.x, trafo.transform.rotation.x, EPS);
00303 EXPECT_NEAR(res.orientation.y, trafo.transform.rotation.y, EPS);
00304 EXPECT_NEAR(res.orientation.z, trafo.transform.rotation.z, EPS);
00305 EXPECT_NEAR(res.orientation.w, trafo.transform.rotation.w, EPS);
00306 }
00307
00308 TEST(TfGeometry, doTransformVector3)
00309 {
00310 geometry_msgs::Vector3 v1, res;
00311 v1.x = 2;
00312 v1.y = 1;
00313 v1.z = 3;
00314
00315 geometry_msgs::TransformStamped trafo;
00316 trafo.transform.translation.x = -1;
00317 trafo.transform.translation.y = 2;
00318 trafo.transform.translation.z = -3;
00319 trafo.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0,0,1), -M_PI / 2.0));
00320
00321 tf2::doTransform(v1, res, trafo);
00322
00323 EXPECT_NEAR(res.x, 1, EPS);
00324 EXPECT_NEAR(res.y, -2, EPS);
00325 EXPECT_NEAR(res.z, 3, EPS);
00326 }
00327
00328 TEST(TfGeometry, doTransformWrench)
00329 {
00330 geometry_msgs::Wrench v1, res;
00331 v1.force.x = 2;
00332 v1.force.y = 1;
00333 v1.force.z = 3;
00334 v1.torque.x = 2;
00335 v1.torque.y = 1;
00336 v1.torque.z = 3;
00337
00338 geometry_msgs::TransformStamped trafo;
00339 trafo.transform.translation.x = -1;
00340 trafo.transform.translation.y = 2;
00341 trafo.transform.translation.z = -3;
00342 trafo.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0,0,1), -M_PI / 2.0));
00343
00344 tf2::doTransform(v1, res, trafo);
00345 EXPECT_NEAR(res.force.x, 1, EPS);
00346 EXPECT_NEAR(res.force.y, -2, EPS);
00347 EXPECT_NEAR(res.force.z, 3, EPS);
00348
00349 EXPECT_NEAR(res.torque.x, 1, EPS);
00350 EXPECT_NEAR(res.torque.y, -2, EPS);
00351 EXPECT_NEAR(res.torque.z, 3, EPS);
00352 }
00353
00354 int main(int argc, char **argv){
00355 testing::InitGoogleTest(&argc, argv);
00356 ros::init(argc, argv, "test");
00357 ros::NodeHandle n;
00358
00359 tf_buffer = new tf2_ros::Buffer();
00360 tf_buffer->setUsingDedicatedThread(true);
00361
00362
00363 t.transform.translation.x = 10;
00364 t.transform.translation.y = 20;
00365 t.transform.translation.z = 30;
00366 t.transform.rotation.x = 1;
00367 t.header.stamp = ros::Time(2.0);
00368 t.header.frame_id = "A";
00369 t.child_frame_id = "B";
00370 tf_buffer->setTransform(t, "test");
00371
00372 int ret = RUN_ALL_TESTS();
00373 delete tf_buffer;
00374 return ret;
00375 }
00376
00377
00378
00379
00380