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 <gtest/gtest.h>
00035
00036 static const double EPS = 1e-6;
00037
00038 tf2::Vector3 get_tf2_vector()
00039 {
00040 return tf2::Vector3(1.0, 2.0, 3.0);
00041 }
00042
00043 geometry_msgs::Vector3& value_initialize(geometry_msgs::Vector3 &m1)
00044 {
00045 m1.x = 1;
00046 m1.y = 2;
00047 m1.z = 3;
00048 return m1;
00049 }
00050
00051 std_msgs::Header& value_initialize(std_msgs::Header & h)
00052 {
00053 h.stamp = ros::Time(10);
00054 h.frame_id = "foobar";
00055 return h;
00056 }
00057
00058 geometry_msgs::Vector3Stamped& value_initialize(geometry_msgs::Vector3Stamped &m1)
00059 {
00060 value_initialize(m1.header);
00061 value_initialize(m1.vector);
00062 return m1;
00063 }
00064
00065 geometry_msgs::Point& value_initialize(geometry_msgs::Point &m1)
00066 {
00067 m1.x = 1;
00068 m1.y = 2;
00069 m1.z = 3;
00070 return m1;
00071 }
00072
00073 geometry_msgs::PointStamped& value_initialize(geometry_msgs::PointStamped &m1)
00074 {
00075 value_initialize(m1.header);
00076 value_initialize(m1.point);
00077 return m1;
00078 }
00079
00080 geometry_msgs::Quaternion & value_initialize(geometry_msgs::Quaternion &m1)
00081 {
00082 m1.x = 0;
00083 m1.y = 0;
00084 m1.z = 0.7071067811;
00085 m1.w = 0.7071067811;
00086 return m1;
00087 }
00088
00089 geometry_msgs::QuaternionStamped& value_initialize(geometry_msgs::QuaternionStamped &m1)
00090 {
00091 value_initialize(m1.header);
00092 value_initialize(m1.quaternion);
00093 return m1;
00094 }
00095
00096 geometry_msgs::Pose & value_initialize(geometry_msgs::Pose & m1)
00097 {
00098 value_initialize(m1.position);
00099 value_initialize(m1.orientation);
00100 return m1;
00101 }
00102
00103 geometry_msgs::PoseStamped& value_initialize(geometry_msgs::PoseStamped &m1)
00104 {
00105 value_initialize(m1.header);
00106 value_initialize(m1.pose);
00107 return m1;
00108 }
00109
00110 geometry_msgs::Transform & value_initialize(geometry_msgs::Transform & m1)
00111 {
00112 value_initialize(m1.translation);
00113 value_initialize(m1.rotation);
00114 return m1;
00115 }
00116
00117 geometry_msgs::TransformStamped& value_initialize(geometry_msgs::TransformStamped &m1)
00118 {
00119 value_initialize(m1.header);
00120 value_initialize(m1.transform);
00121 return m1;
00122 }
00123
00124 void expect_near(const std_msgs::Header & h1, const std_msgs::Header & h2)
00125 {
00126 EXPECT_NEAR(h1.stamp.toSec(), h2.stamp.toSec(), EPS);
00127 EXPECT_STREQ(h1.frame_id.c_str(), h2.frame_id.c_str());
00128 }
00129
00130
00131
00132
00133 void expect_near(const geometry_msgs::Vector3 & v1, const tf2::Vector3 & v2)
00134 {
00135 EXPECT_NEAR(v1.x, v2.x(), EPS);
00136 EXPECT_NEAR(v1.y, v2.y(), EPS);
00137 EXPECT_NEAR(v1.z, v2.z(), EPS);
00138 }
00139
00140 void expect_near(const geometry_msgs::Vector3 & v1, const geometry_msgs::Vector3 & v2)
00141 {
00142 EXPECT_NEAR(v1.x, v2.x, EPS);
00143 EXPECT_NEAR(v1.y, v2.y, EPS);
00144 EXPECT_NEAR(v1.z, v2.z, EPS);
00145 }
00146
00147 void expect_near(const tf2::Vector3 & v1, const tf2::Vector3 & v2)
00148 {
00149 EXPECT_NEAR(v1.x(), v2.x(), EPS);
00150 EXPECT_NEAR(v1.y(), v2.y(), EPS);
00151 EXPECT_NEAR(v1.z(), v2.z(), EPS);
00152 }
00153
00154 void expect_near(const geometry_msgs::Vector3Stamped & p1, const geometry_msgs::Vector3Stamped & p2)
00155 {
00156 expect_near(p1.header, p2.header);
00157 expect_near(p1.vector, p2.vector);
00158 }
00159
00160
00161
00162
00163 void expect_near(const geometry_msgs::Point & p1, const tf2::Vector3 & v2)
00164 {
00165 EXPECT_NEAR(p1.x, v2.x(), EPS);
00166 EXPECT_NEAR(p1.y, v2.y(), EPS);
00167 EXPECT_NEAR(p1.z, v2.z(), EPS);
00168 }
00169
00170 void expect_near(const geometry_msgs::Point & p1, const geometry_msgs::Point & v2)
00171 {
00172 EXPECT_NEAR(p1.x, v2.x, EPS);
00173 EXPECT_NEAR(p1.y, v2.y, EPS);
00174 EXPECT_NEAR(p1.z, v2.z, EPS);
00175 }
00176
00177 void expect_near(const geometry_msgs::PointStamped & p1, const geometry_msgs::PointStamped & p2)
00178 {
00179 expect_near(p1.header, p2.header);
00180 expect_near(p1.point, p2.point);
00181 }
00182
00183
00184
00185
00186
00187 void expect_near(const geometry_msgs::Quaternion & q1, const tf2::Quaternion & v2)
00188 {
00189 EXPECT_NEAR(q1.x, v2.x(), EPS);
00190 EXPECT_NEAR(q1.y, v2.y(), EPS);
00191 EXPECT_NEAR(q1.z, v2.z(), EPS);
00192 }
00193
00194 void expect_near(const geometry_msgs::Quaternion & q1, const geometry_msgs::Quaternion & v2)
00195 {
00196 EXPECT_NEAR(q1.x, v2.x, EPS);
00197 EXPECT_NEAR(q1.y, v2.y, EPS);
00198 EXPECT_NEAR(q1.z, v2.z, EPS);
00199 }
00200
00201 void expect_near(const geometry_msgs::QuaternionStamped & p1, const geometry_msgs::QuaternionStamped & p2)
00202 {
00203 expect_near(p1.header, p2.header);
00204 expect_near(p1.quaternion, p2.quaternion);
00205 }
00206
00207
00208
00209
00210 void expect_near(const geometry_msgs::Pose & p, const tf2::Transform & t)
00211 {
00212 expect_near(p.position, t.getOrigin());
00213 expect_near(p.orientation, t.getRotation());
00214 }
00215
00216 void expect_near(const geometry_msgs::Pose & p1, const geometry_msgs::Pose & p2)
00217 {
00218 expect_near(p1.position, p2.position);
00219 expect_near(p1.orientation, p2.orientation);
00220 }
00221
00222 void expect_near(const geometry_msgs::PoseStamped & p1, const geometry_msgs::PoseStamped & p2)
00223 {
00224 expect_near(p1.header, p2.header);
00225 expect_near(p1.pose, p2.pose);
00226 }
00227
00228
00229
00230
00231 void expect_near(const geometry_msgs::Transform & p, const tf2::Transform & t)
00232 {
00233 expect_near(p.translation, t.getOrigin());
00234 expect_near(p.rotation, t.getRotation());
00235 }
00236
00237 void expect_near(const geometry_msgs::Transform & p1, const geometry_msgs::Transform & p2)
00238 {
00239 expect_near(p1.translation, p2.translation);
00240 expect_near(p1.rotation, p2.rotation);
00241 }
00242
00243 void expect_near(const geometry_msgs::TransformStamped & p1, const geometry_msgs::TransformStamped & p2)
00244 {
00245 expect_near(p1.header, p2.header);
00246 expect_near(p1.transform, p2.transform);
00247 }
00248
00249
00250
00251
00252
00253 template <typename T>
00254 void expect_near(const tf2::Stamped<T>& s1, const tf2::Stamped<T>& s2)
00255 {
00256 expect_near((T)s1, (T)s2);
00257 }
00258
00259
00260
00261
00262
00263 TEST(tf2_geometry_msgs, Vector3)
00264 {
00265 geometry_msgs::Vector3 m1;
00266 value_initialize(m1);
00267 tf2::Vector3 v1;
00268 fromMsg(m1, v1);
00269 SCOPED_TRACE("m1 v1");
00270 expect_near(m1, v1);
00271 geometry_msgs::Vector3 m2 = toMsg(v1);
00272 SCOPED_TRACE("m1 m2");
00273 expect_near(m1, m2);
00274 }
00275
00276 TEST(tf2_geometry_msgs, Point)
00277 {
00278 geometry_msgs::Point m1;
00279 value_initialize(m1);
00280 tf2::Vector3 v1;
00281 SCOPED_TRACE("m1 v1");
00282 fromMsg(m1, v1);
00283 expect_near(m1, v1);
00284 geometry_msgs::Point m2 = toMsg(v1, m2);
00285 SCOPED_TRACE("m1 m2");
00286 expect_near(m1, m2);
00287 }
00288
00289 TEST(tf2_geometry_msgs, Quaternion)
00290 {
00291 geometry_msgs::Quaternion m1;
00292 value_initialize(m1);
00293 tf2::Quaternion q1;
00294 SCOPED_TRACE("m1 q1");
00295 fromMsg(m1, q1);
00296 expect_near(m1, q1);
00297 geometry_msgs::Quaternion m2 = toMsg(q1);
00298 SCOPED_TRACE("m1 m2");
00299 expect_near(m1, m2);
00300 }
00301
00302 TEST(tf2_geometry_msgs, Pose)
00303 {
00304 geometry_msgs::Pose m1;
00305 value_initialize(m1);
00306 tf2::Transform t1;
00307 fromMsg(m1, t1);
00308 SCOPED_TRACE("m1 t1");
00309 expect_near(m1, t1);
00310 geometry_msgs::Pose m2 = toMsg(t1, m2);
00311 SCOPED_TRACE("m1 m2");
00312 expect_near(m1, m2);
00313 }
00314
00315 TEST(tf2_geometry_msgs, Transform)
00316 {
00317 geometry_msgs::Transform m1;
00318 value_initialize(m1);
00319 tf2::Transform t1;
00320 fromMsg(m1, t1);
00321 SCOPED_TRACE("m1 t1");
00322 expect_near(m1, t1);
00323 geometry_msgs::Transform m2 = toMsg(t1);
00324 SCOPED_TRACE("m1 m2");
00325 expect_near(m1, m2);
00326 }
00327
00328 TEST(tf2_geometry_msgs, Vector3Stamped)
00329 {
00330 geometry_msgs::Vector3Stamped m1;
00331 value_initialize(m1);
00332 tf2::Stamped<tf2::Vector3> v1;
00333 fromMsg(m1, v1);
00334 SCOPED_TRACE("m1 v1");
00335
00336 geometry_msgs::Vector3Stamped m2;
00337 m2 = toMsg(v1);
00338 SCOPED_TRACE("m1 m2");
00339 expect_near(m1, m2);
00340 }
00341
00342 TEST(tf2_geometry_msgs, PointStamped)
00343 {
00344 geometry_msgs::PointStamped m1;
00345 value_initialize(m1);
00346 tf2::Stamped<tf2::Vector3> v1;
00347 fromMsg(m1, v1);
00348 SCOPED_TRACE("m1 v1");
00349
00350 geometry_msgs::PointStamped m2;
00351 m2 = toMsg(v1, m2);
00352 SCOPED_TRACE("m1 m2");
00353 expect_near(m1, m2);
00354 }
00355
00356 TEST(tf2_geometry_msgs, QuaternionStamped)
00357 {
00358 geometry_msgs::QuaternionStamped m1;
00359 value_initialize(m1);
00360 tf2::Stamped<tf2::Quaternion> v1;
00361 fromMsg(m1, v1);
00362 SCOPED_TRACE("m1 v1");
00363
00364 geometry_msgs::QuaternionStamped m2;
00365 m2 = tf2::toMsg(v1);
00366 SCOPED_TRACE("m1 m2");
00367 expect_near(m1, m2);
00368 }
00369
00370 TEST(tf2_geometry_msgs, PoseStamped)
00371 {
00372 geometry_msgs::PoseStamped m1;
00373 value_initialize(m1);
00374 tf2::Stamped<tf2::Transform> v1;
00375 SCOPED_TRACE("m1 v1");
00376 fromMsg(m1, v1);
00377
00378 geometry_msgs::PoseStamped m2;
00379 m2 = tf2::toMsg(v1, m2);
00380 SCOPED_TRACE("m1 m2");
00381 expect_near(m1, m2);
00382 }
00383
00384 TEST(tf2_geometry_msgs, TransformStamped)
00385 {
00386 geometry_msgs::TransformStamped m1;
00387 value_initialize(m1);
00388 tf2::Stamped<tf2::Transform> v1;
00389 fromMsg(m1, v1);
00390 SCOPED_TRACE("m1 v1");
00391
00392 geometry_msgs::TransformStamped m2;
00393 m2 = tf2::toMsg(v1);
00394 SCOPED_TRACE("m1 m2");
00395 expect_near(m1, m2);
00396 }
00397
00398
00399
00400
00401 int main(int argc, char **argv){
00402 testing::InitGoogleTest(&argc, argv);
00403 int ret = RUN_ALL_TESTS();
00404 return ret;
00405 }