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

test_tf2
Author(s): Tully Foote, Eitan Marder-Eppstein
autogenerated on Fri Oct 16 2020 19:09:05