buffer_core_test.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 <cmath>
31 #include <gtest/gtest.h>
32 #include <tf2/buffer_core.h>
33 #include "tf2/exceptions.h"
34 #include <ros/ros.h>
35 #include "LinearMath/btVector3.h"
36 #include "LinearMath/btTransform.h"
37 #include "rostest/permuter.h"
38 
39 void setIdentity(geometry_msgs::Transform& trans)
40 {
41  trans.translation.x = 0;
42  trans.translation.y = 0;
43  trans.translation.z = 0;
44  trans.rotation.x = 0;
45  trans.rotation.y = 0;
46  trans.rotation.z = 0;
47  trans.rotation.w = 1;
48 }
49 
50 
51 void push_back_i(std::vector<std::string>& children, std::vector<std::string>& parents,
52  std::vector<double>& dx, std::vector<double>& dy)
53 {
54  /*
55  "a"
56  v (1,0)
57  "b"
58  v (1,0)
59  "c"
60  */
61 
62  children.push_back("b");
63  parents.push_back("a");
64  dx.push_back(1.0);
65  dy.push_back(0.0);
66  children.push_back("c");
67  parents.push_back("b");
68  dx.push_back(1.0);
69  dy.push_back(0.0);
70 }
71 
72 
73 void push_back_y(std::vector<std::string>& children, std::vector<std::string>& parents,
74  std::vector<double>& dx, std::vector<double>& dy)
75 {
76  /*
77  "a"
78  v (1,0)
79  "b" ------(0,1)-----> "d"
80  v (1,0) v (0,1)
81  "c" "e"
82  */
83  // a>b
84  children.push_back("b");
85  parents.push_back("a");
86  dx.push_back(1.0);
87  dy.push_back(0.0);
88  // b>c
89  children.push_back("c");
90  parents.push_back("b");
91  dx.push_back(1.0);
92  dy.push_back(0.0);
93  // b>d
94  children.push_back("d");
95  parents.push_back("b");
96  dx.push_back(0.0);
97  dy.push_back(1.0);
98  // d>e
99  children.push_back("e");
100  parents.push_back("d");
101  dx.push_back(0.0);
102  dy.push_back(1.0);
103 }
104 
105 void push_back_v(std::vector<std::string>& children, std::vector<std::string>& parents,
106  std::vector<double>& dx, std::vector<double>& dy)
107 {
108  /*
109  "a" ------(0,1)-----> "f"
110  v (1,0) v (0,1)
111  "b" "g"
112  v (1,0)
113  "c"
114  */
115  // a>b
116  children.push_back("b");
117  parents.push_back("a");
118  dx.push_back(1.0);
119  dy.push_back(0.0);
120  // b>c
121  children.push_back("c");
122  parents.push_back("b");
123  dx.push_back(1.0);
124  dy.push_back(0.0);
125  // a>f
126  children.push_back("f");
127  parents.push_back("a");
128  dx.push_back(0.0);
129  dy.push_back(1.0);
130  // f>g
131  children.push_back("g");
132  parents.push_back("f");
133  dx.push_back(0.0);
134  dy.push_back(1.0);
135 
136 }
137 
138 void push_back_1(std::vector<std::string>& children, std::vector<std::string>& parents,
139  std::vector<double>& dx, std::vector<double>& dy)
140 {
141  children.push_back("2");
142  parents.push_back("1");
143  dx.push_back(1.0);
144  dy.push_back(0.0);
145 }
146 
147 void setupTree(tf2::BufferCore& mBC, const std::string& mode, const ros::Time & time, const ros::Duration& interpolation_space = ros::Duration())
148 {
149  ROS_DEBUG("Clearing Buffer Core for new test setup");
150  mBC.clear();
151 
152  ROS_DEBUG("Setting up test tree for formation %s", mode.c_str());
153 
154  std::vector<std::string> children;
155  std::vector<std::string> parents;
156  std::vector<double> dx, dy;
157 
158  if (mode == "i")
159  {
160  push_back_i(children, parents, dx, dy);
161  }
162  else if (mode == "y")
163  {
164  push_back_y(children, parents, dx, dy);
165  }
166 
167  else if (mode == "v")
168  {
169  push_back_v(children, parents, dx, dy);
170  }
171 
172  else if (mode == "ring_45")
173  {
174  /* Form a ring of transforms at every 45 degrees on the unit circle. */
175 
176  std::vector<std::string> frames;
177 
178 
179 
180  frames.push_back("a");
181  frames.push_back("b");
182  frames.push_back("c");
183  frames.push_back("d");
184  frames.push_back("e");
185  frames.push_back("f");
186  frames.push_back("g");
187  frames.push_back("h");
188  frames.push_back("i");
189 
190  for (uint8_t iteration = 0; iteration < 2; ++iteration)
191  {
192  double direction = 1;
193  std::string frame_prefix;
194  if (iteration == 0)
195  {
196  frame_prefix = "inverse_";
197  direction = -1;
198  }
199  else
200  frame_prefix ="";
201  for (uint64_t i = 1; i < frames.size(); i++)
202  {
203  geometry_msgs::TransformStamped ts;
204  setIdentity(ts.transform);
205  ts.transform.translation.x = direction * ( sqrt(2)/2 - 1);
206  ts.transform.translation.y = direction * sqrt(2)/2;
207  ts.transform.rotation.x = 0;
208  ts.transform.rotation.y = 0;
209  ts.transform.rotation.z = sin(direction * M_PI/8);
210  ts.transform.rotation.w = cos(direction * M_PI/8);
211  if (time > ros::Time() + (interpolation_space * .5))
212  ts.header.stamp = time - (interpolation_space * .5);
213  else
214  ts.header.stamp = ros::Time();
215 
216  ts.child_frame_id = frame_prefix + frames[i];
217  if (i > 1)
218  ts.header.frame_id = frame_prefix + frames[i-1];
219  else
220  ts.header.frame_id = frames[i-1];
221 
222  EXPECT_TRUE(mBC.setTransform(ts, "authority"));
223  if (interpolation_space > ros::Duration())
224  {
225  ts.header.stamp = time + interpolation_space * .5;
226  EXPECT_TRUE(mBC.setTransform(ts, "authority"));
227 
228  }
229  }
230  }
231  return; // nonstandard setup return before standard executinog
232  }
233  else if (mode == "1")
234  {
235  push_back_1(children, parents, dx, dy);
236 
237  }
238  else if (mode =="1_v")
239  {
240  push_back_1(children, parents, dx, dy);
241  push_back_v(children, parents, dx, dy);
242  }
243  else
244  EXPECT_FALSE("Undefined mode for tree setup. Test harness improperly setup.");
245 
246 
248  for (uint64_t i = 0; i < children.size(); i++)
249  {
250  geometry_msgs::TransformStamped ts;
251  setIdentity(ts.transform);
252  ts.transform.translation.x = dx[i];
253  ts.transform.translation.y = dy[i];
254  if (time > ros::Time() + (interpolation_space * .5))
255  ts.header.stamp = time - (interpolation_space * .5);
256  else
257  ts.header.stamp = ros::Time();
258 
259  ts.header.frame_id = parents[i];
260  ts.child_frame_id = children[i];
261  EXPECT_TRUE(mBC.setTransform(ts, "authority"));
262  if (interpolation_space > ros::Duration())
263  {
264  ts.header.stamp = time + interpolation_space * .5;
265  EXPECT_TRUE(mBC.setTransform(ts, "authority"));
266 
267  }
268  }
269 }
270 
271 
272 TEST(BufferCore_setTransform, NoInsertOnSelfTransform)
273 {
274  tf2::BufferCore mBC;
275  geometry_msgs::TransformStamped tranStamped;
276  setIdentity(tranStamped.transform);
277  tranStamped.header.stamp = ros::Time().fromNSec(10.0);
278  tranStamped.header.frame_id = "same_frame";
279  tranStamped.child_frame_id = "same_frame";
280  EXPECT_FALSE(mBC.setTransform(tranStamped, "authority"));
281 }
282 
283 TEST(BufferCore_setTransform, NoInsertWithNan)
284 {
285  tf2::BufferCore mBC;
286  geometry_msgs::TransformStamped tranStamped;
287  setIdentity(tranStamped.transform);
288  tranStamped.header.stamp = ros::Time().fromNSec(10.0);
289  tranStamped.header.frame_id = "same_frame";
290  tranStamped.child_frame_id = "other_frame";
291  EXPECT_TRUE(mBC.setTransform(tranStamped, "authority"));
292  tranStamped.transform.translation.x = std::nan("");
293  EXPECT_TRUE(std::isnan(tranStamped.transform.translation.x));
294  EXPECT_FALSE(mBC.setTransform(tranStamped, "authority"));
295 
296 }
297 
298 TEST(BufferCore_setTransform, NoInsertWithNoFrameID)
299 {
300  tf2::BufferCore mBC;
301  geometry_msgs::TransformStamped tranStamped;
302  setIdentity(tranStamped.transform);
303  tranStamped.header.stamp = ros::Time().fromNSec(10.0);
304  tranStamped.header.frame_id = "same_frame";
305  tranStamped.child_frame_id = "";
306  EXPECT_FALSE(mBC.setTransform(tranStamped, "authority"));
307  tranStamped.child_frame_id = "/";
308  EXPECT_FALSE(mBC.setTransform(tranStamped, "authority"));
309 
310 }
311 
312 TEST(BufferCore_setTransform, NoInsertWithNoParentID)
313 {
314  tf2::BufferCore mBC;
315  geometry_msgs::TransformStamped tranStamped;
316  setIdentity(tranStamped.transform);
317  tranStamped.header.stamp = ros::Time().fromNSec(10.0);
318  tranStamped.header.frame_id = "";
319  tranStamped.child_frame_id = "some_frame";
320  EXPECT_FALSE(mBC.setTransform(tranStamped, "authority"));
321 
322  tranStamped.header.frame_id = "/";
323  EXPECT_FALSE(mBC.setTransform(tranStamped, "authority"));
324 }
325 
326 /*
327 TEST(tf, ListOneInverse)
328 {
329  unsigned int runs = 4;
330  double epsilon = 1e-6;
331  seed_rand();
332 
333  tf::Transformer mTR(true);
334  std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
335  for ( uint64_t i = 0; i < runs ; i++ )
336  {
337  xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
338  yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
339  zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
340 
341  StampedTransform tranStamped (btTransform(btQuaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i), "my_parent", "child");
342  mTR.setTransform(tranStamped);
343  }
344 
345  // std::cout << mTR.allFramesAsString() << std::endl;
346  // std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl;
347 
348  for ( uint64_t i = 0; i < runs ; i++ )
349 
350  {
351  Stamped<Pose> inpose (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(10 + i), "child");
352 
353  try{
354  Stamped<Pose> outpose;
355  outpose.setIdentity(); //to make sure things are getting mutated
356  mTR.transformPose("my_parent",inpose, outpose);
357  EXPECT_NEAR(outpose.getOrigin().x(), xvalues[i], epsilon);
358  EXPECT_NEAR(outpose.getOrigin().y(), yvalues[i], epsilon);
359  EXPECT_NEAR(outpose.getOrigin().z(), zvalues[i], epsilon);
360  }
361  catch (tf::TransformException & ex)
362  {
363  std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl;
364  bool exception_improperly_thrown = true;
365  EXPECT_FALSE(exception_improperly_thrown);
366  }
367  }
368 
369 }
370 
371 TEST(tf, ListTwoInverse)
372 {
373  unsigned int runs = 4;
374  double epsilon = 1e-6;
375  seed_rand();
376 
377  tf::Transformer mTR(true);
378  std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
379  for ( unsigned int i = 0; i < runs ; i++ )
380  {
381  xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
382  yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
383  zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
384 
385  StampedTransform tranStamped(btTransform(btQuaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i), "my_parent", "child");
386  mTR.setTransform(tranStamped);
387  StampedTransform tranStamped2(btTransform(btQuaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i), "child", "grandchild");
388  mTR.setTransform(tranStamped2);
389  }
390 
391  // std::cout << mTR.allFramesAsString() << std::endl;
392  // std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl;
393 
394  for ( unsigned int i = 0; i < runs ; i++ )
395 
396  {
397  Stamped<Pose> inpose (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(10 + i), "grandchild");
398 
399  try{
400  Stamped<Pose> outpose;
401  outpose.setIdentity(); //to make sure things are getting mutated
402  mTR.transformPose("my_parent",inpose, outpose);
403  EXPECT_NEAR(outpose.getOrigin().x(), 2*xvalues[i], epsilon);
404  EXPECT_NEAR(outpose.getOrigin().y(), 2*yvalues[i], epsilon);
405  EXPECT_NEAR(outpose.getOrigin().z(), 2*zvalues[i], epsilon);
406  }
407  catch (tf::TransformException & ex)
408  {
409  std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl;
410  bool exception_improperly_thrown = true;
411  EXPECT_FALSE(exception_improperly_thrown);
412  }
413  }
414 
415 }
416 
417 
418 TEST(tf, ListOneForward)
419 {
420  unsigned int runs = 4;
421  double epsilon = 1e-6;
422  seed_rand();
423 
424  tf::Transformer mTR(true);
425  std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
426  for ( uint64_t i = 0; i < runs ; i++ )
427  {
428  xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
429  yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
430  zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
431 
432  StampedTransform tranStamped(btTransform(btQuaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i), "my_parent", "child");
433  mTR.setTransform(tranStamped);
434  }
435 
436  // std::cout << mTR.allFramesAsString() << std::endl;
437  // std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl;
438 
439  for ( uint64_t i = 0; i < runs ; i++ )
440 
441  {
442  Stamped<Pose> inpose (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(10 + i), "my_parent");
443 
444  try{
445  Stamped<Pose> outpose;
446  outpose.setIdentity(); //to make sure things are getting mutated
447  mTR.transformPose("child",inpose, outpose);
448  EXPECT_NEAR(outpose.getOrigin().x(), -xvalues[i], epsilon);
449  EXPECT_NEAR(outpose.getOrigin().y(), -yvalues[i], epsilon);
450  EXPECT_NEAR(outpose.getOrigin().z(), -zvalues[i], epsilon);
451  }
452  catch (tf::TransformException & ex)
453  {
454  std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl;
455  bool exception_improperly_thrown = true;
456  EXPECT_FALSE(exception_improperly_thrown);
457  }
458  }
459 
460 }
461 
462 TEST(tf, ListTwoForward)
463 {
464  unsigned int runs = 4;
465  double epsilon = 1e-6;
466  seed_rand();
467 
468  tf::Transformer mTR(true);
469  std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
470  for ( unsigned int i = 0; i < runs ; i++ )
471  {
472  xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
473  yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
474  zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
475 
476  StampedTransform tranStamped(btTransform(btQuaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i), "my_parent", "child");
477  mTR.setTransform(tranStamped);
478  StampedTransform tranStamped2(btTransform(btQuaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i), "child", "grandchild");
479  mTR.setTransform(tranStamped2);
480  }
481 
482  // std::cout << mTR.allFramesAsString() << std::endl;
483  // std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl;
484 
485  for ( unsigned int i = 0; i < runs ; i++ )
486 
487  {
488  Stamped<Pose> inpose (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(10 + i), "my_parent");
489 
490  try{
491  Stamped<Pose> outpose;
492  outpose.setIdentity(); //to make sure things are getting mutated
493  mTR.transformPose("grandchild",inpose, outpose);
494  EXPECT_NEAR(outpose.getOrigin().x(), -2*xvalues[i], epsilon);
495  EXPECT_NEAR(outpose.getOrigin().y(), -2*yvalues[i], epsilon);
496  EXPECT_NEAR(outpose.getOrigin().z(), -2*zvalues[i], epsilon);
497  }
498  catch (tf::TransformException & ex)
499  {
500  std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl;
501  bool exception_improperly_thrown = true;
502  EXPECT_FALSE(exception_improperly_thrown);
503  }
504  }
505 
506 }
507 
508 TEST(tf, TransformThrougRoot)
509 {
510  unsigned int runs = 4;
511  double epsilon = 1e-6;
512  seed_rand();
513 
514  tf::Transformer mTR(true);
515  std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
516  for ( unsigned int i = 0; i < runs ; i++ )
517  {
518  xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
519  yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
520  zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
521 
522  StampedTransform tranStamped(btTransform(btQuaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(1000 + i*100), "my_parent", "childA");
523  mTR.setTransform(tranStamped);
524  StampedTransform tranStamped2(btTransform(btQuaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(1000 + i*100), "my_parent", "childB");
525  mTR.setTransform(tranStamped2);
526  }
527 
528  // std::cout << mTR.allFramesAsString() << std::endl;
529  // std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl;
530 
531  for ( unsigned int i = 0; i < runs ; i++ )
532 
533  {
534  Stamped<Pose> inpose (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(1000 + i*100), "childA");
535 
536  try{
537  Stamped<Pose> outpose;
538  outpose.setIdentity(); //to make sure things are getting mutated
539  mTR.transformPose("childB",inpose, outpose);
540  EXPECT_NEAR(outpose.getOrigin().x(), 0*xvalues[i], epsilon);
541  EXPECT_NEAR(outpose.getOrigin().y(), 0*yvalues[i], epsilon);
542  EXPECT_NEAR(outpose.getOrigin().z(), 0*zvalues[i], epsilon);
543  }
544  catch (tf::TransformException & ex)
545  {
546  std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl;
547  bool exception_improperly_thrown = true;
548  EXPECT_FALSE(exception_improperly_thrown);
549  }
550  }
551 
552 }
553 
554 TEST(tf, TransformThroughNO_PARENT)
555 {
556  unsigned int runs = 4;
557  double epsilon = 1e-6;
558  seed_rand();
559 
560  tf::Transformer mTR(true);
561  std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
562  for ( unsigned int i = 0; i < runs ; i++ )
563  {
564  xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
565  yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
566  zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
567 
568  StampedTransform tranStamped(btTransform(btQuaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i), "my_parentA", "childA");
569  mTR.setTransform(tranStamped);
570  StampedTransform tranStamped2(btTransform(btQuaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i), "my_parentB", "childB");
571  mTR.setTransform(tranStamped2);
572  }
573 
574  // std::cout << mTR.allFramesAsString() << std::endl;
575  // std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl;
576 
577  for ( unsigned int i = 0; i < runs ; i++ )
578 
579  {
580  Stamped<btTransform> inpose (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(10 + i), "childA");
581  bool exception_thrown = false;
582 
583  try{
584  Stamped<btTransform> outpose;
585  outpose.setIdentity(); //to make sure things are getting mutated
586  mTR.transformPose("childB",inpose, outpose);
587  EXPECT_NEAR(outpose.getOrigin().x(), 0*xvalues[i], epsilon);
588  EXPECT_NEAR(outpose.getOrigin().y(), 0*yvalues[i], epsilon);
589  EXPECT_NEAR(outpose.getOrigin().z(), 0*zvalues[i], epsilon);
590  }
591  catch (tf::TransformException & ex)
592  {
593  exception_thrown = true;
594  }
595  EXPECT_TRUE(exception_thrown);
596  }
597 
598 }
599 
600 */
601 
602 
603 TEST(BufferCore_lookupTransform, i_configuration)
604 {
605  double epsilon = 1e-6;
606 
607 
608 
609  rostest::Permuter permuter;
610 
611  std::vector<ros::Time> times;
612  times.push_back(ros::Time(1.0));
613  times.push_back(ros::Time(10.0));
614  times.push_back(ros::Time(0.0));
615  ros::Time eval_time;
616  permuter.addOptionSet(times, &eval_time);
617 
618  std::vector<ros::Duration> durations;
619  durations.push_back(ros::Duration(1.0));
620  durations.push_back(ros::Duration(0.001));
621  durations.push_back(ros::Duration(0.1));
622  ros::Duration interpolation_space;
623  // permuter.addOptionSet(durations, &interpolation_space);
624 
625  std::vector<std::string> frames;
626  frames.push_back("a");
627  frames.push_back("b");
628  frames.push_back("c");
629  std::string source_frame;
630  permuter.addOptionSet(frames, &source_frame);
631 
632  std::string target_frame;
633  permuter.addOptionSet(frames, &target_frame);
634 
635  while (permuter.step())
636  {
637 
638  tf2::BufferCore mBC;
639  setupTree(mBC, "i", eval_time, interpolation_space);
640 
641  geometry_msgs::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, eval_time);
642  //printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec());
643  EXPECT_EQ(outpose.header.stamp, eval_time);
644  EXPECT_EQ(outpose.header.frame_id, source_frame);
645  EXPECT_EQ(outpose.child_frame_id, target_frame);
646  EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon);
647  EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
648  EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
649  EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
650  EXPECT_NEAR(outpose.transform.rotation.z, 0, epsilon);
651  EXPECT_NEAR(outpose.transform.rotation.w, 1, epsilon);
652 
653  //Zero distance
654  if (source_frame == target_frame)
655  {
656  EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon);
657  }
658  else if ((source_frame == "a" && target_frame =="b") ||
659  (source_frame == "b" && target_frame =="c"))
660  {
661  EXPECT_NEAR(outpose.transform.translation.x, 1, epsilon);
662  }
663  else if ((source_frame == "b" && target_frame =="a") ||
664  (source_frame == "c" && target_frame =="b"))
665  {
666  EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
667  }
668  else if (source_frame == "a" && target_frame =="c")
669  {
670  EXPECT_NEAR(outpose.transform.translation.x, 2, epsilon);
671  }
672  else if (source_frame == "c" && target_frame =="a")
673  {
674  EXPECT_NEAR(outpose.transform.translation.x, -2, epsilon);
675  }
676  else
677  {
678  EXPECT_FALSE("i configuration: Shouldn't get here");
679  printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec());
680  }
681 
682  }
683 }
684 
685 /* Check 1 result return false if test parameters unmet */
686 bool check_1_result(const geometry_msgs::TransformStamped& outpose, const std::string& source_frame, const std::string& target_frame, const ros::Time& eval_time, double epsilon)
687 {
688  //printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec());
689  EXPECT_EQ(outpose.header.stamp, eval_time);
690  EXPECT_EQ(outpose.header.frame_id, source_frame);
691  EXPECT_EQ(outpose.child_frame_id, target_frame);
692  EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon);
693  EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
694  EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
695  EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
696  EXPECT_NEAR(outpose.transform.rotation.z, 0, epsilon);
697  EXPECT_NEAR(outpose.transform.rotation.w, 1, epsilon);
698 
699  //Zero distance
700  if (source_frame == target_frame)
701  {
702  EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon);
703  }
704  else if (source_frame == "1" && target_frame =="2")
705  {
706  EXPECT_NEAR(outpose.transform.translation.x, 1, epsilon);
707  }
708  else if (source_frame == "2" && target_frame =="1")
709  {
710  EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
711  }
712  else
713  {
714  //printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec());
715  return false;
716  }
717  return true;
718 }
719 
720 /* Check v result return false if test parameters unmet */
721 bool check_v_result(const geometry_msgs::TransformStamped& outpose, const std::string& source_frame, const std::string& target_frame, const ros::Time& eval_time, double epsilon)
722 {
723  //printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec());
724  EXPECT_EQ(outpose.header.stamp, eval_time);
725  EXPECT_EQ(outpose.header.frame_id, source_frame);
726  EXPECT_EQ(outpose.child_frame_id, target_frame);
727  EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
728  EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
729  EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
730  EXPECT_NEAR(outpose.transform.rotation.z, 0, epsilon);
731  EXPECT_NEAR(outpose.transform.rotation.w, 1, epsilon);
732 
733  //Zero distance
734  if (source_frame == target_frame)
735  {
736  EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon);
737  }
738  else if ((source_frame == "a" && target_frame =="b") ||
739  (source_frame == "b" && target_frame =="c"))
740  {
741  EXPECT_NEAR(outpose.transform.translation.x, 1, epsilon);
742  EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon);
743  }
744  else if ((source_frame == "b" && target_frame =="a") ||
745  (source_frame == "c" && target_frame =="b"))
746  {
747  EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
748  EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon);
749  }
750  else if ((source_frame == "a" && target_frame =="f") ||
751  (source_frame == "f" && target_frame =="g"))
752  {
753  EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon);
754  EXPECT_NEAR(outpose.transform.translation.y, 1, epsilon);
755  }
756  else if ((source_frame == "f" && target_frame =="a") ||
757  (source_frame == "g" && target_frame =="f"))
758  {
759  EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon);
760  EXPECT_NEAR(outpose.transform.translation.y, -1, epsilon);
761  }
762  else if (source_frame == "a" && target_frame =="g")
763  {
764  EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon);
765  EXPECT_NEAR(outpose.transform.translation.y, 2, epsilon);
766  }
767  else if (source_frame == "g" && target_frame =="a")
768  {
769  EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon);
770  EXPECT_NEAR(outpose.transform.translation.y, -2, epsilon);
771  }
772  else if (source_frame == "a" && target_frame =="c")
773  {
774  EXPECT_NEAR(outpose.transform.translation.x, 2, epsilon);
775  EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon);
776  }
777  else if (source_frame == "c" && target_frame =="a")
778  {
779  EXPECT_NEAR(outpose.transform.translation.x, -2, epsilon);
780  EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon);
781  }
782  else if (source_frame == "b" && target_frame =="f")
783  {
784  EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
785  EXPECT_NEAR(outpose.transform.translation.y, 1, epsilon);
786  }
787  else if (source_frame == "f" && target_frame =="b")
788  {
789  EXPECT_NEAR(outpose.transform.translation.x, 1, epsilon);
790  EXPECT_NEAR(outpose.transform.translation.y, -1, epsilon);
791  }
792  else if (source_frame == "c" && target_frame =="f")
793  {
794  EXPECT_NEAR(outpose.transform.translation.x, -2, epsilon);
795  EXPECT_NEAR(outpose.transform.translation.y, 1, epsilon);
796  }
797  else if (source_frame == "f" && target_frame =="c")
798  {
799  EXPECT_NEAR(outpose.transform.translation.x, 2, epsilon);
800  EXPECT_NEAR(outpose.transform.translation.y, -1, epsilon);
801  }
802  else if (source_frame == "b" && target_frame =="g")
803  {
804  EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
805  EXPECT_NEAR(outpose.transform.translation.y, 2, epsilon);
806  }
807  else if (source_frame == "g" && target_frame =="b")
808  {
809  EXPECT_NEAR(outpose.transform.translation.x, 1, epsilon);
810  EXPECT_NEAR(outpose.transform.translation.y, -2, epsilon);
811  }
812  else if (source_frame == "c" && target_frame =="g")
813  {
814  EXPECT_NEAR(outpose.transform.translation.x, -2, epsilon);
815  EXPECT_NEAR(outpose.transform.translation.y, 2, epsilon);
816  }
817  else if (source_frame == "g" && target_frame =="c")
818  {
819  EXPECT_NEAR(outpose.transform.translation.x, 2, epsilon);
820  EXPECT_NEAR(outpose.transform.translation.y, -2, epsilon);
821  }
822  else
823  {
824  //printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec());
825  return false;
826  }
827  return true;
828 }
829 
830 /* Check v result return false if test parameters unmet */
831 bool check_y_result(const geometry_msgs::TransformStamped& outpose, const std::string& source_frame, const std::string& target_frame, const ros::Time& eval_time, double epsilon)
832 {
833  //printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec());
834  EXPECT_EQ(outpose.header.stamp, eval_time);
835  EXPECT_EQ(outpose.header.frame_id, source_frame);
836  EXPECT_EQ(outpose.child_frame_id, target_frame);
837  EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
838  EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
839  EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
840  EXPECT_NEAR(outpose.transform.rotation.z, 0, epsilon);
841  EXPECT_NEAR(outpose.transform.rotation.w, 1, epsilon);
842 
843  //Zero distance
844  if (source_frame == target_frame)
845  {
846  EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon);
847  }
848  else if ((source_frame == "a" && target_frame =="b") ||
849  (source_frame == "b" && target_frame =="c"))
850  {
851  EXPECT_NEAR(outpose.transform.translation.x, 1, epsilon);
852  EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon);
853  }
854  else if ((source_frame == "b" && target_frame =="a") ||
855  (source_frame == "c" && target_frame =="b"))
856  {
857  EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
858  EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon);
859  }
860  else if ((source_frame == "b" && target_frame =="d") ||
861  (source_frame == "d" && target_frame =="e"))
862  {
863  EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon);
864  EXPECT_NEAR(outpose.transform.translation.y, 1, epsilon);
865  }
866  else if ((source_frame == "d" && target_frame =="b") ||
867  (source_frame == "e" && target_frame =="d"))
868  {
869  EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon);
870  EXPECT_NEAR(outpose.transform.translation.y, -1, epsilon);
871  }
872  else if (source_frame == "b" && target_frame =="e")
873  {
874  EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon);
875  EXPECT_NEAR(outpose.transform.translation.y, 2, epsilon);
876  }
877  else if (source_frame == "e" && target_frame =="b")
878  {
879  EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon);
880  EXPECT_NEAR(outpose.transform.translation.y, -2, epsilon);
881  }
882  else if (source_frame == "a" && target_frame =="c")
883  {
884  EXPECT_NEAR(outpose.transform.translation.x, 2, epsilon);
885  EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon);
886  }
887  else if (source_frame == "c" && target_frame =="a")
888  {
889  EXPECT_NEAR(outpose.transform.translation.x, -2, epsilon);
890  EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon);
891  }
892  else if (source_frame == "a" && target_frame =="d")
893  {
894  EXPECT_NEAR(outpose.transform.translation.x, 1, epsilon);
895  EXPECT_NEAR(outpose.transform.translation.y, 1, epsilon);
896  }
897  else if (source_frame == "d" && target_frame =="a")
898  {
899  EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
900  EXPECT_NEAR(outpose.transform.translation.y, -1, epsilon);
901  }
902  else if (source_frame == "c" && target_frame =="d")
903  {
904  EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
905  EXPECT_NEAR(outpose.transform.translation.y, 1, epsilon);
906  }
907  else if (source_frame == "d" && target_frame =="c")
908  {
909  EXPECT_NEAR(outpose.transform.translation.x, 1, epsilon);
910  EXPECT_NEAR(outpose.transform.translation.y, -1, epsilon);
911  }
912  else if (source_frame == "a" && target_frame =="e")
913  {
914  EXPECT_NEAR(outpose.transform.translation.x, 1, epsilon);
915  EXPECT_NEAR(outpose.transform.translation.y, 2, epsilon);
916  }
917  else if (source_frame == "e" && target_frame =="a")
918  {
919  EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
920  EXPECT_NEAR(outpose.transform.translation.y, -2, epsilon);
921  }
922  else if (source_frame == "c" && target_frame =="e")
923  {
924  EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
925  EXPECT_NEAR(outpose.transform.translation.y, 2, epsilon);
926  }
927  else if (source_frame == "e" && target_frame =="c")
928  {
929  EXPECT_NEAR(outpose.transform.translation.x, 1, epsilon);
930  EXPECT_NEAR(outpose.transform.translation.y, -2, epsilon);
931  }
932  else
933  {
934  //printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec());
935  return false;
936  }
937  return true;
938 }
939 
940 
941 TEST(BufferCore_lookupTransform, one_link_configuration)
942 {
943  double epsilon = 1e-6;
944 
945 
946 
947  rostest::Permuter permuter;
948 
949  std::vector<ros::Time> times;
950  times.push_back(ros::Time(1.0));
951  times.push_back(ros::Time(10.0));
952  times.push_back(ros::Time(0.0));
953  ros::Time eval_time;
954  permuter.addOptionSet(times, &eval_time);
955 
956  std::vector<ros::Duration> durations;
957  durations.push_back(ros::Duration(1.0));
958  durations.push_back(ros::Duration(0.001));
959  durations.push_back(ros::Duration(0.1));
960  ros::Duration interpolation_space;
961  // permuter.addOptionSet(durations, &interpolation_space);
962 
963  std::vector<std::string> frames;
964  frames.push_back("1");
965  frames.push_back("2");
966  std::string source_frame;
967  permuter.addOptionSet(frames, &source_frame);
968 
969  std::string target_frame;
970  permuter.addOptionSet(frames, &target_frame);
971 
972  while (permuter.step())
973  {
974 
975  tf2::BufferCore mBC;
976  setupTree(mBC, "1", eval_time, interpolation_space);
977 
978  geometry_msgs::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, eval_time);
979 
980  EXPECT_TRUE(check_1_result(outpose, source_frame, target_frame, eval_time, epsilon));
981  }
982 }
983 
984 
985 TEST(BufferCore_lookupTransform, v_configuration)
986 {
987  double epsilon = 1e-6;
988 
989 
990 
991  rostest::Permuter permuter;
992 
993  std::vector<ros::Time> times;
994  times.push_back(ros::Time(1.0));
995  times.push_back(ros::Time(10.0));
996  times.push_back(ros::Time(0.0));
997  ros::Time eval_time;
998  permuter.addOptionSet(times, &eval_time);
999 
1000  std::vector<ros::Duration> durations;
1001  durations.push_back(ros::Duration(1.0));
1002  durations.push_back(ros::Duration(0.001));
1003  durations.push_back(ros::Duration(0.1));
1004  ros::Duration interpolation_space;
1005  // permuter.addOptionSet(durations, &interpolation_space);
1006 
1007  std::vector<std::string> frames;
1008  frames.push_back("a");
1009  frames.push_back("b");
1010  frames.push_back("c");
1011  frames.push_back("f");
1012  frames.push_back("g");
1013  std::string source_frame;
1014  permuter.addOptionSet(frames, &source_frame);
1015 
1016  std::string target_frame;
1017  permuter.addOptionSet(frames, &target_frame);
1018 
1019  while (permuter.step())
1020  {
1021 
1022  tf2::BufferCore mBC;
1023  setupTree(mBC, "v", eval_time, interpolation_space);
1024 
1025  geometry_msgs::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, eval_time);
1026 
1027  EXPECT_TRUE(check_v_result(outpose, source_frame, target_frame, eval_time, epsilon));
1028  }
1029 }
1030 
1031 
1032 TEST(BufferCore_lookupTransform, y_configuration)
1033 {
1034  double epsilon = 1e-6;
1035 
1036 
1037 
1038  rostest::Permuter permuter;
1039 
1040  std::vector<ros::Time> times;
1041  times.push_back(ros::Time(1.0));
1042  times.push_back(ros::Time(10.0));
1043  times.push_back(ros::Time(0.0));
1044  ros::Time eval_time;
1045  permuter.addOptionSet(times, &eval_time);
1046 
1047  std::vector<ros::Duration> durations;
1048  durations.push_back(ros::Duration(1.0));
1049  durations.push_back(ros::Duration(0.001));
1050  durations.push_back(ros::Duration(0.1));
1051  ros::Duration interpolation_space;
1052  // permuter.addOptionSet(durations, &interpolation_space);
1053 
1054  std::vector<std::string> frames;
1055  frames.push_back("a");
1056  frames.push_back("b");
1057  frames.push_back("c");
1058  frames.push_back("d");
1059  frames.push_back("e");
1060  std::string source_frame;
1061  permuter.addOptionSet(frames, &source_frame);
1062 
1063  std::string target_frame;
1064  permuter.addOptionSet(frames, &target_frame);
1065 
1066  while (permuter.step())
1067  {
1068 
1069  tf2::BufferCore mBC;
1070  setupTree(mBC, "y", eval_time, interpolation_space);
1071 
1072  geometry_msgs::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, eval_time);
1073 
1074  EXPECT_TRUE(check_y_result(outpose, source_frame, target_frame, eval_time, epsilon));
1075  }
1076 }
1077 
1078 TEST(BufferCore_lookupTransform, multi_configuration)
1079 {
1080  double epsilon = 1e-6;
1081 
1082 
1083 
1084  rostest::Permuter permuter;
1085 
1086  std::vector<ros::Time> times;
1087  times.push_back(ros::Time(1.0));
1088  times.push_back(ros::Time(10.0));
1089  times.push_back(ros::Time(0.0));
1090  ros::Time eval_time;
1091  permuter.addOptionSet(times, &eval_time);
1092 
1093  std::vector<ros::Duration> durations;
1094  durations.push_back(ros::Duration(1.0));
1095  durations.push_back(ros::Duration(0.001));
1096  durations.push_back(ros::Duration(0.1));
1097  ros::Duration interpolation_space;
1098  // permuter.addOptionSet(durations, &interpolation_space);
1099 
1100  std::vector<std::string> frames;
1101  frames.push_back("1");
1102  frames.push_back("2");
1103  frames.push_back("a");
1104  frames.push_back("b");
1105  frames.push_back("c");
1106  frames.push_back("f");
1107  frames.push_back("g");
1108  std::string source_frame;
1109  permuter.addOptionSet(frames, &source_frame);
1110 
1111  std::string target_frame;
1112  permuter.addOptionSet(frames, &target_frame);
1113 
1114  while (permuter.step())
1115  {
1116 
1117  tf2::BufferCore mBC;
1118  setupTree(mBC, "1_v", eval_time, interpolation_space);
1119 
1120  if (mBC.canTransform(source_frame, target_frame, eval_time))
1121  {
1122  geometry_msgs::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, eval_time);
1123 
1124  if ((source_frame == "1" || source_frame =="2") && (target_frame =="1" || target_frame == "2"))
1125  EXPECT_TRUE(check_1_result(outpose, source_frame, target_frame, eval_time, epsilon));
1126  else if ((source_frame == "a" || source_frame == "b" || source_frame == "c" || source_frame == "f" || source_frame == "g") &&
1127  (target_frame == "a" || target_frame == "b" || target_frame == "c" || target_frame == "f" || target_frame == "g"))
1128  EXPECT_TRUE(check_v_result(outpose, source_frame, target_frame, eval_time, epsilon));
1129  else
1130  EXPECT_FALSE("Frames unhandled");
1131  }
1132  else
1133  EXPECT_TRUE(((source_frame == "a" || source_frame =="b" || source_frame == "c" || source_frame == "f" || source_frame == "g") &&
1134  (target_frame == "1" || target_frame == "2") )
1135  ||
1136  ((target_frame == "a" || target_frame =="b" || target_frame == "c" || target_frame == "f" || target_frame == "g") &&
1137  (source_frame == "1" || source_frame == "2"))
1138  );
1139 
1140  }
1141 }
1142 
1143 #define CHECK_QUATERNION_NEAR(_q1, _x, _y, _z, _w, _epsilon) \
1144  { \
1145  btQuaternion q1(_q1.x, _q1.y, _q1.z, _q1.w); \
1146  btQuaternion q2(_x, _y, _z, _w); \
1147  double angle = q1.angle(q2); \
1148  EXPECT_TRUE(fabs(angle) < _epsilon || fabs(angle - M_PI) < _epsilon); \
1149  }
1150 
1151 #define CHECK_TRANSFORMS_NEAR(_out, _expected, _eps) \
1152  EXPECT_NEAR(_out.transform.translation.x, _expected.getOrigin().x(), epsilon); \
1153  EXPECT_NEAR(_out.transform.translation.y, _expected.getOrigin().y(), epsilon); \
1154  EXPECT_NEAR(_out.transform.translation.z, _expected.getOrigin().z(), epsilon); \
1155  CHECK_QUATERNION_NEAR(_out.transform.rotation, _expected.getRotation().x(), _expected.getRotation().y(), _expected.getRotation().z(), _expected.getRotation().w(), _eps);
1156 
1157 
1158 // Simple test with compound transform
1159 TEST(BufferCore_lookupTransform, compound_xfm_configuration)
1160 {
1161  /*
1162  * Frames
1163  *
1164  * root->a
1165  *
1166  * root->b->c->d
1167  *
1168  */
1169 
1170  double epsilon = 2e-5; // Larger epsilon for interpolation values
1171 
1172  tf2::BufferCore mBC;
1173 
1174  geometry_msgs::TransformStamped tsa;
1175  tsa.header.frame_id = "root";
1176  tsa.child_frame_id = "a";
1177  tsa.transform.translation.x = 1.0;
1178  tsa.transform.translation.y = 1.0;
1179  tsa.transform.translation.z = 1.0;
1180  btQuaternion q1;
1181  q1.setEuler(0.25, .5, .75);
1182  tsa.transform.rotation.x = q1.x();
1183  tsa.transform.rotation.y = q1.y();
1184  tsa.transform.rotation.z = q1.z();
1185  tsa.transform.rotation.w = q1.w();
1186  EXPECT_TRUE(mBC.setTransform(tsa, "authority"));
1187 
1188  geometry_msgs::TransformStamped tsb;
1189  tsb.header.frame_id = "root";
1190  tsb.child_frame_id = "b";
1191  tsb.transform.translation.x = -1.0;
1192  tsb.transform.translation.y = 0.0;
1193  tsb.transform.translation.z = -1.0;
1194  btQuaternion q2;
1195  q2.setEuler(1.0, 0.25, 0.5);
1196  tsb.transform.rotation.x = q2.x();
1197  tsb.transform.rotation.y = q2.y();
1198  tsb.transform.rotation.z = q2.z();
1199  tsb.transform.rotation.w = q2.w();
1200  EXPECT_TRUE(mBC.setTransform(tsb, "authority"));
1201 
1202  geometry_msgs::TransformStamped tsc;
1203  tsc.header.frame_id = "b";
1204  tsc.child_frame_id = "c";
1205  tsc.transform.translation.x = 0.0;
1206  tsc.transform.translation.y = 2.0;
1207  tsc.transform.translation.z = 0.5;
1208  btQuaternion q3;
1209  q3.setEuler(0.25, .75, 1.25);
1210  tsc.transform.rotation.x = q3.x();
1211  tsc.transform.rotation.y = q3.y();
1212  tsc.transform.rotation.z = q3.z();
1213  tsc.transform.rotation.w = q3.w();
1214  EXPECT_TRUE(mBC.setTransform(tsc, "authority"));
1215 
1216  geometry_msgs::TransformStamped tsd;
1217  tsd.header.frame_id = "c";
1218  tsd.child_frame_id = "d";
1219  tsd.transform.translation.x = 0.5;
1220  tsd.transform.translation.y = -1;
1221  tsd.transform.translation.z = 1.5;
1222  btQuaternion q4;
1223  q4.setEuler(-0.5, 1.0, -.75);
1224  tsd.transform.rotation.x = q4.x();
1225  tsd.transform.rotation.y = q4.y();
1226  tsd.transform.rotation.z = q4.z();
1227  tsd.transform.rotation.w = q4.w();
1228  EXPECT_TRUE(mBC.setTransform(tsd, "authority"));
1229 
1230  btTransform ta, tb, tc, td, expected_ab, expected_bc, expected_cb, expected_ac, expected_ba, expected_ca, expected_ad, expected_da, expected_bd, expected_db, expected_rootd, expected_rootc;
1231  ta.setOrigin(btVector3(1.0, 1.0, 1.0));
1232  ta.setRotation(q1);
1233  tb.setOrigin(btVector3(-1.0, 0.0, -1.0));
1234  tb.setRotation(q2);
1235  tc.setOrigin(btVector3(0.0, 2.0, 0.5));
1236  tc.setRotation(q3);
1237  td.setOrigin(btVector3(0.5, -1, 1.5));
1238  td.setRotation(q4);
1239 
1240 
1241  expected_ab = ta.inverse() * tb;
1242  expected_ac = ta.inverse() * tb * tc;
1243  expected_ad = ta.inverse() * tb * tc * td;
1244  expected_cb = tc.inverse();
1245  expected_bc = tc;
1246  expected_bd = tc * td;
1247  expected_db = expected_bd.inverse();
1248  expected_ba = tb.inverse() * ta;
1249  expected_ca = tc.inverse() * tb.inverse() * ta;
1250  expected_da = td.inverse() * tc.inverse() * tb.inverse() * ta;
1251  expected_rootd = tb * tc * td;
1252  expected_rootc = tb * tc;
1253 
1254  // root -> b -> c
1255  geometry_msgs::TransformStamped out_rootc = mBC.lookupTransform("root", "c", ros::Time());
1256  CHECK_TRANSFORMS_NEAR(out_rootc, expected_rootc, epsilon);
1257 
1258  // root -> b -> c -> d
1259  geometry_msgs::TransformStamped out_rootd = mBC.lookupTransform("root", "d", ros::Time());
1260  CHECK_TRANSFORMS_NEAR(out_rootd, expected_rootd, epsilon);
1261 
1262  // a <- root -> b
1263  geometry_msgs::TransformStamped out_ab = mBC.lookupTransform("a", "b", ros::Time());
1264  CHECK_TRANSFORMS_NEAR(out_ab, expected_ab, epsilon);
1265 
1266  geometry_msgs::TransformStamped out_ba = mBC.lookupTransform("b", "a", ros::Time());
1267  CHECK_TRANSFORMS_NEAR(out_ba, expected_ba, epsilon);
1268 
1269  // a <- root -> b -> c
1270  geometry_msgs::TransformStamped out_ac = mBC.lookupTransform("a", "c", ros::Time());
1271  CHECK_TRANSFORMS_NEAR(out_ac, expected_ac, epsilon);
1272 
1273  geometry_msgs::TransformStamped out_ca = mBC.lookupTransform("c", "a", ros::Time());
1274  CHECK_TRANSFORMS_NEAR(out_ca, expected_ca, epsilon);
1275 
1276  // a <- root -> b -> c -> d
1277  geometry_msgs::TransformStamped out_ad = mBC.lookupTransform("a", "d", ros::Time());
1278  CHECK_TRANSFORMS_NEAR(out_ad, expected_ad, epsilon);
1279 
1280  geometry_msgs::TransformStamped out_da = mBC.lookupTransform("d", "a", ros::Time());
1281  CHECK_TRANSFORMS_NEAR(out_da, expected_da, epsilon);
1282 
1283  // b -> c
1284  geometry_msgs::TransformStamped out_cb = mBC.lookupTransform("c", "b", ros::Time());
1285  CHECK_TRANSFORMS_NEAR(out_cb, expected_cb, epsilon);
1286 
1287  geometry_msgs::TransformStamped out_bc = mBC.lookupTransform("b", "c", ros::Time());
1288  CHECK_TRANSFORMS_NEAR(out_bc, expected_bc, epsilon);
1289 
1290  // b -> c -> d
1291  geometry_msgs::TransformStamped out_bd = mBC.lookupTransform("b", "d", ros::Time());
1292  CHECK_TRANSFORMS_NEAR(out_bd, expected_bd, epsilon);
1293 
1294  geometry_msgs::TransformStamped out_db = mBC.lookupTransform("d", "b", ros::Time());
1295  CHECK_TRANSFORMS_NEAR(out_db, expected_db, epsilon);
1296 }
1297 
1298 // Time varying transforms, testing interpolation
1299 TEST(BufferCore_lookupTransform, helix_configuration)
1300 {
1301  double epsilon = 2e-5; // Larger epsilon for interpolation values
1302 
1303  tf2::BufferCore mBC;
1304 
1305  ros::Time t0 = ros::Time() + ros::Duration(10);
1306  ros::Duration step = ros::Duration(0.05);
1307  ros::Duration half_step = ros::Duration(0.025);
1308  ros::Time t1 = t0 + ros::Duration(5.0);
1309 
1310  /*
1311  * a->b->c
1312  *
1313  * b.z = vel * (t - t0)
1314  * c.x = cos(theta * (t - t0))
1315  * c.y = sin(theta * (t - t0))
1316  *
1317  * a->d
1318  *
1319  * d.z = 2 * cos(theta * (t - t0))
1320  * a->d transforms are at half-step between a->b->c transforms
1321  */
1322 
1323  double theta = 0.25;
1324  double vel = 1.0;
1325 
1326  for (ros::Time t = t0; t <= t1; t += step)
1327  {
1328  ros::Time t2 = t + half_step;
1329  double dt = (t - t0).toSec();
1330  double dt2 = (t2 - t0).toSec();
1331 
1332  geometry_msgs::TransformStamped ts;
1333  ts.header.frame_id = "a";
1334  ts.header.stamp = t;
1335  ts.child_frame_id = "b";
1336  ts.transform.translation.z = vel * dt;
1337  ts.transform.rotation.w = 1.0;
1338  EXPECT_TRUE(mBC.setTransform(ts, "authority"));
1339 
1340  geometry_msgs::TransformStamped ts2;
1341  ts2.header.frame_id = "b";
1342  ts2.header.stamp = t;
1343  ts2.child_frame_id = "c";
1344  ts2.transform.translation.x = cos(theta * dt);
1345  ts2.transform.translation.y = sin(theta * dt);
1346  btQuaternion q;
1347  q.setEuler(0,0,theta*dt);
1348  ts2.transform.rotation.z = q.z();
1349  ts2.transform.rotation.w = q.w();
1350  EXPECT_TRUE(mBC.setTransform(ts2, "authority"));
1351 
1352  geometry_msgs::TransformStamped ts3;
1353  ts3.header.frame_id = "a";
1354  ts3.header.stamp = t2;
1355  ts3.child_frame_id = "d";
1356  ts3.transform.translation.z = cos(theta * dt2);
1357  ts3.transform.rotation.w = 1.0;
1358  EXPECT_TRUE(mBC.setTransform(ts3, "authority"));
1359  }
1360 
1361 
1362  for (ros::Time t = t0 + half_step; t < t1; t += step)
1363  {
1364  ros::Time t2 = t + half_step;
1365  double dt = (t - t0).toSec();
1366  double dt2 = (t2 - t0).toSec();
1367 
1368  geometry_msgs::TransformStamped out_ab = mBC.lookupTransform("a", "b", t);
1369  EXPECT_NEAR(out_ab.transform.translation.z, vel * dt, epsilon);
1370 
1371  geometry_msgs::TransformStamped out_ac = mBC.lookupTransform("a", "c", t);
1372  EXPECT_NEAR(out_ac.transform.translation.x, cos(theta * dt), epsilon);
1373  EXPECT_NEAR(out_ac.transform.translation.y, sin(theta * dt), epsilon);
1374  EXPECT_NEAR(out_ac.transform.translation.z, vel * dt, epsilon);
1375  btQuaternion q;
1376  q.setEuler(0,0,theta*dt);
1377  CHECK_QUATERNION_NEAR(out_ac.transform.rotation, 0, 0, q.z(), q.w(), epsilon);
1378 
1379  geometry_msgs::TransformStamped out_ad = mBC.lookupTransform("a", "d", t);
1380  EXPECT_NEAR(out_ad.transform.translation.z, cos(theta * dt), epsilon);
1381 
1382  geometry_msgs::TransformStamped out_cd = mBC.lookupTransform("c", "d", t2);
1383  EXPECT_NEAR(out_cd.transform.translation.x, -1, epsilon);
1384  EXPECT_NEAR(out_cd.transform.translation.y, 0, epsilon);
1385  EXPECT_NEAR(out_cd.transform.translation.z, cos(theta * dt2) - vel * dt2, epsilon);
1386  btQuaternion mq;
1387  mq.setEuler(0,0,-theta*dt2);
1388  CHECK_QUATERNION_NEAR(out_cd.transform.rotation, 0, 0, mq.z(), mq.w(), epsilon);
1389  }
1390 
1391  // Advanced API
1392  for (ros::Time t = t0 + half_step; t < t1; t += (step + step))
1393  {
1394  ros::Time t2 = t + step;
1395  double dt = (t - t0).toSec();
1396  double dt2 = (t2 - t0).toSec();
1397 
1398  geometry_msgs::TransformStamped out_cd2 = mBC.lookupTransform("c", t, "d", t2, "a");
1399  EXPECT_NEAR(out_cd2.transform.translation.x, -1, epsilon);
1400  EXPECT_NEAR(out_cd2.transform.translation.y, 0, epsilon);
1401  EXPECT_NEAR(out_cd2.transform.translation.z, cos(theta * dt2) - vel * dt, epsilon);
1402  btQuaternion mq2;
1403  mq2.setEuler(0,0,-theta*dt);
1404  CHECK_QUATERNION_NEAR(out_cd2.transform.rotation, 0, 0, mq2.z(), mq2.w(), epsilon);
1405  }
1406 }
1407 
1408 
1409 TEST(BufferCore_lookupTransform, ring_45_configuration)
1410 {
1411  double epsilon = 1e-6;
1412  rostest::Permuter permuter;
1413 
1414  std::vector<ros::Time> times;
1415  times.push_back(ros::Time(1.0));
1416  times.push_back(ros::Time(10.0));
1417  times.push_back(ros::Time(0.0));
1418  ros::Time eval_time;
1419  permuter.addOptionSet(times, &eval_time);
1420 
1421  std::vector<ros::Duration> durations;
1422  durations.push_back(ros::Duration(1.0));
1423  durations.push_back(ros::Duration(0.001));
1424  durations.push_back(ros::Duration(0.1));
1425  ros::Duration interpolation_space;
1426  // permuter.addOptionSet(durations, &interpolation_space);
1427 
1428  std::vector<std::string> frames;
1429  frames.push_back("a");
1430  frames.push_back("b");
1431  frames.push_back("c");
1432  frames.push_back("d");
1433  frames.push_back("e");
1434  frames.push_back("f");
1435  frames.push_back("g");
1436  frames.push_back("h");
1437  frames.push_back("i");
1438  /* frames.push_back("inverse_b");
1439  frames.push_back("inverse_c");
1440  frames.push_back("inverse_d");
1441  frames.push_back("inverse_e");
1442  frames.push_back("inverse_f");
1443  frames.push_back("inverse_g");
1444  frames.push_back("inverse_h");
1445  frames.push_back("inverse_i");*/
1446  std::string source_frame;
1447  permuter.addOptionSet(frames, &source_frame);
1448 
1449  std::string target_frame;
1450  permuter.addOptionSet(frames, &target_frame);
1451 
1452  while (permuter.step())
1453  {
1454 
1455  tf2::BufferCore mBC;
1456  setupTree(mBC, "ring_45", eval_time, interpolation_space);
1457 
1458  geometry_msgs::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, eval_time);
1459 
1460 
1461  //printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec());
1462  EXPECT_EQ(outpose.header.stamp, eval_time);
1463  EXPECT_EQ(outpose.header.frame_id, source_frame);
1464  EXPECT_EQ(outpose.child_frame_id, target_frame);
1465 
1466 
1467 
1468  //Zero distance or all the way
1469  if (source_frame == target_frame ||
1470  (source_frame == "a" && target_frame == "i") ||
1471  (source_frame == "i" && target_frame == "a") ||
1472  (source_frame == "a" && target_frame == "inverse_i") ||
1473  (source_frame == "inverse_i" && target_frame == "a") )
1474  {
1475  //printf ("here %s %s\n", source_frame.c_str(), target_frame.c_str());
1476  EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon);
1477  EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon);
1478  EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
1479  EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
1480  EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
1481  EXPECT_NEAR(outpose.transform.rotation.z, 0, epsilon);
1482  EXPECT_NEAR(fabs(outpose.transform.rotation.w), 1, epsilon);
1483  }
1484  // Chaining 1
1485  else if ((source_frame == "a" && target_frame =="b") ||
1486  (source_frame == "b" && target_frame =="c") ||
1487  (source_frame == "c" && target_frame =="d") ||
1488  (source_frame == "d" && target_frame =="e") ||
1489  (source_frame == "e" && target_frame =="f") ||
1490  (source_frame == "f" && target_frame =="g") ||
1491  (source_frame == "g" && target_frame =="h") ||
1492  (source_frame == "h" && target_frame =="i")
1493  )
1494  {
1495  EXPECT_NEAR(outpose.transform.translation.x, sqrt(2)/2 - 1, epsilon);
1496  EXPECT_NEAR(outpose.transform.translation.y, sqrt(2)/2 , epsilon);
1497  EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
1498  EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
1499  EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
1500  EXPECT_NEAR(outpose.transform.rotation.z, sin(M_PI/8), epsilon);
1501  EXPECT_NEAR(outpose.transform.rotation.w, cos(M_PI/8), epsilon);
1502  }
1503  // Inverse Chaining 1
1504  else if ((source_frame == "b" && target_frame =="a") ||
1505  (source_frame == "c" && target_frame =="b") ||
1506  (source_frame == "d" && target_frame =="c") ||
1507  (source_frame == "e" && target_frame =="d") ||
1508  (source_frame == "f" && target_frame =="e") ||
1509  (source_frame == "g" && target_frame =="f") ||
1510  (source_frame == "h" && target_frame =="g") ||
1511  (source_frame == "i" && target_frame =="h")
1512  )
1513  {
1514  EXPECT_NEAR(outpose.transform.translation.x, sqrt(2)/2 - 1, epsilon);
1515  EXPECT_NEAR(outpose.transform.translation.y, -sqrt(2)/2 , epsilon);
1516  EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
1517  EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
1518  EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
1519  EXPECT_NEAR(outpose.transform.rotation.z, sin(-M_PI/8), epsilon);
1520  EXPECT_NEAR(outpose.transform.rotation.w, cos(-M_PI/8), epsilon);
1521  }
1522  // Chaining 2
1523  else if ((source_frame == "a" && target_frame =="c") ||
1524  (source_frame == "b" && target_frame =="d") ||
1525  (source_frame == "c" && target_frame =="e") ||
1526  (source_frame == "d" && target_frame =="f") ||
1527  (source_frame == "e" && target_frame =="g") ||
1528  (source_frame == "f" && target_frame =="h") ||
1529  (source_frame == "g" && target_frame =="i")
1530  )
1531  {
1532  EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
1533  EXPECT_NEAR(outpose.transform.translation.y, 1 , epsilon);
1534  EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
1535  EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
1536  EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
1537  EXPECT_NEAR(outpose.transform.rotation.z, sin(M_PI/4), epsilon);
1538  EXPECT_NEAR(outpose.transform.rotation.w, cos(M_PI/4), epsilon);
1539  }
1540  // Inverse Chaining 2
1541  else if ((source_frame == "c" && target_frame =="a") ||
1542  (source_frame == "d" && target_frame =="b") ||
1543  (source_frame == "e" && target_frame =="c") ||
1544  (source_frame == "f" && target_frame =="d") ||
1545  (source_frame == "g" && target_frame =="e") ||
1546  (source_frame == "h" && target_frame =="f") ||
1547  (source_frame == "i" && target_frame =="g")
1548  )
1549  {
1550  EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
1551  EXPECT_NEAR(outpose.transform.translation.y, -1 , epsilon);
1552  EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
1553  EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
1554  EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
1555  EXPECT_NEAR(outpose.transform.rotation.z, sin(-M_PI/4), epsilon);
1556  EXPECT_NEAR(outpose.transform.rotation.w, cos(-M_PI/4), epsilon);
1557  }
1558  // Chaining 3
1559  else if ((source_frame == "a" && target_frame =="d") ||
1560  (source_frame == "b" && target_frame =="e") ||
1561  (source_frame == "c" && target_frame =="f") ||
1562  (source_frame == "d" && target_frame =="g") ||
1563  (source_frame == "e" && target_frame =="h") ||
1564  (source_frame == "f" && target_frame =="i")
1565  )
1566  {
1567  EXPECT_NEAR(outpose.transform.translation.x, -1 - sqrt(2)/2, epsilon);
1568  EXPECT_NEAR(outpose.transform.translation.y, sqrt(2)/2 , epsilon);
1569  EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
1570  EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
1571  EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
1572  EXPECT_NEAR(outpose.transform.rotation.z, sin(M_PI*3/8), epsilon);
1573  EXPECT_NEAR(outpose.transform.rotation.w, cos(M_PI*3/8), epsilon);
1574  }
1575  // Inverse Chaining 3
1576  else if ((target_frame == "a" && source_frame =="d") ||
1577  (target_frame == "b" && source_frame =="e") ||
1578  (target_frame == "c" && source_frame =="f") ||
1579  (target_frame == "d" && source_frame =="g") ||
1580  (target_frame == "e" && source_frame =="h") ||
1581  (target_frame == "f" && source_frame =="i")
1582  )
1583  {
1584  EXPECT_NEAR(outpose.transform.translation.x, -1 - sqrt(2)/2, epsilon);
1585  EXPECT_NEAR(outpose.transform.translation.y, - sqrt(2)/2 , epsilon);
1586  EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
1587  EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
1588  EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
1589  EXPECT_NEAR(outpose.transform.rotation.z, sin(-M_PI*3/8), epsilon);
1590  EXPECT_NEAR(outpose.transform.rotation.w, cos(-M_PI*3/8), epsilon);
1591  }
1592  // Chaining 4
1593  else if ((source_frame == "a" && target_frame =="e") ||
1594  (source_frame == "b" && target_frame =="f") ||
1595  (source_frame == "c" && target_frame =="g") ||
1596  (source_frame == "d" && target_frame =="h") ||
1597  (source_frame == "e" && target_frame =="i")
1598  )
1599  {
1600  EXPECT_NEAR(outpose.transform.translation.x, -2, epsilon);
1601  EXPECT_NEAR(outpose.transform.translation.y, 0 , epsilon);
1602  EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
1603  EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
1604  EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
1605  EXPECT_NEAR(outpose.transform.rotation.z, sin(M_PI/2), epsilon);
1606  EXPECT_NEAR(outpose.transform.rotation.w, cos(M_PI/2), epsilon);
1607  }
1608  // Inverse Chaining 4
1609  else if ((target_frame == "a" && source_frame =="e") ||
1610  (target_frame == "b" && source_frame =="f") ||
1611  (target_frame == "c" && source_frame =="g") ||
1612  (target_frame == "d" && source_frame =="h") ||
1613  (target_frame == "e" && source_frame =="i")
1614  )
1615  {
1616  EXPECT_NEAR(outpose.transform.translation.x, -2, epsilon);
1617  EXPECT_NEAR(outpose.transform.translation.y, 0 , epsilon);
1618  EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
1619  EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
1620  EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
1621  EXPECT_NEAR(outpose.transform.rotation.z, sin(-M_PI/2), epsilon);
1622  EXPECT_NEAR(outpose.transform.rotation.w, cos(-M_PI/2), epsilon);
1623  }
1624  // Chaining 5
1625  else if ((source_frame == "a" && target_frame =="f") ||
1626  (source_frame == "b" && target_frame =="g") ||
1627  (source_frame == "c" && target_frame =="h") ||
1628  (source_frame == "d" && target_frame =="i")
1629  )
1630  {
1631  EXPECT_NEAR(outpose.transform.translation.x, -1 - sqrt(2) /2, epsilon);
1632  EXPECT_NEAR(outpose.transform.translation.y, - sqrt(2) /2 , epsilon);
1633  EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
1634  EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
1635  EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
1636  EXPECT_NEAR(outpose.transform.rotation.z, sin(M_PI*5/8), epsilon);
1637  EXPECT_NEAR(outpose.transform.rotation.w, cos(M_PI*5/8), epsilon);
1638  }
1639  // Inverse Chaining 5
1640  else if ((target_frame == "a" && source_frame =="f") ||
1641  (target_frame == "b" && source_frame =="g") ||
1642  (target_frame == "c" && source_frame =="h") ||
1643  (target_frame == "d" && source_frame =="i")
1644  )
1645  {
1646  EXPECT_NEAR(outpose.transform.translation.x, -1 - sqrt(2)/2, epsilon);
1647  EXPECT_NEAR(outpose.transform.translation.y, sqrt(2)/2 , epsilon);
1648  EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
1649  EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
1650  EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
1651  EXPECT_NEAR(outpose.transform.rotation.z, sin(-M_PI*5/8), epsilon);
1652  EXPECT_NEAR(outpose.transform.rotation.w, cos(-M_PI*5/8), epsilon);
1653  }
1654  // Chaining 6
1655  else if ((source_frame == "a" && target_frame =="g") ||
1656  (source_frame == "b" && target_frame =="h") ||
1657  (source_frame == "c" && target_frame =="i")
1658  )
1659  {
1660  EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
1661  EXPECT_NEAR(outpose.transform.translation.y, -1 , epsilon);
1662  EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
1663  EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
1664  EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
1665  EXPECT_NEAR(outpose.transform.rotation.z, sin(M_PI*6/8), epsilon);
1666  EXPECT_NEAR(outpose.transform.rotation.w, cos(M_PI*6/8), epsilon);
1667  }
1668  // Inverse Chaining 6
1669  else if ((target_frame == "a" && source_frame =="g") ||
1670  (target_frame == "b" && source_frame =="h") ||
1671  (target_frame == "c" && source_frame =="i")
1672  )
1673  {
1674  EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
1675  EXPECT_NEAR(outpose.transform.translation.y, 1 , epsilon);
1676  EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
1677  EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
1678  EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
1679  EXPECT_NEAR(outpose.transform.rotation.z, sin(-M_PI*6/8), epsilon);
1680  EXPECT_NEAR(outpose.transform.rotation.w, cos(-M_PI*6/8), epsilon);
1681  }
1682  // Chaining 7
1683  else if ((source_frame == "a" && target_frame =="h") ||
1684  (source_frame == "b" && target_frame =="i")
1685  )
1686  {
1687  EXPECT_NEAR(outpose.transform.translation.x, sqrt(2)/2 - 1, epsilon);
1688  EXPECT_NEAR(outpose.transform.translation.y, -sqrt(2)/2 , epsilon);
1689  EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
1690  EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
1691  EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
1692  EXPECT_NEAR(outpose.transform.rotation.z, sin(M_PI*7/8), epsilon);
1693  EXPECT_NEAR(outpose.transform.rotation.w, cos(M_PI*7/8), epsilon);
1694  }
1695  // Inverse Chaining 7
1696  else if ((target_frame == "a" && source_frame =="h") ||
1697  (target_frame == "b" && source_frame =="i")
1698  )
1699  {
1700  EXPECT_NEAR(outpose.transform.translation.x, sqrt(2)/2 - 1, epsilon);
1701  EXPECT_NEAR(outpose.transform.translation.y, sqrt(2)/2 , epsilon);
1702  EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
1703  EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
1704  EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
1705  EXPECT_NEAR(outpose.transform.rotation.z, sin(-M_PI*7/8), epsilon);
1706  EXPECT_NEAR(outpose.transform.rotation.w, cos(-M_PI*7/8), epsilon);
1707  }
1708  else
1709  {
1710  EXPECT_FALSE("Ring_45 testing Shouldn't get here");
1711  printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec());
1712  }
1713 
1714  }
1715 }
1716 
1717 TEST(BufferCore_lookupTransform, invalid_arguments)
1718 {
1719  tf2::BufferCore mBC;
1720 
1721  setupTree(mBC, "i", ros::Time(1.0));
1722 
1723  EXPECT_NO_THROW(mBC.lookupTransform("b", "a", ros::Time()));
1724 
1725  //Empty frame_id
1726  EXPECT_THROW(mBC.lookupTransform("", "a", ros::Time()), tf2::InvalidArgumentException);
1727  EXPECT_THROW(mBC.lookupTransform("b", "", ros::Time()), tf2::InvalidArgumentException);
1728 
1729  //frame_id with /
1730  EXPECT_THROW(mBC.lookupTransform("/b", "a", ros::Time()), tf2::InvalidArgumentException);
1731  EXPECT_THROW(mBC.lookupTransform("b", "/a", ros::Time()), tf2::InvalidArgumentException);
1732 
1733 };
1734 
1735 TEST(BufferCore_canTransform, invalid_arguments)
1736 {
1737  tf2::BufferCore mBC;
1738 
1739  setupTree(mBC, "i", ros::Time(1.0));
1740 
1741  EXPECT_TRUE(mBC.canTransform("b", "a", ros::Time()));
1742 
1743 
1744  //Empty frame_id
1745  EXPECT_FALSE(mBC.canTransform("", "a", ros::Time()));
1746  EXPECT_FALSE(mBC.canTransform("b", "", ros::Time()));
1747 
1748  //frame_id with /
1749  EXPECT_FALSE(mBC.canTransform("/b", "a", ros::Time()));
1750  EXPECT_FALSE(mBC.canTransform("b", "/a", ros::Time()));
1751 
1752 };
1753 
1754 struct TransformableHelper
1755 {
1756  TransformableHelper()
1757  : called(false)
1758  {}
1759 
1760  void callback(tf2::TransformableRequestHandle request_handle, const std::string& target_frame, const std::string& source_frame,
1761  ros::Time time, tf2::TransformableResult result)
1762  {
1763  called = true;
1764  }
1765 
1766  bool called;
1767 };
1768 
1769 TEST(BufferCore_transformableCallbacks, alreadyTransformable)
1770 {
1771  tf2::BufferCore b;
1772  TransformableHelper h;
1773 
1774  geometry_msgs::TransformStamped t;
1775  t.header.stamp = ros::Time(1);
1776  t.header.frame_id = "a";
1777  t.child_frame_id = "b";
1778  t.transform.rotation.w = 1.0;
1779  b.setTransform(t, "me");
1780 
1781  tf2::TransformableCallbackHandle cb_handle = b.addTransformableCallback(boost::bind(&TransformableHelper::callback, &h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5));
1782  EXPECT_EQ(b.addTransformableRequest(cb_handle, "a", "b", ros::Time(1)), 0U);
1783 }
1784 
1785 TEST(BufferCore_transformableCallbacks, waitForNewTransform)
1786 {
1787  tf2::BufferCore b;
1788  TransformableHelper h;
1789  tf2::TransformableCallbackHandle cb_handle = b.addTransformableCallback(boost::bind(&TransformableHelper::callback, &h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5));
1790  EXPECT_GT(b.addTransformableRequest(cb_handle, "a", "b", ros::Time(10)), 0U);
1791 
1792  geometry_msgs::TransformStamped t;
1793  for (uint32_t i = 1; i <= 10; ++i)
1794  {
1795  t.header.stamp = ros::Time(i);
1796  t.header.frame_id = "a";
1797  t.child_frame_id = "b";
1798  t.transform.rotation.w = 1.0;
1799  b.setTransform(t, "me");
1800 
1801  if (i < 10)
1802  {
1803  ASSERT_FALSE(h.called);
1804  }
1805  else
1806  {
1807  ASSERT_TRUE(h.called);
1808  }
1809  }
1810 }
1811 
1812 TEST(BufferCore_transformableCallbacks, waitForOldTransform)
1813 {
1814  tf2::BufferCore b;
1815  TransformableHelper h;
1816  tf2::TransformableCallbackHandle cb_handle = b.addTransformableCallback(boost::bind(&TransformableHelper::callback, &h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5));
1817  EXPECT_GT(b.addTransformableRequest(cb_handle, "a", "b", ros::Time(1)), 0U);
1818 
1819  geometry_msgs::TransformStamped t;
1820  for (uint32_t i = 10; i > 0; --i)
1821  {
1822  t.header.stamp = ros::Time(i);
1823  t.header.frame_id = "a";
1824  t.child_frame_id = "b";
1825  t.transform.rotation.w = 1.0;
1826  b.setTransform(t, "me");
1827 
1828  if (i > 1)
1829  {
1830  ASSERT_FALSE(h.called);
1831  }
1832  else
1833  {
1834  ASSERT_TRUE(h.called);
1835  }
1836  }
1837 }
1838 
1839 /*
1840 TEST(tf, Exceptions)
1841 {
1842 
1843  tf::Transformer mTR(true);
1844 
1845 
1846  Stamped<btTransform> outpose;
1847 
1848  //connectivity when no data
1849  EXPECT_FALSE(mTR.canTransform("parent", "me", ros::Time().fromNSec(10000000)));
1850  try
1851  {
1852  mTR.transformPose("parent",Stamped<Pose>(btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(10000000) , "me"), outpose);
1853  EXPECT_FALSE("ConnectivityException Not Thrown");
1854  }
1855  catch ( tf::LookupException &ex)
1856  {
1857  EXPECT_TRUE("Lookupgh Exception Caught");
1858  }
1859  catch (tf::TransformException& ex)
1860  {
1861  printf("%s\n",ex.what());
1862  EXPECT_FALSE("Other Exception Caught");
1863  }
1864 
1865  mTR.setTransform( StampedTransform(btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(100000), "parent", "me"));
1866 
1867  //Extrapolation not valid with one value
1868  EXPECT_FALSE(mTR.canTransform("parent", "me", ros::Time().fromNSec(200000)));
1869  try
1870  {
1871  mTR.transformPose("parent",Stamped<Pose>(btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(200000) , "me"), outpose);
1872  EXPECT_TRUE("ExtrapolationException Not Thrown");
1873  }
1874  catch ( tf::ExtrapolationException &ex)
1875  {
1876  EXPECT_TRUE("Extrapolation Exception Caught");
1877  }
1878  catch (tf::TransformException& ex)
1879  {
1880  printf("%s\n",ex.what());
1881  EXPECT_FALSE("Other Exception Caught");
1882  }
1883 
1884 
1885  mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(300000), "parent", "me"));
1886 
1887  //NO Extration when Interpolating
1888  //inverse list
1889  EXPECT_TRUE(mTR.canTransform("parent", "me", ros::Time().fromNSec(200000)));
1890  try
1891  {
1892  mTR.transformPose("parent",Stamped<Pose>(btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(200000) , "me"), outpose);
1893  EXPECT_TRUE("ExtrapolationException Not Thrown");
1894  }
1895  catch ( tf::ExtrapolationException &ex)
1896  {
1897  EXPECT_FALSE("Extrapolation Exception Caught");
1898  }
1899  catch (tf::TransformException& ex)
1900  {
1901  printf("%s\n",ex.what());
1902  EXPECT_FALSE("Other Exception Caught");
1903  }
1904 
1905 
1906 
1907  //forward list
1908  EXPECT_TRUE(mTR.canTransform("me", "parent", ros::Time().fromNSec(200000)));
1909  try
1910  {
1911  mTR.transformPose("me",Stamped<Pose>(btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(200000) , "parent"), outpose);
1912  EXPECT_TRUE("ExtrapolationException Not Thrown");
1913  }
1914  catch ( tf::ExtrapolationException &ex)
1915  {
1916  EXPECT_FALSE("Extrapolation Exception Caught");
1917  }
1918  catch (tf::TransformException& ex)
1919  {
1920  printf("%s\n",ex.what());
1921  EXPECT_FALSE("Other Exception Caught");
1922  }
1923 
1924 
1925  //Extrapolating backwards
1926  //inverse list
1927  EXPECT_FALSE(mTR.canTransform("parent", "me", ros::Time().fromNSec(1000)));
1928  try
1929  {
1930  mTR.transformPose("parent",Stamped<Pose> (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(1000) , "me"), outpose);
1931  EXPECT_FALSE("ExtrapolationException Not Thrown");
1932  }
1933  catch ( tf::ExtrapolationException &ex)
1934  {
1935  EXPECT_TRUE("Extrapolation Exception Caught");
1936  }
1937  catch (tf::TransformException& ex)
1938  {
1939  printf("%s\n",ex.what());
1940  EXPECT_FALSE("Other Exception Caught");
1941  }
1942  //forwards list
1943  EXPECT_FALSE(mTR.canTransform("me", "parent", ros::Time().fromNSec(1000)));
1944  try
1945  {
1946  mTR.transformPose("me",Stamped<Pose> (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(1000) , "parent"), outpose);
1947  EXPECT_FALSE("ExtrapolationException Not Thrown");
1948  }
1949  catch ( tf::ExtrapolationException &ex)
1950  {
1951  EXPECT_TRUE("Extrapolation Exception Caught");
1952  }
1953  catch (tf::TransformException& ex)
1954  {
1955  printf("%s\n",ex.what());
1956  EXPECT_FALSE("Other Exception Caught");
1957  }
1958 
1959 
1960 
1961  // Test extrapolation inverse and forward linkages FORWARD
1962 
1963  //inverse list
1964  EXPECT_FALSE(mTR.canTransform("parent", "me", ros::Time().fromNSec(350000)));
1965  try
1966  {
1967  mTR.transformPose("parent", Stamped<Pose> (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(350000) , "me"), outpose);
1968  EXPECT_FALSE("ExtrapolationException Not Thrown");
1969  }
1970  catch ( tf::ExtrapolationException &ex)
1971  {
1972  EXPECT_TRUE("Extrapolation Exception Caught");
1973  }
1974  catch (tf::TransformException& ex)
1975  {
1976  printf("%s\n",ex.what());
1977  EXPECT_FALSE("Other Exception Caught");
1978  }
1979 
1980  //forward list
1981  EXPECT_FALSE(mTR.canTransform("parent", "me", ros::Time().fromNSec(350000)));
1982  try
1983  {
1984  mTR.transformPose("me", Stamped<Pose> (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(350000) , "parent"), outpose);
1985  EXPECT_FALSE("ExtrapolationException Not Thrown");
1986  }
1987  catch ( tf::ExtrapolationException &ex)
1988  {
1989  EXPECT_TRUE("Extrapolation Exception Caught");
1990  }
1991  catch (tf::TransformException& ex)
1992  {
1993  printf("%s\n",ex.what());
1994  EXPECT_FALSE("Other Exception Caught");
1995  }
1996 
1997 
1998 
1999 
2000 }
2001 
2002 
2003 
2004 TEST(tf, NoExtrapolationExceptionFromParent)
2005 {
2006  tf::Transformer mTR(true, ros::Duration().fromNSec(1000000));
2007 
2008 
2009 
2010  mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(1000), "parent", "a"));
2011  mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(10000), "parent", "a"));
2012 
2013 
2014  mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(1000), "parent", "b"));
2015  mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(10000), "parent", "b"));
2016 
2017  mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(1000), "parent's parent", "parent"));
2018  mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(1000), "parent's parent's parent", "parent's parent"));
2019 
2020  mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(10000), "parent's parent", "parent"));
2021  mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(10000), "parent's parent's parent", "parent's parent"));
2022 
2023  Stamped<Point> output;
2024 
2025  try
2026  {
2027  mTR.transformPoint( "b", Stamped<Point>(Point(1,1,1), ros::Time().fromNSec(2000), "a"), output);
2028  }
2029  catch (ExtrapolationException &ex)
2030  {
2031  EXPECT_FALSE("Shouldn't have gotten this exception");
2032  }
2033 
2034 
2035 
2036 };
2037 
2038 
2039 
2040 TEST(tf, ExtrapolationFromOneValue)
2041 {
2042  tf::Transformer mTR(true, ros::Duration().fromNSec(1000000));
2043 
2044 
2045 
2046  mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(1000), "parent", "a"));
2047 
2048  mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(1000), "parent's parent", "parent"));
2049 
2050 
2051  Stamped<Point> output;
2052 
2053  bool excepted = false;
2054  //Past time
2055  try
2056  {
2057  mTR.transformPoint( "parent", Stamped<Point>(Point(1,1,1), ros::Time().fromNSec(10), "a"), output);
2058  }
2059  catch (ExtrapolationException &ex)
2060  {
2061  excepted = true;
2062  }
2063 
2064  EXPECT_TRUE(excepted);
2065 
2066  excepted = false;
2067  //Future one element
2068  try
2069  {
2070  mTR.transformPoint( "parent", Stamped<Point>(Point(1,1,1), ros::Time().fromNSec(100000), "a"), output);
2071  }
2072  catch (ExtrapolationException &ex)
2073  {
2074  excepted = true;
2075  }
2076 
2077  EXPECT_TRUE(excepted);
2078 
2079  //Past multi link
2080  excepted = false;
2081  try
2082  {
2083  mTR.transformPoint( "parent's parent", Stamped<Point>(Point(1,1,1), ros::Time().fromNSec(1), "a"), output);
2084  }
2085  catch (ExtrapolationException &ex)
2086  {
2087  excepted = true;
2088  }
2089 
2090  EXPECT_TRUE(excepted);
2091 
2092  //Future case multi link
2093  excepted = false;
2094  try
2095  {
2096  mTR.transformPoint( "parent's parent", Stamped<Point>(Point(1,1,1), ros::Time().fromNSec(10000), "a"), output);
2097  }
2098  catch (ExtrapolationException &ex)
2099  {
2100  excepted = true;
2101  }
2102 
2103  EXPECT_TRUE(excepted);
2104 
2105  mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(20000), "parent", "a"));
2106 
2107  excepted = false;
2108  try
2109  {
2110  mTR.transformPoint( "parent", Stamped<Point>(Point(1,1,1), ros::Time().fromNSec(10000), "a"), output);
2111  }
2112  catch (ExtrapolationException &ex)
2113  {
2114  excepted = true;
2115  }
2116 
2117  EXPECT_FALSE(excepted);
2118 
2119 };
2120 
2121 
2122 
2123 TEST(tf, getLatestCommonTime)
2124 {
2125  tf::Transformer mTR(true);
2126  mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(1000), "parent", "a"));
2127  mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(2000), "parent's parent", "parent"));
2128 
2129  //simple case
2130  ros::Time t;
2131  mTR.getLatestCommonTime("a", "parent's parent", t, NULL);
2132  EXPECT_EQ(t, ros::Time().fromNSec(1000));
2133 
2134  //no connection
2135  EXPECT_EQ(tf::LOOKUP_ERROR, mTR.getLatestCommonTime("a", "not valid", t, NULL));
2136  EXPECT_EQ(t, ros::Time());
2137 
2138  //testing with update
2139  mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(3000), "parent", "a"));
2140  mTR.getLatestCommonTime("a", "parent's parent",t, NULL);
2141  EXPECT_EQ(t, ros::Time().fromNSec(2000));
2142 
2143  //longer chain
2144  mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(4000), "parent", "b"));
2145  mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(3000), "b", "c"));
2146  mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(9000), "c", "d"));
2147  mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(5000), "f", "e"));
2148 
2149  //shared parent
2150  mTR.getLatestCommonTime("a", "b",t, NULL);
2151  EXPECT_EQ(t, ros::Time().fromNSec(3000));
2152 
2153  //two degrees
2154  mTR.getLatestCommonTime("a", "c", t, NULL);
2155  EXPECT_EQ(t, ros::Time().fromNSec(3000));
2156  //reversed
2157  mTR.getLatestCommonTime("c", "a", t, NULL);
2158  EXPECT_EQ(t, ros::Time().fromNSec(3000));
2159 
2160  //three degrees
2161  mTR.getLatestCommonTime("a", "d", t, NULL);
2162  EXPECT_EQ(t, ros::Time().fromNSec(3000));
2163  //reversed
2164  mTR.getLatestCommonTime("d", "a", t, NULL);
2165  EXPECT_EQ(t, ros::Time().fromNSec(3000));
2166 
2167  //disconnected tree
2168  mTR.getLatestCommonTime("e", "f", t, NULL);
2169  EXPECT_EQ(t, ros::Time().fromNSec(5000));
2170  //reversed order
2171  mTR.getLatestCommonTime("f", "e", t, NULL);
2172  EXPECT_EQ(t, ros::Time().fromNSec(5000));
2173 
2174 
2175  mTR.setExtrapolationLimit(ros::Duration().fromNSec(20000));
2176 
2177  //check timestamps resulting
2178  tf::Stamped<tf::Point> output, output2;
2179  try
2180  {
2181  mTR.transformPoint( "parent", Stamped<Point>(Point(1,1,1), ros::Time(), "b"), output);
2182  mTR.transformPoint( "a", ros::Time(),Stamped<Point>(Point(1,1,1), ros::Time(), "b"), "c", output2);
2183  }
2184  catch (tf::TransformException &ex)
2185  {
2186  printf("%s\n", ex.what());
2187  EXPECT_FALSE("Shouldn't get this Exception");
2188  }
2189 
2190  EXPECT_EQ(output.stamp_, ros::Time().fromNSec(4000));
2191  EXPECT_EQ(output2.stamp_, ros::Time().fromNSec(3000));
2192 
2193 
2194  //zero length lookup zero time
2195  ros::Time now1 = ros::Time::now();
2196  ros::Time time_output;
2197  mTR.getLatestCommonTime("a", "a", time_output, NULL);
2198  EXPECT_LE(now1.toSec(), time_output.toSec());
2199  EXPECT_LE(time_output.toSec(), ros::Time::now().toSec());
2200 
2201 
2202 }
2203 
2204 TEST(tf, RepeatedTimes)
2205 {
2206  Transformer mTR;
2207  mTR.setTransform( StampedTransform (btTransform(btQuaternion(1,0,0), btVector3(0,0,0)), ros::Time().fromNSec(4000), "parent", "b"));
2208  mTR.setTransform( StampedTransform (btTransform(btQuaternion(1,1,0), btVector3(0,0,0)), ros::Time().fromNSec(4000), "parent", "b"));
2209 
2210  tf::StampedTransform output;
2211  try{
2212  mTR.lookupTransform("parent", "b" , ros::Time().fromNSec(4000), output);
2213  EXPECT_TRUE(!std::isnan(output.getOrigin().x()));
2214  EXPECT_TRUE(!std::isnan(output.getOrigin().y()));
2215  EXPECT_TRUE(!std::isnan(output.getOrigin().z()));
2216  EXPECT_TRUE(!std::isnan(output.getRotation().x()));
2217  EXPECT_TRUE(!std::isnan(output.getRotation().y()));
2218  EXPECT_TRUE(!std::isnan(output.getRotation().z()));
2219  EXPECT_TRUE(!std::isnan(output.getRotation().w()));
2220  }
2221  catch (...)
2222  {
2223  EXPECT_FALSE("Excetion improperly thrown");
2224  }
2225 
2226 
2227 }
2228 
2229 TEST(tf, frameExists)
2230 {
2231  Transformer mTR;
2232 
2233  // test with fully qualified name
2234  EXPECT_FALSE(mTR.frameExists("/b"));;
2235  EXPECT_FALSE(mTR.frameExists("/parent"));
2236  EXPECT_FALSE(mTR.frameExists("/other"));
2237  EXPECT_FALSE(mTR.frameExists("/frame"));
2238 
2239  //test with resolveping
2240  EXPECT_FALSE(mTR.frameExists("b"));;
2241  EXPECT_FALSE(mTR.frameExists("parent"));
2242  EXPECT_FALSE(mTR.frameExists("other"));
2243  EXPECT_FALSE(mTR.frameExists("frame"));
2244 
2245  mTR.setTransform( StampedTransform (btTransform(btQuaternion(1,0,0), btVector3(0,0,0)), ros::Time().fromNSec(4000), "/parent", "/b"));
2246 
2247  // test with fully qualified name
2248  EXPECT_TRUE(mTR.frameExists("/b"));
2249  EXPECT_TRUE(mTR.frameExists("/parent"));
2250  EXPECT_FALSE(mTR.frameExists("/other"));
2251  EXPECT_FALSE(mTR.frameExists("/frame"));
2252 
2253  //Test with resolveping
2254  EXPECT_TRUE(mTR.frameExists("b"));
2255  EXPECT_TRUE(mTR.frameExists("parent"));
2256  EXPECT_FALSE(mTR.frameExists("other"));
2257  EXPECT_FALSE(mTR.frameExists("frame"));
2258 
2259  mTR.setTransform( StampedTransform (btTransform(btQuaternion(1,1,0), btVector3(0,0,0)), ros::Time().fromNSec(4000), "/frame", "/other"));
2260 
2261  // test with fully qualified name
2262  EXPECT_TRUE(mTR.frameExists("/b"));
2263  EXPECT_TRUE(mTR.frameExists("/parent"));
2264  EXPECT_TRUE(mTR.frameExists("/other"));
2265  EXPECT_TRUE(mTR.frameExists("/frame"));
2266 
2267  //Test with resolveping
2268  EXPECT_TRUE(mTR.frameExists("b"));
2269  EXPECT_TRUE(mTR.frameExists("parent"));
2270  EXPECT_TRUE(mTR.frameExists("other"));
2271  EXPECT_TRUE(mTR.frameExists("frame"));
2272 
2273 }
2274 
2275 TEST(tf, resolve)
2276 {
2277  //no prefix
2278  EXPECT_STREQ("/id", tf::resolve("","id").c_str());
2279  //prefix w/o /
2280  EXPECT_STREQ("/asdf/id", tf::resolve("asdf","id").c_str());
2281  //prefix w /
2282  EXPECT_STREQ("/asdf/id", tf::resolve("/asdf","id").c_str());
2283  // frame_id w / -> no prefix
2284  EXPECT_STREQ("/id", tf::resolve("asdf","/id").c_str());
2285  // frame_id w / -> no prefix
2286  EXPECT_STREQ("/id", tf::resolve("/asdf","/id").c_str());
2287 
2288 }
2289 
2290 TEST(tf, canTransform)
2291 {
2292  Transformer mTR;
2293 
2294  //confirm zero length list disconnected will return true
2295  EXPECT_TRUE(mTR.canTransform("some_frame","some_frame", ros::Time()));
2296  EXPECT_TRUE(mTR.canTransform("some_frame","some_frame", ros::Time::now()));
2297 
2298  //Create a two link tree between times 10 and 20
2299  for (int i = 10; i < 20; i++)
2300  {
2301  mTR.setTransform( StampedTransform (btTransform(btQuaternion(1,0,0), btVector3(0,0,0)), ros::Time().fromSec(i), "parent", "child"));
2302  mTR.setTransform( StampedTransform (btTransform(btQuaternion(1,0,0), btVector3(0,0,0)), ros::Time().fromSec(i), "parent", "other_child"));
2303  }
2304 
2305  // four different timestamps related to tf state
2306  ros::Time zero_time = ros::Time().fromSec(0);
2307  ros::Time old_time = ros::Time().fromSec(5);
2308  ros::Time valid_time = ros::Time().fromSec(15);
2309  ros::Time future_time = ros::Time().fromSec(25);
2310 
2311 
2312  //confirm zero length list disconnected will return true
2313  EXPECT_TRUE(mTR.canTransform("some_frame","some_frame", zero_time));
2314  EXPECT_TRUE(mTR.canTransform("some_frame","some_frame", old_time));
2315  EXPECT_TRUE(mTR.canTransform("some_frame","some_frame", valid_time));
2316  EXPECT_TRUE(mTR.canTransform("some_frame","some_frame", future_time));
2317 
2318  // Basic API Tests
2319 
2320  //Valid data should pass
2321  EXPECT_TRUE(mTR.canTransform("child", "parent", valid_time));
2322  EXPECT_TRUE(mTR.canTransform("child", "other_child", valid_time));
2323 
2324  //zero data should pass
2325  EXPECT_TRUE(mTR.canTransform("child", "parent", zero_time));
2326  EXPECT_TRUE(mTR.canTransform("child", "other_child", zero_time));
2327 
2328  //Old data should fail
2329  EXPECT_FALSE(mTR.canTransform("child", "parent", old_time));
2330  EXPECT_FALSE(mTR.canTransform("child", "other_child", old_time));
2331 
2332  //Future data should fail
2333  EXPECT_FALSE(mTR.canTransform("child", "parent", future_time));
2334  EXPECT_FALSE(mTR.canTransform("child", "other_child", future_time));
2335 
2336  //Same Frame should pass for all times
2337  EXPECT_TRUE(mTR.canTransform("child", "child", zero_time));
2338  EXPECT_TRUE(mTR.canTransform("child", "child", old_time));
2339  EXPECT_TRUE(mTR.canTransform("child", "child", valid_time));
2340  EXPECT_TRUE(mTR.canTransform("child", "child", future_time));
2341 
2342  // Advanced API Tests
2343 
2344  // Source = Fixed
2345  //zero data in fixed frame should pass
2346  EXPECT_TRUE(mTR.canTransform("child", zero_time, "parent", valid_time, "child"));
2347  EXPECT_TRUE(mTR.canTransform("child", zero_time, "other_child", valid_time, "child"));
2348  //Old data in fixed frame should pass
2349  EXPECT_TRUE(mTR.canTransform("child", old_time, "parent", valid_time, "child"));
2350  EXPECT_TRUE(mTR.canTransform("child", old_time, "other_child", valid_time, "child"));
2351  //valid data in fixed frame should pass
2352  EXPECT_TRUE(mTR.canTransform("child", valid_time, "parent", valid_time, "child"));
2353  EXPECT_TRUE(mTR.canTransform("child", valid_time, "other_child", valid_time, "child"));
2354  //future data in fixed frame should pass
2355  EXPECT_TRUE(mTR.canTransform("child", future_time, "parent", valid_time, "child"));
2356  EXPECT_TRUE(mTR.canTransform("child", future_time, "other_child", valid_time, "child"));
2357 
2358  //transforming through fixed into the past
2359  EXPECT_FALSE(mTR.canTransform("child", valid_time, "parent", old_time, "child"));
2360  EXPECT_FALSE(mTR.canTransform("child", valid_time, "other_child", old_time, "child"));
2361  //transforming through fixed into the future
2362  EXPECT_FALSE(mTR.canTransform("child", valid_time, "parent", future_time, "child"));
2363  EXPECT_FALSE(mTR.canTransform("child", valid_time, "other_child", future_time, "child"));
2364 
2365  // Target = Fixed
2366  //zero data in fixed frame should pass
2367  EXPECT_TRUE(mTR.canTransform("child", zero_time, "parent", valid_time, "parent"));
2368  //Old data in fixed frame should pass
2369  EXPECT_FALSE(mTR.canTransform("child", old_time, "parent", valid_time, "parent"));
2370  //valid data in fixed frame should pass
2371  EXPECT_TRUE(mTR.canTransform("child", valid_time, "parent", valid_time, "parent"));
2372  //future data in fixed frame should pass
2373  EXPECT_FALSE(mTR.canTransform("child", future_time, "parent", valid_time, "parent"));
2374 
2375  //transforming through fixed into the zero
2376  EXPECT_TRUE(mTR.canTransform("child", valid_time, "parent", zero_time, "parent"));
2377  //transforming through fixed into the past
2378  EXPECT_TRUE(mTR.canTransform("child", valid_time, "parent", old_time, "parent"));
2379  //transforming through fixed into the valid
2380  EXPECT_TRUE(mTR.canTransform("child", valid_time, "parent", valid_time, "parent"));
2381  //transforming through fixed into the future
2382  EXPECT_TRUE(mTR.canTransform("child", valid_time, "parent", future_time, "parent"));
2383 
2384 }
2385 
2386 TEST(tf, lookupTransform)
2387 {
2388  Transformer mTR;
2389  //Create a two link tree between times 10 and 20
2390  for (int i = 10; i < 20; i++)
2391  {
2392  mTR.setTransform( StampedTransform (btTransform(btQuaternion(1,0,0), btVector3(0,0,0)), ros::Time().fromSec(i), "parent", "child"));
2393  mTR.setTransform( StampedTransform (btTransform(btQuaternion(1,0,0), btVector3(0,0,0)), ros::Time().fromSec(i), "parent", "other_child"));
2394  }
2395 
2396  // four different timestamps related to tf state
2397  ros::Time zero_time = ros::Time().fromSec(0);
2398  ros::Time old_time = ros::Time().fromSec(5);
2399  ros::Time valid_time = ros::Time().fromSec(15);
2400  ros::Time future_time = ros::Time().fromSec(25);
2401 
2402  //output
2403  tf::StampedTransform output;
2404 
2405  // Basic API Tests
2406 
2407  try
2408  {
2409  //confirm zero length list disconnected will return true
2410  mTR.lookupTransform("some_frame","some_frame", zero_time, output);
2411  mTR.lookupTransform("some_frame","some_frame", old_time, output);
2412  mTR.lookupTransform("some_frame","some_frame", valid_time, output);
2413  mTR.lookupTransform("some_frame","some_frame", future_time, output);
2414  mTR.lookupTransform("child","child", future_time, output);
2415  mTR.lookupTransform("other_child","other_child", future_time, output);
2416 
2417  //Valid data should pass
2418  mTR.lookupTransform("child", "parent", valid_time, output);
2419  mTR.lookupTransform("child", "other_child", valid_time, output);
2420 
2421  //zero data should pass
2422  mTR.lookupTransform("child", "parent", zero_time, output);
2423  mTR.lookupTransform("child", "other_child", zero_time, output);
2424  }
2425  catch (tf::TransformException &ex)
2426  {
2427  printf("Exception improperly thrown: %s", ex.what());
2428  EXPECT_FALSE("Exception thrown");
2429  }
2430  try
2431  {
2432  //Old data should fail
2433  mTR.lookupTransform("child", "parent", old_time, output);
2434  EXPECT_FALSE("Exception should have been thrown");
2435  }
2436  catch (tf::TransformException)
2437  {
2438  EXPECT_TRUE("Exception Thrown Correctly");
2439  }
2440  try {
2441  //Future data should fail
2442  mTR.lookupTransform("child", "parent", future_time, output);
2443  EXPECT_FALSE("Exception should have been thrown");
2444  }
2445  catch (tf::TransformException)
2446  {
2447  EXPECT_TRUE("Exception Thrown Correctly");
2448  }
2449 
2450  try {
2451  //Same Frame should pass for all times
2452  mTR.lookupTransform("child", "child", zero_time, output);
2453  mTR.lookupTransform("child", "child", old_time, output);
2454  mTR.lookupTransform("child", "child", valid_time, output);
2455  mTR.lookupTransform("child", "child", future_time, output);
2456 
2457  // Advanced API Tests
2458 
2459  // Source = Fixed
2460  //zero data in fixed frame should pass
2461  mTR.lookupTransform("child", zero_time, "parent", valid_time, "child", output);
2462  mTR.lookupTransform("child", zero_time, "other_child", valid_time, "child", output);
2463  //Old data in fixed frame should pass
2464  mTR.lookupTransform("child", old_time, "parent", valid_time, "child", output);
2465  mTR.lookupTransform("child", old_time, "other_child", valid_time, "child", output);
2466  //valid data in fixed frame should pass
2467  mTR.lookupTransform("child", valid_time, "parent", valid_time, "child", output);
2468  mTR.lookupTransform("child", valid_time, "other_child", valid_time, "child", output);
2469  //future data in fixed frame should pass
2470  mTR.lookupTransform("child", future_time, "parent", valid_time, "child", output);
2471  mTR.lookupTransform("child", future_time, "other_child", valid_time, "child", output);
2472  }
2473  catch (tf::TransformException &ex)
2474  {
2475  printf("Exception improperly thrown: %s", ex.what());
2476  EXPECT_FALSE("Exception incorrectly thrown");
2477  }
2478 
2479  try {
2480  //transforming through fixed into the past
2481  mTR.lookupTransform("child", valid_time, "parent", old_time, "child", output);
2482  EXPECT_FALSE("Exception should have been thrown");
2483  }
2484  catch (tf::TransformException)
2485  {
2486  EXPECT_TRUE("Exception Thrown Correctly");
2487  }
2488 
2489  try {
2490  //transforming through fixed into the future
2491  mTR.lookupTransform("child", valid_time, "parent", future_time, "child", output);
2492  EXPECT_FALSE("Exception should have been thrown");
2493  }
2494  catch (tf::TransformException)
2495  {
2496  EXPECT_TRUE("Exception Thrown Correctly");
2497  }
2498 
2499  try {
2500  // Target = Fixed
2501  //zero data in fixed frame should pass
2502  mTR.lookupTransform("child", zero_time, "parent", valid_time, "parent", output);
2503  //valid data in fixed frame should pass
2504  mTR.lookupTransform("child", valid_time, "parent", valid_time, "parent", output);
2505  }
2506  catch (tf::TransformException &ex)
2507  {
2508  printf("Exception improperly thrown: %s", ex.what());
2509  EXPECT_FALSE("Exception incorrectly thrown");
2510  }
2511 
2512  try {
2513  //Old data in fixed frame should pass
2514  mTR.lookupTransform("child", old_time, "parent", valid_time, "parent", output);
2515  EXPECT_FALSE("Exception should have been thrown");
2516  }
2517  catch (tf::TransformException)
2518  {
2519  EXPECT_TRUE("Exception Thrown Correctly");
2520  }
2521  try {
2522  //future data in fixed frame should pass
2523  mTR.lookupTransform("child", future_time, "parent", valid_time, "parent", output);
2524  EXPECT_FALSE("Exception should have been thrown");
2525  }
2526  catch (tf::TransformException)
2527  {
2528  EXPECT_TRUE("Exception Thrown Correctly");
2529  }
2530 
2531  try {
2532  //transforming through fixed into the zero
2533  mTR.lookupTransform("child", valid_time, "parent", zero_time, "parent", output);
2534  //transforming through fixed into the past
2535  mTR.lookupTransform("child", valid_time, "parent", old_time, "parent", output);
2536  //transforming through fixed into the valid
2537  mTR.lookupTransform("child", valid_time, "parent", valid_time, "parent", output);
2538  //transforming through fixed into the future
2539  mTR.lookupTransform("child", valid_time, "parent", future_time, "parent", output);
2540  }
2541  catch (tf::TransformException &ex)
2542  {
2543  printf("Exception improperly thrown: %s", ex.what());
2544  EXPECT_FALSE("Exception improperly thrown");
2545  }
2546 
2547 
2548  //make sure zero goes to now for zero length
2549  try
2550  {
2551  ros::Time now1 = ros::Time::now();
2552 
2553  mTR.lookupTransform("a", "a", ros::Time(),output);
2554  EXPECT_LE(now1.toSec(), output.stamp_.toSec());
2555  EXPECT_LE(output.stamp_.toSec(), ros::Time::now().toSec());
2556  }
2557  catch (tf::TransformException &ex)
2558  {
2559  printf("Exception improperly thrown: %s", ex.what());
2560  EXPECT_FALSE("Exception improperly thrown");
2561  }
2562 
2563 }
2564 
2565 
2566 TEST(tf, getFrameStrings)
2567 {
2568  Transformer mTR;
2569 
2570 
2571  mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(4000), "/parent", "/b"));
2572  std::vector <std::string> frames_string;
2573  mTR.getFrameStrings(frames_string);
2574  ASSERT_EQ(frames_string.size(), (unsigned)2);
2575  EXPECT_STREQ(frames_string[0].c_str(), std::string("/b").c_str());
2576  EXPECT_STREQ(frames_string[1].c_str(), std::string("/parent").c_str());
2577 
2578 
2579  mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(4000), "/frame", "/other"));
2580 
2581  mTR.getFrameStrings(frames_string);
2582  ASSERT_EQ(frames_string.size(), (unsigned)4);
2583  EXPECT_STREQ(frames_string[0].c_str(), std::string("/b").c_str());
2584  EXPECT_STREQ(frames_string[1].c_str(), std::string("/parent").c_str());
2585  EXPECT_STREQ(frames_string[2].c_str(), std::string("/other").c_str());
2586  EXPECT_STREQ(frames_string[3].c_str(), std::string("/frame").c_str());
2587 
2588 }
2589 
2590 bool expectInvalidQuaternion(tf::Quaternion q)
2591 {
2592  try
2593  {
2594  tf::assertQuaternionValid(q);
2595  printf("this should have thrown\n");
2596  return false;
2597  }
2598  catch (tf::InvalidArgument &ex)
2599  {
2600  return true;
2601  }
2602  catch (...)
2603  {
2604  printf("A different type of exception was expected\n");
2605  return false;
2606  }
2607  return false;
2608 }
2609 
2610 bool expectValidQuaternion(tf::Quaternion q)
2611 {
2612  try
2613  {
2614  tf::assertQuaternionValid(q);
2615  }
2616  catch (tf::TransformException &ex)
2617  {
2618  return false;
2619  }
2620  return true;
2621 }
2622 
2623 bool expectInvalidQuaternion(geometry_msgs::Quaternion q)
2624 {
2625  try
2626  {
2627  tf::assertQuaternionValid(q);
2628  printf("this should have thrown\n");
2629  return false;
2630  }
2631  catch (tf::InvalidArgument &ex)
2632  {
2633  return true;
2634  }
2635  catch (...)
2636  {
2637  printf("A different type of exception was expected\n");
2638  return false;
2639  }
2640  return false;
2641 }
2642 
2643 bool expectValidQuaternion(geometry_msgs::Quaternion q)
2644 {
2645  try
2646  {
2647  tf::assertQuaternionValid(q);
2648  }
2649  catch (tf::TransformException &ex)
2650  {
2651  return false;
2652  }
2653  return true;
2654 }
2655 
2656 
2657 TEST(tf, assertQuaternionValid)
2658 {
2659  tf::Quaternion q(1,0,0,0);
2660  EXPECT_TRUE(expectValidQuaternion(q));
2661  q.setX(0);
2662  EXPECT_TRUE(expectInvalidQuaternion(q));
2663  q.setY(1);
2664  EXPECT_TRUE(expectValidQuaternion(q));
2665  q.setZ(1);
2666  EXPECT_TRUE(expectInvalidQuaternion(q));
2667  q.setY(0);
2668  EXPECT_TRUE(expectValidQuaternion(q));
2669  q.setW(1);
2670  EXPECT_TRUE(expectInvalidQuaternion(q));
2671  q.setZ(0);
2672  EXPECT_TRUE(expectValidQuaternion(q));
2673  q.setZ(sqrt(2.0)/2.0);
2674  EXPECT_TRUE(expectInvalidQuaternion(q));
2675  q.setW(sqrt(2.0)/2.0);
2676  EXPECT_TRUE(expectValidQuaternion(q));
2677 
2678  q.setZ(sqrt(2.0)/2.0 + 0.01);
2679  EXPECT_TRUE(expectInvalidQuaternion(q));
2680 
2681  q.setZ(sqrt(2.0)/2.0 - 0.01);
2682  EXPECT_TRUE(expectInvalidQuaternion(q));
2683 
2684  EXPECT_THROW(tf::assertQuaternionValid(q), tf::InvalidArgument);
2685  // Waiting for gtest 1.1 or later
2686  // EXPECT_NO_THROW(tf::assertQuaternionValid(q));
2687  //q.setX(0);
2688  //EXPECT_THROW(tf::assertQuaternionValid(q), tf::InvalidArgument);
2689  //q.setY(1);
2690  //EXPECT_NO_THROW(tf::assertQuaternionValid(q));
2691 
2692 }
2693 TEST(tf, assertQuaternionMsgValid)
2694 {
2695  geometry_msgs::Quaternion q;
2696  q.x = 1;//others zeroed to start
2697 
2698  EXPECT_TRUE(expectValidQuaternion(q));
2699  q.x = 0;
2700  EXPECT_TRUE(expectInvalidQuaternion(q));
2701  q.y = 1;
2702  EXPECT_TRUE(expectValidQuaternion(q));
2703  q.z = 1;
2704  EXPECT_TRUE(expectInvalidQuaternion(q));
2705  q.y = 0;
2706  EXPECT_TRUE(expectValidQuaternion(q));
2707  q.w = 1;
2708  EXPECT_TRUE(expectInvalidQuaternion(q));
2709  q.z = 0;
2710  EXPECT_TRUE(expectValidQuaternion(q));
2711  q.z = sqrt(2.0)/2.0;
2712  EXPECT_TRUE(expectInvalidQuaternion(q));
2713  q.w = sqrt(2.0)/2.0;
2714  EXPECT_TRUE(expectValidQuaternion(q));
2715 
2716  q.z = sqrt(2.0)/2.0 + 0.01;
2717  EXPECT_TRUE(expectInvalidQuaternion(q));
2718 
2719  q.z = sqrt(2.0)/2.0 - 0.01;
2720  EXPECT_TRUE(expectInvalidQuaternion(q));
2721 
2722 
2723  // Waiting for gtest 1.1 or later
2724  // EXPECT_NO_THROW(tf::assertQuaternionValid(q));
2725  //q.x = 0);
2726  //EXPECT_THROW(tf::assertQuaternionValid(q), tf::InvalidArgument);
2727  //q.y = 1);
2728  //EXPECT_NO_THROW(tf::assertQuaternionValid(q));
2729 
2730 }
2731 
2732 
2733 TEST(tf2_stamped, OperatorEqualEqual)
2734 {
2735  btTransform transform0, transform1, transform0a;
2736  transform0.setIdentity();
2737  transform0a.setIdentity();
2738  transform1.setIdentity();
2739  transform1.setOrigin(btVector3(1, 0, 0));
2740  tf2::StampedTransform stamped_transform_reference(transform0a, ros::Time(), "frame_id", "child_frame_id");
2741  tf2::StampedTransform stamped_transform0A(transform0, ros::Time(), "frame_id", "child_frame_id");
2742  EXPECT_TRUE(stamped_transform0A == stamped_transform_reference); // Equal
2743  tf2::StampedTransform stamped_transform0B(transform0, ros::Time(), "frame_id_not_equal", "child_frame_id");
2744  EXPECT_FALSE(stamped_transform0B == stamped_transform_reference); // Different Frame id
2745  tf2::StampedTransform stamped_transform0C(transform0, ros::Time(1.0), "frame_id", "child_frame_id");
2746  EXPECT_FALSE(stamped_transform0C == stamped_transform_reference); // Different Time
2747  tf2::StampedTransform stamped_transform0D(transform0, ros::Time(1.0), "frame_id_not_equal", "child_frame_id");
2748  EXPECT_FALSE(stamped_transform0D == stamped_transform_reference); // Different frame id and time
2749  tf2::StampedTransform stamped_transform0E(transform1, ros::Time(), "frame_id_not_equal", "child_frame_id");
2750  EXPECT_FALSE(stamped_transform0E == stamped_transform_reference); // Different transform, frame id
2751  tf2::StampedTransform stamped_transform0F(transform1, ros::Time(1.0), "frame_id", "child_frame_id");
2752  EXPECT_FALSE(stamped_transform0F == stamped_transform_reference); // Different transform, time
2753  tf2::StampedTransform stamped_transform0G(transform1, ros::Time(1.0), "frame_id_not_equal", "child_frame_id");
2754  EXPECT_FALSE(stamped_transform0G == stamped_transform_reference); // Different transform, frame id and time
2755  tf2::StampedTransform stamped_transform0H(transform1, ros::Time(), "frame_id", "child_frame_id");
2756  EXPECT_FALSE(stamped_transform0H == stamped_transform_reference); // Different transform
2757 
2758 
2759  //Different child_frame_id
2760  tf2::StampedTransform stamped_transform1A(transform0, ros::Time(), "frame_id", "child_frame_id2");
2761  EXPECT_FALSE(stamped_transform1A == stamped_transform_reference); // Equal
2762  tf2::StampedTransform stamped_transform1B(transform0, ros::Time(), "frame_id_not_equal", "child_frame_id2");
2763  EXPECT_FALSE(stamped_transform1B == stamped_transform_reference); // Different Frame id
2764  tf2::StampedTransform stamped_transform1C(transform0, ros::Time(1.0), "frame_id", "child_frame_id2");
2765  EXPECT_FALSE(stamped_transform1C == stamped_transform_reference); // Different Time
2766  tf2::StampedTransform stamped_transform1D(transform0, ros::Time(1.0), "frame_id_not_equal", "child_frame_id2");
2767  EXPECT_FALSE(stamped_transform1D == stamped_transform_reference); // Different frame id and time
2768  tf2::StampedTransform stamped_transform1E(transform1, ros::Time(), "frame_id_not_equal", "child_frame_id2");
2769  EXPECT_FALSE(stamped_transform1E == stamped_transform_reference); // Different transform, frame id
2770  tf2::StampedTransform stamped_transform1F(transform1, ros::Time(1.0), "frame_id", "child_frame_id2");
2771  EXPECT_FALSE(stamped_transform1F == stamped_transform_reference); // Different transform, time
2772  tf2::StampedTransform stamped_transform1G(transform1, ros::Time(1.0), "frame_id_not_equal", "child_frame_id2");
2773  EXPECT_FALSE(stamped_transform1G == stamped_transform_reference); // Different transform, frame id and time
2774  tf2::StampedTransform stamped_transform1H(transform1, ros::Time(), "frame_id", "child_frame_id2");
2775  EXPECT_FALSE(stamped_transform1H == stamped_transform_reference); // Different transform
2776 
2777 }
2778 
2779 TEST(tf2_stamped, OperatorEqual)
2780 {
2781  btTransform pose0, pose1, pose0a;
2782  pose0.setIdentity();
2783  pose1.setIdentity();
2784  pose1.setOrigin(btVector3(1, 0, 0));
2785  tf2::Stamped<btTransform> stamped_pose0(pose0, ros::Time(), "frame_id");
2786  tf2::Stamped<btTransform> stamped_pose1(pose1, ros::Time(1.0), "frame_id_not_equal");
2787  EXPECT_FALSE(stamped_pose1 == stamped_pose0);
2788  stamped_pose1 = stamped_pose0;
2789  EXPECT_TRUE(stamped_pose1 == stamped_pose0);
2790 
2791 }
2792  */
2793 int main(int argc, char **argv){
2794  testing::InitGoogleTest(&argc, argv);
2795  ros::Time::init(); //needed for ros::TIme::now()
2796  ros::init(argc, argv, "tf_unittest");
2797  return RUN_ALL_TESTS();
2798 }
push_back_1
void push_back_1(std::vector< std::string > &children, std::vector< std::string > &parents, std::vector< double > &dx, std::vector< double > &dy)
Definition: buffer_core_test.cpp:138
epsilon
double epsilon
Definition: test_utils.cpp:21
push_back_v
void push_back_v(std::vector< std::string > &children, std::vector< std::string > &parents, std::vector< double > &dx, std::vector< double > &dy)
Definition: buffer_core_test.cpp:105
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
check_v_result
bool check_v_result(const geometry_msgs::TransformStamped &outpose, const std::string &source_frame, const std::string &target_frame, const ros::Time &eval_time, double epsilon)
Definition: buffer_core_test.cpp:721
ros.h
tf2::BufferCore::addTransformableCallback
TransformableCallbackHandle addTransformableCallback(const TransformableCallback &cb)
main
int main(int argc, char **argv)
Definition: buffer_core_test.cpp:2793
TransformableHelper::callback
void callback(tf2::TransformableRequestHandle request_handle, const std::string &target_frame, const std::string &source_frame, ros::Time time, tf2::TransformableResult result)
Definition: buffer_core_test.cpp:1760
tf2::BufferCore::setTransform
bool setTransform(const geometry_msgs::TransformStamped &transform, const std::string &authority, bool is_static=false)
tf2::BufferCore::lookupTransform
geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const ros::Time &target_time, const std::string &source_frame, const ros::Time &source_time, const std::string &fixed_frame) const
TimeBase< Time, Duration >::toSec
double toSec() const
step
unsigned int step
setupTree
void setupTree(tf2::BufferCore &mBC, const std::string &mode, const ros::Time &time, const ros::Duration &interpolation_space=ros::Duration())
Definition: buffer_core_test.cpp:147
push_back_i
void push_back_i(std::vector< std::string > &children, std::vector< std::string > &parents, std::vector< double > &dx, std::vector< double > &dy)
Definition: buffer_core_test.cpp:51
ROS_DEBUG
#define ROS_DEBUG(...)
push_back_y
void push_back_y(std::vector< std::string > &children, std::vector< std::string > &parents, std::vector< double > &dx, std::vector< double > &dy)
Definition: buffer_core_test.cpp:73
tf2::TransformableRequestHandle
uint64_t TransformableRequestHandle
tf2::BufferCore::clear
void clear()
TEST
TEST(BufferCore_setTransform, NoInsertOnSelfTransform)
Definition: buffer_core_test.cpp:272
setIdentity
void setIdentity(geometry_msgs::Transform &trans)
Definition: buffer_core_test.cpp:39
TransformableHelper::TransformableHelper
TransformableHelper()
Definition: buffer_core_test.cpp:1756
tf2::BufferCore::canTransform
bool canTransform(const std::string &target_frame, const ros::Time &target_time, const std::string &source_frame, const ros::Time &source_time, const std::string &fixed_frame, std::string *error_msg=NULL) const
buffer_core.h
TransformableHelper
Definition: buffer_core_test.cpp:1754
exceptions.h
ros::Time::init
static void init()
CHECK_TRANSFORMS_NEAR
#define CHECK_TRANSFORMS_NEAR(_out, _expected, _eps)
Definition: buffer_core_test.cpp:1151
TimeBase< Time, Duration >::fromNSec
Time & fromNSec(uint64_t t)
ros::Time
tf2::BufferCore
tf2::BufferCore::addTransformableRequest
TransformableRequestHandle addTransformableRequest(TransformableCallbackHandle handle, const std::string &target_frame, const std::string &source_frame, ros::Time time)
tf2::InvalidArgumentException
ros::Duration
tf2::TransformableResult
TransformableResult
TransformableHelper::called
bool called
Definition: buffer_core_test.cpp:1766
check_1_result
bool check_1_result(const geometry_msgs::TransformStamped &outpose, const std::string &source_frame, const std::string &target_frame, const ros::Time &eval_time, double epsilon)
Definition: buffer_core_test.cpp:686
t
geometry_msgs::TransformStamped t
tf2::TransformableCallbackHandle
uint32_t TransformableCallbackHandle
CHECK_QUATERNION_NEAR
#define CHECK_QUATERNION_NEAR(_q1, _x, _y, _z, _w, _epsilon)
Definition: buffer_core_test.cpp:1143
check_y_result
bool check_y_result(const geometry_msgs::TransformStamped &outpose, const std::string &source_frame, const std::string &target_frame, const ros::Time &eval_time, double epsilon)
Definition: buffer_core_test.cpp:831

test_tf2
Author(s): Tully Foote, Eitan Marder-Eppstein
autogenerated on Sun May 4 2025 02:16:43