test_tomsg_frommsg.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
34 #include <gtest/gtest.h>
35 
36 static const double EPS = 1e-6;
37 
38 tf2::Vector3 get_tf2_vector()
39 {
40  return tf2::Vector3(1.0, 2.0, 3.0);
41 }
42 
43 geometry_msgs::Vector3& value_initialize(geometry_msgs::Vector3 &m1)
44 {
45  m1.x = 1;
46  m1.y = 2;
47  m1.z = 3;
48  return m1;
49 }
50 
51 std_msgs::Header& value_initialize(std_msgs::Header & h)
52 {
53  h.stamp = ros::Time(10);
54  h.frame_id = "foobar";
55  return h;
56 }
57 
58 geometry_msgs::Vector3Stamped& value_initialize(geometry_msgs::Vector3Stamped &m1)
59 {
60  value_initialize(m1.header);
61  value_initialize(m1.vector);
62  return m1;
63 }
64 
65 geometry_msgs::Point& value_initialize(geometry_msgs::Point &m1)
66 {
67  m1.x = 1;
68  m1.y = 2;
69  m1.z = 3;
70  return m1;
71 }
72 
73 geometry_msgs::PointStamped& value_initialize(geometry_msgs::PointStamped &m1)
74 {
75  value_initialize(m1.header);
76  value_initialize(m1.point);
77  return m1;
78 }
79 
80 geometry_msgs::Quaternion & value_initialize(geometry_msgs::Quaternion &m1)
81 {
82  m1.x = 0;
83  m1.y = 0;
84  m1.z = 0.7071067811;
85  m1.w = 0.7071067811;
86  return m1;
87 }
88 
89 geometry_msgs::QuaternionStamped& value_initialize(geometry_msgs::QuaternionStamped &m1)
90 {
91  value_initialize(m1.header);
92  value_initialize(m1.quaternion);
93  return m1;
94 }
95 
96 geometry_msgs::Pose & value_initialize(geometry_msgs::Pose & m1)
97 {
98  value_initialize(m1.position);
99  value_initialize(m1.orientation);
100  return m1;
101 }
102 
103 geometry_msgs::PoseStamped& value_initialize(geometry_msgs::PoseStamped &m1)
104 {
105  value_initialize(m1.header);
106  value_initialize(m1.pose);
107  return m1;
108 }
109 
110 geometry_msgs::Transform & value_initialize(geometry_msgs::Transform & m1)
111 {
112  value_initialize(m1.translation);
113  value_initialize(m1.rotation);
114  return m1;
115 }
116 
117 geometry_msgs::TransformStamped& value_initialize(geometry_msgs::TransformStamped &m1)
118 {
119  value_initialize(m1.header);
120  value_initialize(m1.transform);
121  return m1;
122 }
123 
124 void expect_near(const std_msgs::Header & h1, const std_msgs::Header & h2)
125 {
126  EXPECT_NEAR(h1.stamp.toSec(), h2.stamp.toSec(), EPS);
127  EXPECT_STREQ(h1.frame_id.c_str(), h2.frame_id.c_str());
128 }
129 
130 /*
131  * Vector3
132  */
133 void expect_near(const geometry_msgs::Vector3 & v1, const tf2::Vector3 & v2)
134 {
135  EXPECT_NEAR(v1.x, v2.x(), EPS);
136  EXPECT_NEAR(v1.y, v2.y(), EPS);
137  EXPECT_NEAR(v1.z, v2.z(), EPS);
138 }
139 
140 void expect_near(const geometry_msgs::Vector3 & v1, const geometry_msgs::Vector3 & v2)
141 {
142  EXPECT_NEAR(v1.x, v2.x, EPS);
143  EXPECT_NEAR(v1.y, v2.y, EPS);
144  EXPECT_NEAR(v1.z, v2.z, EPS);
145 }
146 
147 void expect_near(const tf2::Vector3 & v1, const tf2::Vector3 & v2)
148 {
149  EXPECT_NEAR(v1.x(), v2.x(), EPS);
150  EXPECT_NEAR(v1.y(), v2.y(), EPS);
151  EXPECT_NEAR(v1.z(), v2.z(), EPS);
152 }
153 
154 void expect_near(const geometry_msgs::Vector3Stamped & p1, const geometry_msgs::Vector3Stamped & p2)
155 {
156  expect_near(p1.header, p2.header);
157  expect_near(p1.vector, p2.vector);
158 }
159 
160 /*
161  * Point
162  */
163 void expect_near(const geometry_msgs::Point & p1, const tf2::Vector3 & v2)
164 {
165  EXPECT_NEAR(p1.x, v2.x(), EPS);
166  EXPECT_NEAR(p1.y, v2.y(), EPS);
167  EXPECT_NEAR(p1.z, v2.z(), EPS);
168 }
169 
170 void expect_near(const geometry_msgs::Point & p1, const geometry_msgs::Point & v2)
171 {
172  EXPECT_NEAR(p1.x, v2.x, EPS);
173  EXPECT_NEAR(p1.y, v2.y, EPS);
174  EXPECT_NEAR(p1.z, v2.z, EPS);
175 }
176 
177 void expect_near(const geometry_msgs::PointStamped & p1, const geometry_msgs::PointStamped & p2)
178 {
179  expect_near(p1.header, p2.header);
180  expect_near(p1.point, p2.point);
181 }
182 
183 
184 /*
185  * Quaternion
186  */
187 void expect_near(const geometry_msgs::Quaternion & q1, const tf2::Quaternion & v2)
188 {
189  EXPECT_NEAR(q1.x, v2.x(), EPS);
190  EXPECT_NEAR(q1.y, v2.y(), EPS);
191  EXPECT_NEAR(q1.z, v2.z(), EPS);
192 }
193 
194 void expect_near(const geometry_msgs::Quaternion & q1, const geometry_msgs::Quaternion & v2)
195 {
196  EXPECT_NEAR(q1.x, v2.x, EPS);
197  EXPECT_NEAR(q1.y, v2.y, EPS);
198  EXPECT_NEAR(q1.z, v2.z, EPS);
199 }
200 
201 void expect_near(const geometry_msgs::QuaternionStamped & p1, const geometry_msgs::QuaternionStamped & p2)
202 {
203  expect_near(p1.header, p2.header);
204  expect_near(p1.quaternion, p2.quaternion);
205 }
206 
207 /*
208  * Pose
209  */
210 void expect_near(const geometry_msgs::Pose & p, const tf2::Transform & t)
211 {
212  expect_near(p.position, t.getOrigin());
213  expect_near(p.orientation, t.getRotation());
214 }
215 
216 void expect_near(const geometry_msgs::Pose & p1, const geometry_msgs::Pose & p2)
217 {
218  expect_near(p1.position, p2.position);
219  expect_near(p1.orientation, p2.orientation);
220 }
221 
222 void expect_near(const geometry_msgs::PoseStamped & p1, const geometry_msgs::PoseStamped & p2)
223 {
224  expect_near(p1.header, p2.header);
225  expect_near(p1.pose, p2.pose);
226 }
227 
228 /*
229  * Transform
230  */
231 void expect_near(const geometry_msgs::Transform & p, const tf2::Transform & t)
232 {
233  expect_near(p.translation, t.getOrigin());
234  expect_near(p.rotation, t.getRotation());
235 }
236 
237 void expect_near(const geometry_msgs::Transform & p1, const geometry_msgs::Transform & p2)
238 {
239  expect_near(p1.translation, p2.translation);
240  expect_near(p1.rotation, p2.rotation);
241 }
242 
243 void expect_near(const geometry_msgs::TransformStamped & p1, const geometry_msgs::TransformStamped & p2)
244 {
245  expect_near(p1.header, p2.header);
246  expect_near(p1.transform, p2.transform);
247 }
248 
249 /*
250  * Stamped templated expect_near
251  */
252 
253 template <typename T>
254 void expect_near(const tf2::Stamped<T>& s1, const tf2::Stamped<T>& s2)
255 {
256  expect_near((T)s1, (T)s2);
257 }
258 
259 /*********************
260  * Tests
261  *********************/
262 
263 TEST(tf2_geometry_msgs, Vector3)
264 {
265  geometry_msgs::Vector3 m1;
266  value_initialize(m1);
267  tf2::Vector3 v1;
268  fromMsg(m1, v1);
269  SCOPED_TRACE("m1 v1");
270  expect_near(m1, v1);
271  geometry_msgs::Vector3 m2 = toMsg(v1);
272  SCOPED_TRACE("m1 m2");
273  expect_near(m1, m2);
274 }
275 
276 TEST(tf2_geometry_msgs, Point)
277 {
278  geometry_msgs::Point m1;
279  value_initialize(m1);
280  tf2::Vector3 v1;
281  SCOPED_TRACE("m1 v1");
282  fromMsg(m1, v1);
283  expect_near(m1, v1);
284  geometry_msgs::Point m2 = toMsg(v1, m2);
285  SCOPED_TRACE("m1 m2");
286  expect_near(m1, m2);
287 }
288 
289 TEST(tf2_geometry_msgs, Quaternion)
290 {
291  geometry_msgs::Quaternion m1;
292  value_initialize(m1);
293  tf2::Quaternion q1;
294  SCOPED_TRACE("m1 q1");
295  fromMsg(m1, q1);
296  expect_near(m1, q1);
297  geometry_msgs::Quaternion m2 = toMsg(q1);
298  SCOPED_TRACE("m1 m2");
299  expect_near(m1, m2);
300 }
301 
302 TEST(tf2_geometry_msgs, Pose)
303 {
304  geometry_msgs::Pose m1;
305  value_initialize(m1);
306  tf2::Transform t1;
307  fromMsg(m1, t1);
308  SCOPED_TRACE("m1 t1");
309  expect_near(m1, t1);
310  geometry_msgs::Pose m2 = toMsg(t1, m2);
311  SCOPED_TRACE("m1 m2");
312  expect_near(m1, m2);
313 }
314 
315 TEST(tf2_geometry_msgs, Transform)
316 {
317  geometry_msgs::Transform m1;
318  value_initialize(m1);
319  tf2::Transform t1;
320  fromMsg(m1, t1);
321  SCOPED_TRACE("m1 t1");
322  expect_near(m1, t1);
323  geometry_msgs::Transform m2 = toMsg(t1);
324  SCOPED_TRACE("m1 m2");
325  expect_near(m1, m2);
326 }
327 
328 TEST(tf2_geometry_msgs, Vector3Stamped)
329 {
330  geometry_msgs::Vector3Stamped m1;
331  value_initialize(m1);
333  fromMsg(m1, v1);
334  SCOPED_TRACE("m1 v1");
335  // expect_near(m1, v1);
336  geometry_msgs::Vector3Stamped m2;
337  m2 = toMsg(v1);
338  SCOPED_TRACE("m1 m2");
339  expect_near(m1, m2);
340 }
341 
342 TEST(tf2_geometry_msgs, PointStamped)
343 {
344  geometry_msgs::PointStamped m1;
345  value_initialize(m1);
347  fromMsg(m1, v1);
348  SCOPED_TRACE("m1 v1");
349  // expect_near(m1, v1); //TODO implement cross verification explicityly
350  geometry_msgs::PointStamped m2;
351  m2 = toMsg(v1, m2);
352  SCOPED_TRACE("m1 m2");
353  expect_near(m1, m2);
354 }
355 
356 TEST(tf2_geometry_msgs, QuaternionStamped)
357 {
358  geometry_msgs::QuaternionStamped m1;
359  value_initialize(m1);
361  fromMsg(m1, v1);
362  SCOPED_TRACE("m1 v1");
363  // expect_near(m1, v1); //TODO implement cross verification explicityly
364  geometry_msgs::QuaternionStamped m2;
365  m2 = tf2::toMsg(v1);
366  SCOPED_TRACE("m1 m2");
367  expect_near(m1, m2);
368 }
369 
370 TEST(tf2_geometry_msgs, PoseStamped)
371 {
372  geometry_msgs::PoseStamped m1;
373  value_initialize(m1);
375  SCOPED_TRACE("m1 v1");
376  fromMsg(m1, v1);
377  // expect_near(m1, v1); //TODO implement cross verification explicityly
378  geometry_msgs::PoseStamped m2;
379  m2 = tf2::toMsg(v1, m2);
380  SCOPED_TRACE("m1 m2");
381  expect_near(m1, m2);
382 }
383 
384 TEST(tf2_geometry_msgs, TransformStamped)
385 {
386  geometry_msgs::TransformStamped m1;
387  value_initialize(m1);
389  fromMsg(m1, v1);
390  SCOPED_TRACE("m1 v1");
391  // expect_near(m1, v1);
392  geometry_msgs::TransformStamped m2;
393  m2 = tf2::toMsg(v1);
394  SCOPED_TRACE("m1 m2");
395  expect_near(m1, m2);
396 }
397 
398 
399 
400 
401 int main(int argc, char **argv){
402  testing::InitGoogleTest(&argc, argv);
403  int ret = RUN_ALL_TESTS();
404  return ret;
405 }
expect_near
void expect_near(const std_msgs::Header &h1, const std_msgs::Header &h2)
Definition: test_tomsg_frommsg.cpp:124
fromMsg
void fromMsg(const A &, B &b)
tf2_geometry_msgs.h
tf2::Stamped
main
int main(int argc, char **argv)
Definition: test_tomsg_frommsg.cpp:401
tf2::Transform
TEST
TEST(tf2_geometry_msgs, Vector3)
Definition: test_tomsg_frommsg.cpp:263
ros::Time
value_initialize
geometry_msgs::Vector3 & value_initialize(geometry_msgs::Vector3 &m1)
Definition: test_tomsg_frommsg.cpp:43
tf2::Quaternion
toMsg
B toMsg(const A &a)
EPS
static const double EPS
Definition: test_tomsg_frommsg.cpp:36
get_tf2_vector
tf2::Vector3 get_tf2_vector()
Definition: test_tomsg_frommsg.cpp:38
t
geometry_msgs::TransformStamped t
Definition: test_tf2_geometry_msgs.cpp:41


tf2_geometry_msgs
Author(s): Wim Meeussen
autogenerated on Sun Feb 4 2024 03:18:21