simple_tf2_core.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 
30 #include <gtest/gtest.h>
31 #include <tf2/buffer_core.h>
32 #include <sys/time.h>
33 #include <ros/ros.h>
34 #include "tf2/LinearMath/Vector3.h"
35 #include "tf2/exceptions.h"
36 
37 TEST(tf2, setTransformFail)
38 {
39  tf2::BufferCore tfc;
40  geometry_msgs::TransformStamped st;
41  EXPECT_FALSE(tfc.setTransform(st, "authority1"));
42 
43 }
44 
45 TEST(tf2, setTransformValid)
46 {
47  tf2::BufferCore tfc;
48  geometry_msgs::TransformStamped st;
49  st.header.frame_id = "foo";
50  st.header.stamp = ros::Time(1.0);
51  st.child_frame_id = "child";
52  st.transform.rotation.w = 1;
53  EXPECT_TRUE(tfc.setTransform(st, "authority1"));
54 
55 }
56 
57 TEST(tf2, setTransformInvalidQuaternion)
58 {
59  tf2::BufferCore tfc;
60  geometry_msgs::TransformStamped st;
61  st.header.frame_id = "foo";
62  st.header.stamp = ros::Time(1.0);
63  st.child_frame_id = "child";
64  st.transform.rotation.w = 0;
65  EXPECT_FALSE(tfc.setTransform(st, "authority1"));
66 
67 }
68 
69 TEST(tf2_lookupTransform, LookupException_Nothing_Exists)
70 {
71  tf2::BufferCore tfc;
72  EXPECT_THROW(tfc.lookupTransform("a", "b", ros::Time().fromSec(1.0)), tf2::LookupException);
73 
74 }
75 
76 TEST(tf2_canTransform, Nothing_Exists)
77 {
78  tf2::BufferCore tfc;
79  EXPECT_FALSE(tfc.canTransform("a", "b", ros::Time().fromSec(1.0)));
80 
81  std::string error_msg = std::string();
82  EXPECT_FALSE(tfc.canTransform("a", "b", ros::Time().fromSec(1.0), &error_msg));
83  ASSERT_STREQ(error_msg.c_str(), "canTransform: target_frame a does not exist. canTransform: source_frame b does not exist.");
84 
85 }
86 
87 TEST(tf2_lookupTransform, LookupException_One_Exists)
88 {
89  tf2::BufferCore tfc;
90  geometry_msgs::TransformStamped st;
91  st.header.frame_id = "foo";
92  st.header.stamp = ros::Time(1.0);
93  st.child_frame_id = "child";
94  st.transform.rotation.w = 1;
95  EXPECT_TRUE(tfc.setTransform(st, "authority1"));
96  EXPECT_THROW(tfc.lookupTransform("foo", "bar", ros::Time().fromSec(1.0)), tf2::LookupException);
97 
98 }
99 
100 TEST(tf2_canTransform, One_Exists)
101 {
102  tf2::BufferCore tfc;
103  geometry_msgs::TransformStamped st;
104  st.header.frame_id = "foo";
105  st.header.stamp = ros::Time(1.0);
106  st.child_frame_id = "child";
107  st.transform.rotation.w = 1;
108  EXPECT_TRUE(tfc.setTransform(st, "authority1"));
109  EXPECT_FALSE(tfc.canTransform("foo", "bar", ros::Time().fromSec(1.0)));
110 }
111 
112 TEST(tf2_chainAsVector, chain_v_configuration)
113 {
114  tf2::BufferCore mBC;
115  tf2::TestBufferCore tBC;
116 
117  geometry_msgs::TransformStamped st;
118  st.header.stamp = ros::Time(0);
119  st.transform.rotation.w = 1;
120 
121  st.header.frame_id = "a";
122  st.child_frame_id = "b";
123  mBC.setTransform(st, "authority1");
124 
125  st.header.frame_id = "b";
126  st.child_frame_id = "c";
127  mBC.setTransform(st, "authority1");
128 
129  st.header.frame_id = "a";
130  st.child_frame_id = "d";
131  mBC.setTransform(st, "authority1");
132 
133  st.header.frame_id = "d";
134  st.child_frame_id = "e";
135  mBC.setTransform(st, "authority1");
136 
137  std::vector<std::string> chain;
138 
139 
140  mBC._chainAsVector( "c", ros::Time(), "c", ros::Time(), "c", chain);
141  EXPECT_EQ( 0, chain.size());
142 
143  mBC._chainAsVector( "a", ros::Time(), "c", ros::Time(), "c", chain);
144  EXPECT_EQ( 3, chain.size());
145  if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "c" );
146  if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "b" );
147  if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "a" );
148 
149  mBC._chainAsVector( "c", ros::Time(), "a", ros::Time(), "c", chain);
150  EXPECT_EQ( 3, chain.size());
151  if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "a" );
152  if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "b" );
153  if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "c" );
154 
155  mBC._chainAsVector( "a", ros::Time(), "c", ros::Time(), "a", chain);
156  EXPECT_EQ( 3, chain.size());
157  if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "c" );
158  if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "b" );
159  if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "a" );
160 
161  mBC._chainAsVector( "c", ros::Time(), "a", ros::Time(), "a", chain);
162  EXPECT_EQ( 3, chain.size());
163  if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "a" );
164  if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "b" );
165  if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "c" );
166 
167  mBC._chainAsVector( "c", ros::Time(), "e", ros::Time(), "c", chain);
168 
169  EXPECT_EQ( 5, chain.size());
170  if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "e" );
171  if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "d" );
172  if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "a" );
173  if( chain.size() >= 4 ) EXPECT_EQ( chain[3], "b" );
174  if( chain.size() >= 5 ) EXPECT_EQ( chain[4], "c" );
175 
176  mBC._chainAsVector( "c", ros::Time(), "e", ros::Time(), "a", chain);
177 
178  EXPECT_EQ( 5, chain.size());
179  if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "e" );
180  if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "d" );
181  if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "a" );
182  if( chain.size() >= 4 ) EXPECT_EQ( chain[3], "b" );
183  if( chain.size() >= 5 ) EXPECT_EQ( chain[4], "c" );
184 
185  mBC._chainAsVector( "c", ros::Time(), "e", ros::Time(), "e", chain);
186 
187  EXPECT_EQ( 5, chain.size());
188  if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "e" );
189  if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "d" );
190  if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "a" );
191  if( chain.size() >= 4 ) EXPECT_EQ( chain[3], "b" );
192  if( chain.size() >= 5 ) EXPECT_EQ( chain[4], "c" );
193 }
194 
195 TEST(tf2_walkToTopParent, walk_i_configuration)
196 {
197  tf2::BufferCore mBC;
198  tf2::TestBufferCore tBC;
199 
200  geometry_msgs::TransformStamped st;
201  st.header.stamp = ros::Time(0);
202  st.transform.rotation.w = 1;
203 
204  st.header.frame_id = "a";
205  st.child_frame_id = "b";
206  mBC.setTransform(st, "authority1");
207 
208  st.header.frame_id = "b";
209  st.child_frame_id = "c";
210  mBC.setTransform(st, "authority1");
211 
212  st.header.frame_id = "c";
213  st.child_frame_id = "d";
214  mBC.setTransform(st, "authority1");
215 
216  st.header.frame_id = "d";
217  st.child_frame_id = "e";
218  mBC.setTransform(st, "authority1");
219 
220  std::vector<tf2::CompactFrameID> id_chain;
221  tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("a"), mBC._lookupFrameNumber("e"), 0, &id_chain);
222 
223  EXPECT_EQ(5, id_chain.size() );
224  if( id_chain.size() >= 1 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[0]));
225  if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1]));
226  if( id_chain.size() >= 3 ) EXPECT_EQ("c", tBC._lookupFrameString(mBC, id_chain[2]));
227  if( id_chain.size() >= 4 ) EXPECT_EQ("b", tBC._lookupFrameString(mBC, id_chain[3]));
228  if( id_chain.size() >= 5 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[4]));
229 
230  id_chain.clear();
231  tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("a"), 0, &id_chain);
232 
233  EXPECT_EQ(5, id_chain.size() );
234  if( id_chain.size() >= 1 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[0]));
235  if( id_chain.size() >= 2 ) EXPECT_EQ("b", tBC._lookupFrameString(mBC, id_chain[1]));
236  if( id_chain.size() >= 3 ) EXPECT_EQ("c", tBC._lookupFrameString(mBC, id_chain[2]));
237  if( id_chain.size() >= 4 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[3]));
238  if( id_chain.size() >= 5 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[4]));
239 
240 }
241 
242 TEST(tf2_walkToTopParent, walk_v_configuration)
243 {
244  tf2::BufferCore mBC;
245  tf2::TestBufferCore tBC;
246 
247  geometry_msgs::TransformStamped st;
248  st.header.stamp = ros::Time(0);
249  st.transform.rotation.w = 1;
250 
251  // st.header.frame_id = "aaa";
252  // st.child_frame_id = "aa";
253  // mBC.setTransform(st, "authority1");
254  //
255  // st.header.frame_id = "aa";
256  // st.child_frame_id = "a";
257  // mBC.setTransform(st, "authority1");
258 
259  st.header.frame_id = "a";
260  st.child_frame_id = "b";
261  mBC.setTransform(st, "authority1");
262 
263  st.header.frame_id = "b";
264  st.child_frame_id = "c";
265  mBC.setTransform(st, "authority1");
266 
267  st.header.frame_id = "a";
268  st.child_frame_id = "d";
269  mBC.setTransform(st, "authority1");
270 
271  st.header.frame_id = "d";
272  st.child_frame_id = "e";
273  mBC.setTransform(st, "authority1");
274 
275  std::vector<tf2::CompactFrameID> id_chain;
276  tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("c"), 0, &id_chain);
277 
278  EXPECT_EQ(5, id_chain.size());
279  if( id_chain.size() >= 1 ) EXPECT_EQ("c", tBC._lookupFrameString(mBC, id_chain[0]));
280  if( id_chain.size() >= 2 ) EXPECT_EQ("b", tBC._lookupFrameString(mBC, id_chain[1]));
281  if( id_chain.size() >= 3 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[2]));
282  if( id_chain.size() >= 4 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[3]));
283  if( id_chain.size() >= 5 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[4]));
284 
285  tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("c"), mBC._lookupFrameNumber("e"), 0, &id_chain);
286  EXPECT_EQ(5, id_chain.size());
287  if( id_chain.size() >= 1 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[0]));
288  if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1]));
289  if( id_chain.size() >= 3 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[2]));
290  if( id_chain.size() >= 4 ) EXPECT_EQ("b", tBC._lookupFrameString(mBC, id_chain[3]));
291  if( id_chain.size() >= 5 ) EXPECT_EQ("c", tBC._lookupFrameString(mBC, id_chain[4]));
292 
293  tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("a"), mBC._lookupFrameNumber("e"), 0, &id_chain);
294  EXPECT_EQ( id_chain.size(), 3 );
295  if( id_chain.size() >= 1 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[0]));
296  if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1]));
297  if( id_chain.size() >= 3 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[2]));
298 
299  tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("a"), 0, &id_chain);
300  EXPECT_EQ( id_chain.size(), 3 );
301  if( id_chain.size() >= 1 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[0]));
302  if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1]));
303  if( id_chain.size() >= 3 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[2]));
304 
305  tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("d"), 0, &id_chain);
306  EXPECT_EQ( id_chain.size(), 2 );
307  if( id_chain.size() >= 1 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[0]));
308  if( id_chain.size() >= 2 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[1]));
309 
310  tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("d"), mBC._lookupFrameNumber("e"), 0, &id_chain);
311  EXPECT_EQ( id_chain.size(), 2 );
312  if( id_chain.size() >= 1 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[0]));
313  if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1]));
314 }
315 
316 
317 int main(int argc, char **argv){
318  testing::InitGoogleTest(&argc, argv);
319  ros::Time::init(); //needed for ros::TIme::now()
320  return RUN_ALL_TESTS();
321 }
TEST
TEST(tf2, setTransformFail)
Definition: simple_tf2_core.cpp:37
tf2::BufferCore
A Class which provides coordinate transforms between any two frames in a system.
Definition: buffer_core.h:88
tf2::BufferCore::setTransform
bool setTransform(const geometry_msgs::TransformStamped &transform, const std::string &authority, bool is_static=false)
Add transform information to the tf data structure.
Definition: buffer_core.cpp:201
tf2::TestBufferCore::_lookupFrameString
const std::string & _lookupFrameString(BufferCore &buffer, CompactFrameID frame_id_num) const
Definition: buffer_core.h:426
buffer_core.h
TimeBase< Time, Duration >::fromSec
Time & fromSec(double t)
tf2::LookupException
An exception class to notify of bad frame number.
Definition: exceptions.h:70
tf2::BufferCore::_chainAsVector
void _chainAsVector(const std::string &target_frame, ros::Time target_time, const std::string &source_frame, ros::Time source_time, const std::string &fixed_frame, std::vector< std::string > &output) const
Backwards compatabilityA way to see what frames are in a chain Useful for debugging.
Definition: buffer_core.cpp:1577
ros::Time::init
static void init()
tf2::BufferCore::canTransform
bool canTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, std::string *error_msg=NULL) const
Test if a transform is possible.
Definition: buffer_core.cpp:799
tf2::BufferCore::_lookupFrameNumber
CompactFrameID _lookupFrameNumber(const std::string &frameid_str) const
Definition: buffer_core.h:278
main
int main(int argc, char **argv)
Definition: simple_tf2_core.cpp:317
tf2::TestBufferCore::_walkToTopParent
int _walkToTopParent(BufferCore &buffer, ros::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string *error_string, std::vector< CompactFrameID > *frame_chain) const
Definition: buffer_core.cpp:1650
tf2::BufferCore::lookupTransform
geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time) const
Get the transform between two frames by frame ID.
Definition: buffer_core.cpp:591

tf2
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Fri Oct 16 2020 19:08:50