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

tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Sat Aug 19 2023 02:38:08