test_tf2_geometry_msgs.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 
33 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
34 #include <tf2_ros/transform_listener.h>
35 #include <ros/ros.h>
36 #include <gtest/gtest.h>
37 #include <tf2_ros/buffer.h>
38 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
39 
40 tf2_ros::Buffer* tf_buffer;
41 geometry_msgs::TransformStamped t;
42 static const double EPS = 1e-3;
43 
44 
45 TEST(TfGeometry, Frame)
46 {
47  geometry_msgs::PoseStamped v1;
48  v1.pose.position.x = 1;
49  v1.pose.position.y = 2;
50  v1.pose.position.z = 3;
51  v1.pose.orientation.x = 1;
52  v1.header.stamp = ros::Time(2);
53  v1.header.frame_id = "A";
54 
55  // simple api
56  geometry_msgs::PoseStamped v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0));
57  EXPECT_NEAR(v_simple.pose.position.x, -9, EPS);
58  EXPECT_NEAR(v_simple.pose.position.y, 18, EPS);
59  EXPECT_NEAR(v_simple.pose.position.z, 27, EPS);
60  EXPECT_NEAR(v_simple.pose.orientation.x, 0.0, EPS);
61  EXPECT_NEAR(v_simple.pose.orientation.y, 0.0, EPS);
62  EXPECT_NEAR(v_simple.pose.orientation.z, 0.0, EPS);
63  EXPECT_NEAR(v_simple.pose.orientation.w, 1.0, EPS);
64 
65  // advanced api
66  geometry_msgs::PoseStamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0),
67  "A", ros::Duration(3.0));
68  EXPECT_NEAR(v_advanced.pose.position.x, -9, EPS);
69  EXPECT_NEAR(v_advanced.pose.position.y, 18, EPS);
70  EXPECT_NEAR(v_advanced.pose.position.z, 27, EPS);
71  EXPECT_NEAR(v_advanced.pose.orientation.x, 0.0, EPS);
72  EXPECT_NEAR(v_advanced.pose.orientation.y, 0.0, EPS);
73  EXPECT_NEAR(v_advanced.pose.orientation.z, 0.0, EPS);
74  EXPECT_NEAR(v_advanced.pose.orientation.w, 1.0, EPS);
75 }
76 
77 TEST(TfGeometry, PoseWithCovarianceStamped)
78 {
79  geometry_msgs::PoseWithCovarianceStamped v1;
80  v1.pose.pose.position.x = 1;
81  v1.pose.pose.position.y = 2;
82  v1.pose.pose.position.z = 3;
83  v1.pose.pose.orientation.x = 1;
84  v1.header.stamp = ros::Time(2);
85  v1.header.frame_id = "A";
86  v1.pose.covariance[0] = 1;
87  v1.pose.covariance[7] = 1;
88  v1.pose.covariance[14] = 1;
89  v1.pose.covariance[21] = 1;
90  v1.pose.covariance[28] = 1;
91  v1.pose.covariance[35] = 1;
92 
93  // simple api
94  const geometry_msgs::PoseWithCovarianceStamped v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0));
95  EXPECT_NEAR(v_simple.pose.pose.position.x, -9, EPS);
96  EXPECT_NEAR(v_simple.pose.pose.position.y, 18, EPS);
97  EXPECT_NEAR(v_simple.pose.pose.position.z, 27, EPS);
98  EXPECT_NEAR(v_simple.pose.pose.orientation.x, 0.0, EPS);
99  EXPECT_NEAR(v_simple.pose.pose.orientation.y, 0.0, EPS);
100  EXPECT_NEAR(v_simple.pose.pose.orientation.z, 0.0, EPS);
101  EXPECT_NEAR(v_simple.pose.pose.orientation.w, 1.0, EPS);
102 
103  // no rotation in this transformation, so no change to covariance
104  EXPECT_NEAR(v_simple.pose.covariance[0], 1.0, EPS);
105  EXPECT_NEAR(v_simple.pose.covariance[7], 1.0, EPS);
106  EXPECT_NEAR(v_simple.pose.covariance[14], 1.0, EPS);
107  EXPECT_NEAR(v_simple.pose.covariance[21], 1.0, EPS);
108  EXPECT_NEAR(v_simple.pose.covariance[28], 1.0, EPS);
109  EXPECT_NEAR(v_simple.pose.covariance[35], 1.0, EPS);
110 
111  // advanced api
112  const geometry_msgs::PoseWithCovarianceStamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0),
113  "A", ros::Duration(3.0));
114  EXPECT_NEAR(v_advanced.pose.pose.position.x, -9, EPS);
115  EXPECT_NEAR(v_advanced.pose.pose.position.y, 18, EPS);
116  EXPECT_NEAR(v_advanced.pose.pose.position.z, 27, EPS);
117  EXPECT_NEAR(v_advanced.pose.pose.orientation.x, 0.0, EPS);
118  EXPECT_NEAR(v_advanced.pose.pose.orientation.y, 0.0, EPS);
119  EXPECT_NEAR(v_advanced.pose.pose.orientation.z, 0.0, EPS);
120  EXPECT_NEAR(v_advanced.pose.pose.orientation.w, 1.0, EPS);
121 
122  // no rotation in this transformation, so no change to covariance
123  EXPECT_NEAR(v_advanced.pose.covariance[0], 1.0, EPS);
124  EXPECT_NEAR(v_advanced.pose.covariance[7], 1.0, EPS);
125  EXPECT_NEAR(v_advanced.pose.covariance[14], 1.0, EPS);
126  EXPECT_NEAR(v_advanced.pose.covariance[21], 1.0, EPS);
127  EXPECT_NEAR(v_advanced.pose.covariance[28], 1.0, EPS);
128  EXPECT_NEAR(v_advanced.pose.covariance[35], 1.0, EPS);
129 
132  // rotate pi/2 radians about x-axis
133  geometry_msgs::TransformStamped t_rot;
134  t_rot.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(1,0,0), M_PI/2));
135  t_rot.header.stamp = ros::Time(2.0);
136  t_rot.header.frame_id = "A";
137  t_rot.child_frame_id = "rotated";
138  tf_buffer->setTransform(t_rot, "rotation_test");
139 
140  // need to put some covariance in the matrix
141  v1.pose.covariance[1] = 1;
142  v1.pose.covariance[6] = 1;
143  v1.pose.covariance[12] = 1;
144 
145  // perform rotation
146  const geometry_msgs::PoseWithCovarianceStamped v_rotated = tf_buffer->transform(v1, "rotated", ros::Duration(2.0));
147 
148  // the covariance matrix should now be transformed
149  EXPECT_NEAR(v_rotated.pose.covariance[0], 1.0, EPS);
150  EXPECT_NEAR(v_rotated.pose.covariance[1], 0.0, EPS);
151  EXPECT_NEAR(v_rotated.pose.covariance[2],-1.0, EPS);
152  EXPECT_NEAR(v_rotated.pose.covariance[6], 1.0, EPS);
153  EXPECT_NEAR(v_rotated.pose.covariance[7], 1.0, EPS);
154  EXPECT_NEAR(v_rotated.pose.covariance[8], 0.0, EPS);
155  EXPECT_NEAR(v_rotated.pose.covariance[12],-1.0, EPS);
156  EXPECT_NEAR(v_rotated.pose.covariance[13], 0.0, EPS);
157  EXPECT_NEAR(v_rotated.pose.covariance[14], 1.0, EPS);
158 
159  // set buffer back to original transform
160  tf_buffer->setTransform(t, "test");
161 }
162 
163 TEST(TfGeometry, Transform)
164 {
165  geometry_msgs::TransformStamped v1;
166  v1.transform.translation.x = 1;
167  v1.transform.translation.y = 2;
168  v1.transform.translation.z = 3;
169  v1.transform.rotation.x = 1;
170  v1.header.stamp = ros::Time(2);
171  v1.header.frame_id = "A";
172 
173  // simple api
174  geometry_msgs::TransformStamped v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0));
175  EXPECT_NEAR(v_simple.transform.translation.x, -9, EPS);
176  EXPECT_NEAR(v_simple.transform.translation.y, 18, EPS);
177  EXPECT_NEAR(v_simple.transform.translation.z, 27, EPS);
178  EXPECT_NEAR(v_simple.transform.rotation.x, 0.0, EPS);
179  EXPECT_NEAR(v_simple.transform.rotation.y, 0.0, EPS);
180  EXPECT_NEAR(v_simple.transform.rotation.z, 0.0, EPS);
181  EXPECT_NEAR(v_simple.transform.rotation.w, 1.0, EPS);
182 
183 
184  // advanced api
185  geometry_msgs::TransformStamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0),
186  "A", ros::Duration(3.0));
187  EXPECT_NEAR(v_advanced.transform.translation.x, -9, EPS);
188  EXPECT_NEAR(v_advanced.transform.translation.y, 18, EPS);
189  EXPECT_NEAR(v_advanced.transform.translation.z, 27, EPS);
190  EXPECT_NEAR(v_advanced.transform.rotation.x, 0.0, EPS);
191  EXPECT_NEAR(v_advanced.transform.rotation.y, 0.0, EPS);
192  EXPECT_NEAR(v_advanced.transform.rotation.z, 0.0, EPS);
193  EXPECT_NEAR(v_advanced.transform.rotation.w, 1.0, EPS);
194 }
195 
196 TEST(TfGeometry, Vector)
197 {
198  geometry_msgs::Vector3Stamped v1, res;
199  v1.vector.x = 1;
200  v1.vector.y = 2;
201  v1.vector.z = 3;
202  v1.header.stamp = ros::Time(2.0);
203  v1.header.frame_id = "A";
204 
205  // simple api
206  geometry_msgs::Vector3Stamped v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0));
207  EXPECT_NEAR(v_simple.vector.x, 1, EPS);
208  EXPECT_NEAR(v_simple.vector.y, -2, EPS);
209  EXPECT_NEAR(v_simple.vector.z, -3, EPS);
210 
211  // advanced api
212  geometry_msgs::Vector3Stamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0),
213  "A", ros::Duration(3.0));
214  EXPECT_NEAR(v_advanced.vector.x, 1, EPS);
215  EXPECT_NEAR(v_advanced.vector.y, -2, EPS);
216  EXPECT_NEAR(v_advanced.vector.z, -3, EPS);
217 }
218 
219 
220 TEST(TfGeometry, Point)
221 {
222  geometry_msgs::PointStamped v1, res;
223  v1.point.x = 1;
224  v1.point.y = 2;
225  v1.point.z = 3;
226  v1.header.stamp = ros::Time(2.0);
227  v1.header.frame_id = "A";
228 
229  // simple api
230  geometry_msgs::PointStamped v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0));
231  EXPECT_NEAR(v_simple.point.x, -9, EPS);
232  EXPECT_NEAR(v_simple.point.y, 18, EPS);
233  EXPECT_NEAR(v_simple.point.z, 27, EPS);
234 
235  // advanced api
236  geometry_msgs::PointStamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0),
237  "A", ros::Duration(3.0));
238  EXPECT_NEAR(v_advanced.point.x, -9, EPS);
239  EXPECT_NEAR(v_advanced.point.y, 18, EPS);
240  EXPECT_NEAR(v_advanced.point.z, 27, EPS);
241 }
242 
243 TEST(TfGeometry, doTransformPoint)
244 {
245  geometry_msgs::Point v1, res;
246  v1.x = 2;
247  v1.y = 1;
248  v1.z = 3;
249 
250  geometry_msgs::TransformStamped trafo;
251  trafo.transform.translation.x = -1;
252  trafo.transform.translation.y = 2;
253  trafo.transform.translation.z = -3;
254  trafo.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0,0,1), -M_PI / 2.0));
255 
256  tf2::doTransform(v1, res, trafo);
257 
258  EXPECT_NEAR(res.x, 0, EPS);
259  EXPECT_NEAR(res.y, 0, EPS);
260  EXPECT_NEAR(res.z, 0, EPS);
261 }
262 
263 TEST(TfGeometry, doTransformQuaterion)
264 {
265  geometry_msgs::Quaternion v1, res;
266  v1.w = 1;
267 
268  geometry_msgs::TransformStamped trafo;
269  trafo.transform.translation.x = -1;
270  trafo.transform.translation.y = 2;
271  trafo.transform.translation.z = -3;
272  trafo.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0,0,1), -M_PI / 2.0));
273 
274  tf2::doTransform(v1, res, trafo);
275 
276  EXPECT_NEAR(res.x, trafo.transform.rotation.x, EPS);
277  EXPECT_NEAR(res.y, trafo.transform.rotation.y, EPS);
278  EXPECT_NEAR(res.z, trafo.transform.rotation.z, EPS);
279  EXPECT_NEAR(res.w, trafo.transform.rotation.w, EPS);
280 }
281 
282 TEST(TfGeometry, doTransformPose)
283 {
284  geometry_msgs::Pose v1, res;
285  v1.position.x = 2;
286  v1.position.y = 1;
287  v1.position.z = 3;
288  v1.orientation.w = 1;
289 
290  geometry_msgs::TransformStamped trafo;
291  trafo.transform.translation.x = -1;
292  trafo.transform.translation.y = 2;
293  trafo.transform.translation.z = -3;
294  trafo.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0,0,1), -M_PI / 2.0));
295 
296  tf2::doTransform(v1, res, trafo);
297 
298  EXPECT_NEAR(res.position.x, 0, EPS);
299  EXPECT_NEAR(res.position.y, 0, EPS);
300  EXPECT_NEAR(res.position.z, 0, EPS);
301 
302  EXPECT_NEAR(res.orientation.x, trafo.transform.rotation.x, EPS);
303  EXPECT_NEAR(res.orientation.y, trafo.transform.rotation.y, EPS);
304  EXPECT_NEAR(res.orientation.z, trafo.transform.rotation.z, EPS);
305  EXPECT_NEAR(res.orientation.w, trafo.transform.rotation.w, EPS);
306 }
307 
308 TEST(TfGeometry, doTransformVector3)
309 {
310  geometry_msgs::Vector3 v1, res;
311  v1.x = 2;
312  v1.y = 1;
313  v1.z = 3;
314 
315  geometry_msgs::TransformStamped trafo;
316  trafo.transform.translation.x = -1;
317  trafo.transform.translation.y = 2;
318  trafo.transform.translation.z = -3;
319  trafo.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0,0,1), -M_PI / 2.0));
320 
321  tf2::doTransform(v1, res, trafo);
322 
323  EXPECT_NEAR(res.x, 1, EPS);
324  EXPECT_NEAR(res.y, -2, EPS);
325  EXPECT_NEAR(res.z, 3, EPS);
326 }
327 
328 TEST(TfGeometry, doTransformWrench)
329 {
330  geometry_msgs::Wrench v1, res;
331  v1.force.x = 2;
332  v1.force.y = 1;
333  v1.force.z = 3;
334  v1.torque.x = 2;
335  v1.torque.y = 1;
336  v1.torque.z = 3;
337 
338  geometry_msgs::TransformStamped trafo;
339  trafo.transform.translation.x = -1;
340  trafo.transform.translation.y = 2;
341  trafo.transform.translation.z = -3;
342  trafo.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0,0,1), -M_PI / 2.0));
343 
344  tf2::doTransform(v1, res, trafo);
345  EXPECT_NEAR(res.force.x, 1, EPS);
346  EXPECT_NEAR(res.force.y, -2, EPS);
347  EXPECT_NEAR(res.force.z, 3, EPS);
348 
349  EXPECT_NEAR(res.torque.x, 1, EPS);
350  EXPECT_NEAR(res.torque.y, -2, EPS);
351  EXPECT_NEAR(res.torque.z, 3, EPS);
352 }
353 
354 int main(int argc, char **argv){
355  testing::InitGoogleTest(&argc, argv);
356  ros::init(argc, argv, "test");
357  ros::NodeHandle n;
358 
359  tf_buffer = new tf2_ros::Buffer();
360  tf_buffer->setUsingDedicatedThread(true);
361 
362  // populate buffer
363  t.transform.translation.x = 10;
364  t.transform.translation.y = 20;
365  t.transform.translation.z = 30;
366  t.transform.rotation.x = 1;
367  t.header.stamp = ros::Time(2.0);
368  t.header.frame_id = "A";
369  t.child_frame_id = "B";
370  tf_buffer->setTransform(t, "test");
371 
372  int ret = RUN_ALL_TESTS();
373  delete tf_buffer;
374  return ret;
375 }
376 
377 
378 
379 
380 
transform_listener.h
ros::NodeHandle
tf2::BufferCore::setUsingDedicatedThread
void setUsingDedicatedThread(bool value)
tf2::BufferCore::setTransform
bool setTransform(const geometry_msgs::TransformStamped &transform, const std::string &authority, bool is_static=false)
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
tf2::doTransform
void doTransform(const T &data_in, T &data_out, const geometry_msgs::TransformStamped &transform)
tf2_ros::Buffer
t
geometry_msgs::TransformStamped t
Definition: test_tf2_geometry_msgs.cpp:41
main
int main(int argc, char **argv)
Definition: test_tf2_geometry_msgs.cpp:354
tf2_geometry_msgs.h
TEST
TEST(TfGeometry, Frame)
Definition: test_tf2_geometry_msgs.cpp:45
tf2::toMsg
B toMsg(const A &a)
tf_buffer
tf2_ros::Buffer * tf_buffer
Definition: test_tf2_geometry_msgs.cpp:40
EPS
static const double EPS
Definition: test_tf2_geometry_msgs.cpp:42
tf2_ros::BufferInterface::transform
T & transform(const T &in, T &out, const std::string &target_frame, ros::Duration timeout=ros::Duration(0.0)) const

tf2_geometry_msgs
Author(s): Wim Meeussen
autogenerated on Fri Oct 16 2020 19:08:59