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