30 #include <gtest/gtest.h> 36 #include "rostest/permuter.h" 44 timeval temp_time_struct;
45 gettimeofday(&temp_time_struct,NULL);
46 srand(temp_time_struct.tv_usec);
49 void generate_rand_vectors(
double scale, uint64_t runs, std::vector<double>& xvalues, std::vector<double>& yvalues, std::vector<double>&zvalues)
52 for ( uint64_t i = 0; i < runs ; i++ )
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;
61 void push_back_i(std::vector<std::string>& children, std::vector<std::string>& parents,
62 std::vector<double>& dx, std::vector<double>& dy)
72 children.push_back(
"b");
73 parents.push_back(
"a");
76 children.push_back(
"c");
77 parents.push_back(
"b");
83 void push_back_y(std::vector<std::string>& children, std::vector<std::string>& parents,
84 std::vector<double>& dx, std::vector<double>& dy)
94 children.push_back(
"b");
95 parents.push_back(
"a");
99 children.push_back(
"c");
100 parents.push_back(
"b");
104 children.push_back(
"d");
105 parents.push_back(
"b");
109 children.push_back(
"e");
110 parents.push_back(
"d");
115 void push_back_v(std::vector<std::string>& children, std::vector<std::string>& parents,
116 std::vector<double>& dx, std::vector<double>& dy)
126 children.push_back(
"b");
127 parents.push_back(
"a");
131 children.push_back(
"c");
132 parents.push_back(
"b");
136 children.push_back(
"f");
137 parents.push_back(
"a");
141 children.push_back(
"g");
142 parents.push_back(
"f");
148 void push_back_1(std::vector<std::string>& children, std::vector<std::string>& parents,
149 std::vector<double>& dx, std::vector<double>& dy)
151 children.push_back(
"2");
152 parents.push_back(
"1");
159 ROS_DEBUG(
"Clearing Buffer Core for new test setup");
162 ROS_DEBUG(
"Setting up test tree for formation %s", mode.c_str());
164 std::vector<std::string> children;
165 std::vector<std::string> parents;
166 std::vector<double> dx, dy;
172 else if (mode ==
"y")
177 else if (mode ==
"v")
182 else if (mode ==
"ring_45")
186 std::vector<std::string> frames;
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");
198 for (uint8_t iteration = 0; iteration < 2; ++iteration)
200 double direction = 1;
201 std::string frame_prefix;
204 frame_prefix =
"inverse_";
209 for (uint64_t i = 1; i < frames.size(); i++)
215 if (time >
ros::Time() + (interpolation_space * .5))
216 ts.
stamp_ = time - (interpolation_space * .5);
220 ts.
frame_id_ = frame_prefix + frames[i-1];
228 ts.
stamp_ = time + interpolation_space * .5;
233 else if (mode ==
"1")
238 else if (mode ==
"1_v")
244 EXPECT_FALSE(
"Undefined mode for tree setup. Test harness improperly setup.");
248 for (uint64_t i = 0; i < children.size(); i++)
253 if (time >
ros::Time() + (interpolation_space * .5))
254 ts.
stamp_ = time - (interpolation_space * .5);
263 ts.
stamp_ = time + interpolation_space * .5;
270 #define CHECK_QUATERNION_NEAR(_q1, _q2, _epsilon) \ 271 EXPECT_NEAR(_q1.angle(_q2), 0, _epsilon); \ 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); 308 tsb.child_frame_id_ =
"b";
311 q2.
setRPY(1.0, 0.25, 0.5);
317 tsc.child_frame_id_ =
"c";
320 q3.
setRPY(0.25, 0.75, 1.25);
326 tsd.child_frame_id_ =
"d";
329 q4.
setRPY(-0.5, 1.0, -0.75);
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;
336 expected_ab = tsa.
inverse() * tsb;
337 expected_ac = tsa.
inverse() * tsb * tsc;
338 expected_ad = tsa.
inverse() * tsb * tsc * tsd;
341 expected_bd = tsc * tsd;
342 expected_db = expected_bd.
inverse();
343 expected_ba = tsb.
inverse() * tsa;
346 expected_rootd = tsb * tsc * tsd;
347 expected_rootc = tsb * tsc;
406 TEST(
tf, lookupTransform_helix_configuration)
433 for (
ros::Time t = t0; t <= t1; t += step)
436 double dt = (t - t0).toSec();
437 double dt2 = (t2 - t0).toSec();
451 ts2.child_frame_id_ =
"c";
452 ts2.setOrigin(
tf::Vector3(cos(theta*dt), sin(theta*dt),0));
462 ts3.child_frame_id_ =
"d";
467 for (
ros::Time t = t0 + half_step; t < t1; t += step)
470 double dt = (t - t0).toSec();
471 double dt2 = (t2 - t0).toSec();
498 q2.
setRPY(0,0,-theta*dt2);
504 for (
ros::Time t = t0 + half_step; t < t1; t += (step + step))
507 double dt = (t - t0).toSec();
508 double dt2 = (t2 - t0).toSec();
515 mq2.
setRPY(0,0,-theta*dt);
525 rostest::Permuter permuter;
527 std::vector<ros::Time> times;
532 permuter.addOptionSet(times, &eval_time);
534 std::vector<ros::Duration> durations;
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);
554 std::string target_frame;
555 permuter.addOptionSet(frames, &target_frame);
557 while (permuter.step())
560 setupTree(mTR,
"ring_45", eval_time, interpolation_space);
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;
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") )
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")
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")
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")
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")
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")
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")
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")
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")
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")
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")
703 else if ((source_frame ==
"a" && target_frame ==
"g") ||
704 (source_frame ==
"b" && target_frame ==
"h") ||
705 (source_frame ==
"c" && target_frame ==
"i")
712 else if ((target_frame ==
"a" && source_frame ==
"g") ||
713 (target_frame ==
"b" && source_frame ==
"h") ||
714 (target_frame ==
"c" && source_frame ==
"i")
721 else if ((source_frame ==
"a" && target_frame ==
"h") ||
722 (source_frame ==
"b" && target_frame ==
"i")
729 else if ((target_frame ==
"a" && source_frame ==
"h") ||
730 (target_frame ==
"b" && source_frame ==
"i")
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());
745 TEST(
tf, setTransformNoInsertOnSelfTransform)
759 EXPECT_TRUE(std::isnan(tranStamped.
getOrigin().
z()));
764 TEST(
tf, setTransformNoInsertWithNoFrameID)
771 TEST(
tf, setTransformNoInsertWithNoParentID)
785 std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
786 for ( uint64_t i = 0; i < runs ; i++ )
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;
800 for ( uint64_t i = 0; i < runs ; i++ )
807 outpose.setIdentity();
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);
815 std::cout <<
"TransformExcepion got through!!!!! " << ex.what() << std::endl;
816 bool exception_improperly_thrown =
true;
817 EXPECT_FALSE(exception_improperly_thrown);
823 outpose.setIdentity();
825 EXPECT_NEAR(outpose.getOrigin().x(), 0,
epsilon);
826 EXPECT_NEAR(outpose.getOrigin().y(), 0,
epsilon);
827 EXPECT_NEAR(outpose.getOrigin().z(), 0,
epsilon);
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++ )
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;
852 qt.
setRPY(yawvalues[i],pitchvalues[i],rollvalues[i]);
861 for ( uint64_t i = 0; i < runs ; i++ )
869 outpose.setIdentity();
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);
877 outpose.setIdentity();
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);
886 std::cout <<
"TransformExcepion got through!!!!! " << ex.what() << std::endl;
887 bool exception_improperly_thrown =
true;
888 EXPECT_FALSE(exception_improperly_thrown);
894 outpose.setIdentity();
896 EXPECT_NEAR(outpose.getOrigin().x(), 0,
epsilon);
897 EXPECT_NEAR(outpose.getOrigin().y(), 0,
epsilon);
898 EXPECT_NEAR(outpose.getOrigin().z(), 0,
epsilon);
910 std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
911 for ( uint64_t i = 0; i < runs ; i++ )
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;
925 for ( uint64_t i = 0; i < runs ; i++ )
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;
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);
943 std::cout <<
"TransformExcepion got through!!!!! " << ex.what() << std::endl;
944 bool exception_improperly_thrown =
true;
945 EXPECT_FALSE(exception_improperly_thrown);
958 std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
959 for ( uint64_t i = 0; i < runs ; i++ )
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;
973 for ( uint64_t i = 0; i < runs ; i++ )
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;
985 EXPECT_NEAR(outvec.x(),
x,
epsilon);
986 EXPECT_NEAR(outvec.y(),
y,
epsilon);
987 EXPECT_NEAR(outvec.z(),
z,
epsilon);
991 std::cout <<
"TransformExcepion got through!!!!! " << ex.what() << std::endl;
992 bool exception_improperly_thrown =
true;
993 EXPECT_FALSE(exception_improperly_thrown);
1001 uint64_t runs = 400;
1006 std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
1007 for ( uint64_t i = 0; i < runs ; i++ )
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;
1022 for ( uint64_t i = 0; i < runs ; i++ )
1026 qt.
setRPY(xvalues[i],yvalues[i],zvalues[i]);
1035 EXPECT_NEAR(outvec.angle(invec) , 0,
epsilon);
1039 std::cout <<
"TransformExcepion got through!!!!! " << ex.what() << std::endl;
1040 bool exception_improperly_thrown =
true;
1041 EXPECT_FALSE(exception_improperly_thrown);
1050 uint64_t runs = 400;
1053 std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
1056 for ( uint64_t i = 0; i < runs ; i++ )
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);
1070 TEST(data, Vector3StampedConversions)
1073 uint64_t runs = 400;
1076 std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
1079 for ( uint64_t i = 0; i < runs ; i++ )
1083 geometry_msgs::Vector3Stamped msgv;
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);
1097 uint64_t runs = 400;
1100 std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
1103 for ( uint64_t i = 0; i < runs ; i++ )
1106 btv.
setRPY(xvalues[i], yvalues[i], zvalues[i]);
1108 geometry_msgs::Quaternion msgv;
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);
1119 TEST(data, QuaternionStampedConversions)
1122 uint64_t runs = 400;
1125 std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
1128 for ( uint64_t i = 0; i < runs ; i++ )
1131 btv.setRPY(xvalues[i], yvalues[i], zvalues[i]);
1133 geometry_msgs::QuaternionStamped msgv;
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);
1148 uint64_t runs = 400;
1151 std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
1153 std::vector<double> xvalues2(runs), yvalues2(runs), zvalues2(runs);
1156 for ( uint64_t i = 0; i < runs ; i++ )
1159 qt.
setRPY(xvalues2[i], yvalues2[i], zvalues2[i]);
1162 geometry_msgs::Transform msgv;
1179 uint64_t runs = 400;
1182 std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
1184 std::vector<double> xvalues2(runs), yvalues2(runs), zvalues2(runs);
1187 for ( uint64_t i = 0; i < runs ; i++ )
1190 qt.
setRPY(xvalues2[i], yvalues2[i], zvalues2[i]);
1193 geometry_msgs::PoseStamped msgv;
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);
1210 unsigned int runs = 4;
1215 std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
1216 for ( uint64_t i = 0; i < runs ; i++ )
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;
1229 for ( uint64_t i = 0; i < runs ; i++ )
1236 outpose.setIdentity();
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);
1244 std::cout <<
"TransformExcepion got through!!!!! " << ex.what() << std::endl;
1245 bool exception_improperly_thrown =
true;
1246 EXPECT_FALSE(exception_improperly_thrown);
1254 unsigned int runs = 4;
1259 std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
1260 for (
unsigned int i = 0; i < runs ; i++ )
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;
1275 for (
unsigned int i = 0; i < runs ; i++ )
1282 outpose.setIdentity();
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);
1290 std::cout <<
"TransformExcepion got through!!!!! " << ex.what() << std::endl;
1291 bool exception_improperly_thrown =
true;
1292 EXPECT_FALSE(exception_improperly_thrown);
1301 unsigned int runs = 4;
1306 std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
1307 for ( uint64_t i = 0; i < runs ; i++ )
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;
1320 for ( uint64_t i = 0; i < runs ; i++ )
1327 outpose.setIdentity();
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);
1335 std::cout <<
"TransformExcepion got through!!!!! " << ex.what() << std::endl;
1336 bool exception_improperly_thrown =
true;
1337 EXPECT_FALSE(exception_improperly_thrown);
1345 unsigned int runs = 4;
1350 std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
1351 for (
unsigned int i = 0; i < runs ; i++ )
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;
1366 for (
unsigned int i = 0; i < runs ; i++ )
1373 outpose.setIdentity();
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);
1381 std::cout <<
"TransformExcepion got through!!!!! " << ex.what() << std::endl;
1382 bool exception_improperly_thrown =
true;
1383 EXPECT_FALSE(exception_improperly_thrown);
1391 unsigned int runs = 4;
1396 std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
1397 for (
unsigned int i = 0; i < runs ; i++ )
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;
1412 for (
unsigned int i = 0; i < runs ; i++ )
1419 outpose.setIdentity();
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);
1427 std::cout <<
"TransformExcepion got through!!!!! " << ex.what() << std::endl;
1428 bool exception_improperly_thrown =
true;
1429 EXPECT_FALSE(exception_improperly_thrown);
1437 unsigned int runs = 4;
1442 std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
1443 for (
unsigned int i = 0; i < runs ; i++ )
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;
1458 for (
unsigned int i = 0; i < runs ; i++ )
1462 bool exception_thrown =
false;
1466 outpose.setIdentity();
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);
1474 exception_thrown =
true;
1476 EXPECT_TRUE(exception_thrown);
1485 std::vector<std::string> children;
1486 std::vector<std::string> parents;
1488 children.push_back(
"a");
1489 parents.push_back(
"c");
1491 children.push_back(
"b");
1492 parents.push_back(
"c");
1494 children.push_back(
"c");
1495 parents.push_back(
"e");
1497 children.push_back(
"d");
1498 parents.push_back(
"e");
1500 children.push_back(
"e");
1501 parents.push_back(
"f");
1503 children.push_back(
"f");
1504 parents.push_back(
"j");
1507 children.push_back(
"/k");
1508 parents.push_back(
"l");
1512 for (uint64_t i = 0; i < children.size(); i++)
1521 for (uint64_t i = 0; i < children.size(); i++)
1524 EXPECT_STREQ(parents[i].c_str(), output.c_str());
1538 std::vector<std::string> children;
1539 std::vector<std::string> parents;
1541 children.push_back(
"b");
1542 parents.push_back(
"a");
1543 children.push_back(
"a");
1544 parents.push_back(
"NO_PARENT");
1548 for (uint64_t i = 0; i < children.size(); i++)
1559 outpose.setIdentity();
1561 EXPECT_NEAR(outpose.getOrigin().x(), 0,
epsilon);
1562 EXPECT_NEAR(outpose.getOrigin().y(), 0,
epsilon);
1563 EXPECT_NEAR(outpose.getOrigin().z(), 0,
epsilon);
1576 std::string error_str;
1598 EXPECT_TRUE(fabs(((stop_time-start_time)-timeout).toSec()) < eps);
1604 EXPECT_TRUE(fabs(((stop_time-start_time)-timeout).toSec()) < eps);
1613 EXPECT_TRUE(fabs(((stop_time-start_time)).toSec()) < eps);
1619 EXPECT_TRUE(fabs(((stop_time-start_time)).toSec()) < eps);
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");
1640 EXPECT_TRUE(
"Lookupgh Exception Caught");
1644 printf(
"%s\n",ex.what());
1645 EXPECT_FALSE(
"Other Exception Caught");
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");
1659 EXPECT_TRUE(
"Extrapolation Exception Caught");
1663 printf(
"%s\n",ex.what());
1664 EXPECT_FALSE(
"Other Exception Caught");
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");
1680 EXPECT_FALSE(
"Extrapolation Exception Caught");
1684 printf(
"%s\n",ex.what());
1685 EXPECT_FALSE(
"Other Exception Caught");
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");
1699 EXPECT_FALSE(
"Extrapolation Exception Caught");
1703 printf(
"%s\n",ex.what());
1704 EXPECT_FALSE(
"Other Exception Caught");
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");
1718 EXPECT_TRUE(
"Extrapolation Exception Caught");
1722 printf(
"%s\n",ex.what());
1723 EXPECT_FALSE(
"Other Exception Caught");
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");
1734 EXPECT_TRUE(
"Extrapolation Exception Caught");
1738 printf(
"%s\n",ex.what());
1739 EXPECT_FALSE(
"Other Exception Caught");
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");
1755 EXPECT_TRUE(
"Extrapolation Exception Caught");
1759 printf(
"%s\n",ex.what());
1760 EXPECT_FALSE(
"Other Exception Caught");
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");
1772 EXPECT_TRUE(
"Extrapolation Exception Caught");
1776 printf(
"%s\n",ex.what());
1777 EXPECT_FALSE(
"Other Exception Caught");
1814 EXPECT_FALSE(
"Shouldn't have gotten this exception");
1836 bool excepted =
false;
1847 EXPECT_TRUE(excepted);
1860 EXPECT_TRUE(excepted);
1873 EXPECT_TRUE(excepted);
1886 EXPECT_TRUE(excepted);
1900 EXPECT_FALSE(excepted);
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()));
1929 EXPECT_FALSE(
"Excetion improperly thrown");
2000 for (
int i = 10; i < 20; i++)
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));
2024 EXPECT_TRUE(mTR.
canTransform(
"child",
"parent", valid_time));
2025 EXPECT_TRUE(mTR.
canTransform(
"child",
"other_child", valid_time));
2028 EXPECT_TRUE(mTR.
canTransform(
"child",
"parent", zero_time));
2029 EXPECT_TRUE(mTR.
canTransform(
"child",
"other_child", zero_time));
2032 EXPECT_FALSE(mTR.
canTransform(
"child",
"parent", old_time));
2033 EXPECT_FALSE(mTR.
canTransform(
"child",
"other_child", old_time));
2036 EXPECT_FALSE(mTR.
canTransform(
"child",
"parent", future_time));
2037 EXPECT_FALSE(mTR.
canTransform(
"child",
"other_child", future_time));
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));
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"));
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"));
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"));
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"));
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"));
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"));
2070 EXPECT_TRUE(mTR.
canTransform(
"child", zero_time,
"parent", valid_time,
"parent"));
2072 EXPECT_FALSE(mTR.
canTransform(
"child", old_time,
"parent", valid_time,
"parent"));
2074 EXPECT_TRUE(mTR.
canTransform(
"child", valid_time,
"parent", valid_time,
"parent"));
2076 EXPECT_FALSE(mTR.
canTransform(
"child", future_time,
"parent", valid_time,
"parent"));
2079 EXPECT_TRUE(mTR.
canTransform(
"child", valid_time,
"parent", zero_time,
"parent"));
2081 EXPECT_TRUE(mTR.
canTransform(
"child", valid_time,
"parent", old_time,
"parent"));
2083 EXPECT_TRUE(mTR.
canTransform(
"child", valid_time,
"parent", valid_time,
"parent"));
2085 EXPECT_TRUE(mTR.
canTransform(
"child", valid_time,
"parent", future_time,
"parent"));
2093 for (
int i = 10; i < 20; i++)
2118 mTR.
lookupTransform(
"some_frame4",
"some_frame4", future_time, output);
2120 mTR.
lookupTransform(
"other_child",
"other_child", future_time, output);
2132 printf(
"Exception improperly thrown: %s", ex.what());
2133 EXPECT_FALSE(
"Exception thrown");
2139 EXPECT_FALSE(
"Exception should have been thrown");
2143 EXPECT_TRUE(
"Exception Thrown Correctly");
2148 EXPECT_FALSE(
"Exception should have been thrown");
2152 EXPECT_TRUE(
"Exception Thrown Correctly");
2166 mTR.
lookupTransform(
"child", zero_time,
"parent", valid_time,
"child", output);
2167 mTR.
lookupTransform(
"child", zero_time,
"other_child", valid_time,
"child", output);
2169 mTR.
lookupTransform(
"child", old_time,
"parent", valid_time,
"child", output);
2170 mTR.
lookupTransform(
"child", old_time,
"other_child", valid_time,
"child", output);
2172 mTR.
lookupTransform(
"child", valid_time,
"parent", valid_time,
"child", output);
2173 mTR.
lookupTransform(
"child", valid_time,
"other_child", valid_time,
"child", output);
2175 mTR.
lookupTransform(
"child", future_time,
"parent", valid_time,
"child", output);
2176 mTR.
lookupTransform(
"child", future_time,
"other_child", valid_time,
"child", output);
2180 printf(
"Exception improperly thrown: %s", ex.what());
2181 EXPECT_FALSE(
"Exception incorrectly thrown");
2186 mTR.
lookupTransform(
"child", valid_time,
"parent", old_time,
"child", output);
2187 EXPECT_FALSE(
"Exception should have been thrown");
2191 EXPECT_TRUE(
"Exception Thrown Correctly");
2196 mTR.
lookupTransform(
"child", valid_time,
"parent", future_time,
"child", output);
2197 EXPECT_FALSE(
"Exception should have been thrown");
2201 EXPECT_TRUE(
"Exception Thrown Correctly");
2207 mTR.
lookupTransform(
"child", zero_time,
"parent", valid_time,
"parent", output);
2209 mTR.
lookupTransform(
"child", valid_time,
"parent", valid_time,
"parent", output);
2213 printf(
"Exception improperly thrown: %s", ex.what());
2214 EXPECT_FALSE(
"Exception incorrectly thrown");
2219 mTR.
lookupTransform(
"child", old_time,
"parent", valid_time,
"parent", output);
2220 EXPECT_FALSE(
"Exception should have been thrown");
2224 EXPECT_TRUE(
"Exception Thrown Correctly");
2228 mTR.
lookupTransform(
"child", future_time,
"parent", valid_time,
"parent", output);
2229 EXPECT_FALSE(
"Exception should have been thrown");
2233 EXPECT_TRUE(
"Exception Thrown Correctly");
2238 mTR.
lookupTransform(
"child", valid_time,
"parent", zero_time,
"parent", output);
2240 mTR.
lookupTransform(
"child", valid_time,
"parent", old_time,
"parent", output);
2242 mTR.
lookupTransform(
"child", valid_time,
"parent", valid_time,
"parent", output);
2244 mTR.
lookupTransform(
"child", valid_time,
"parent", future_time,
"parent", output);
2248 printf(
"Exception improperly thrown: %s", ex.what());
2249 EXPECT_FALSE(
"Exception improperly thrown");
2266 printf(
"Exception improperly thrown: %s", ex.what());
2267 EXPECT_FALSE(
"Exception improperly thrown");
2281 std::vector <std::string> 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());
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());
2304 printf(
"this should have thrown\n");
2313 printf(
"A different type of exception was expected\n");
2337 printf(
"this should have thrown\n");
2346 printf(
"A different type of exception was expected\n");
2382 q.setZ(sqrt(2.0)/2.0);
2384 q.setW(sqrt(2.0)/2.0);
2387 q.setZ(sqrt(2.0)/2.0 + 0.01);
2390 q.setZ(sqrt(2.0)/2.0 - 0.01);
2394 q.setValue(1,0,0,0);
2421 geometry_msgs::Quaternion q;
2437 q.z = sqrt(2.0)/2.0;
2439 q.w = sqrt(2.0)/2.0;
2442 q.z = sqrt(2.0)/2.0 + 0.01;
2445 q.z = sqrt(2.0)/2.0 - 0.01;
2449 q.x = 1.0; q.y = 0.0; q.z = 0.0; q.w = 0.0;
2475 TEST(data, StampedOperatorEqualEqual)
2484 EXPECT_TRUE(stamped_pose0A == stamped_pose_reference);
2486 EXPECT_FALSE(stamped_pose0B == stamped_pose_reference);
2488 EXPECT_FALSE(stamped_pose0C == stamped_pose_reference);
2490 EXPECT_FALSE(stamped_pose0D == stamped_pose_reference);
2492 EXPECT_FALSE(stamped_pose0E == stamped_pose_reference);
2494 EXPECT_FALSE(stamped_pose0F == stamped_pose_reference);
2496 EXPECT_FALSE(stamped_pose0G == stamped_pose_reference);
2498 EXPECT_FALSE(stamped_pose0H == stamped_pose_reference);
2502 TEST(data, StampedTransformOperatorEqualEqual)
2511 EXPECT_TRUE(stamped_transform0A == stamped_transform_reference);
2513 EXPECT_FALSE(stamped_transform0B == stamped_transform_reference);
2515 EXPECT_FALSE(stamped_transform0C == stamped_transform_reference);
2517 EXPECT_FALSE(stamped_transform0D == stamped_transform_reference);
2519 EXPECT_FALSE(stamped_transform0E == stamped_transform_reference);
2521 EXPECT_FALSE(stamped_transform0F == stamped_transform_reference);
2523 EXPECT_FALSE(stamped_transform0G == stamped_transform_reference);
2525 EXPECT_FALSE(stamped_transform0H == stamped_transform_reference);
2530 EXPECT_FALSE(stamped_transform1A == stamped_transform_reference);
2532 EXPECT_FALSE(stamped_transform1B == stamped_transform_reference);
2534 EXPECT_FALSE(stamped_transform1C == stamped_transform_reference);
2536 EXPECT_FALSE(stamped_transform1D == stamped_transform_reference);
2538 EXPECT_FALSE(stamped_transform1E == stamped_transform_reference);
2540 EXPECT_FALSE(stamped_transform1F == stamped_transform_reference);
2542 EXPECT_FALSE(stamped_transform1G == stamped_transform_reference);
2544 EXPECT_FALSE(stamped_transform1H == stamped_transform_reference);
2556 EXPECT_FALSE(stamped_pose1 == stamped_pose0);
2557 stamped_pose1 = stamped_pose0;
2558 EXPECT_TRUE(stamped_pose1 == stamped_pose0);
2563 testing::InitGoogleTest(&argc, argv);
2566 return RUN_ALL_TESTS();
void assertQuaternionValid(const tf::Quaternion &q)
Throw InvalidArgument if quaternion is malformed.
bool expectValidQuaternion(tf::Quaternion q)
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
Time & fromNSec(uint64_t t)
void push_back_v(std::vector< std::string > &children, std::vector< std::string > &parents, std::vector< double > &dx, std::vector< double > &dy)
static void poseStampedTFToMsg(const Stamped< Pose > &bt, geometry_msgs::PoseStamped &msg)
convert Stamped<Pose> to PoseStamped msg
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
static void quaternionStampedMsgToTF(const geometry_msgs::QuaternionStamped &msg, Stamped< Quaternion > &bt)
convert QuaternionStamped msg to Stamped<Quaternion>
void generate_rand_vectors(double scale, uint64_t runs, std::vector< double > &xvalues, std::vector< double > &yvalues, std::vector< double > &zvalues)
static void vector3StampedTFToMsg(const Stamped< Vector3 > &bt, geometry_msgs::Vector3Stamped &msg)
convert Stamped<Vector3> to Vector3Stamped msg
static void vector3TFToMsg(const Vector3 &bt_v, geometry_msgs::Vector3 &msg_v)
convert Vector3 to Vector3 msg
void setupTree(tf::Transformer &mTR, const std::string &mode, const ros::Time &time, const ros::Duration &interpolation_space=ros::Duration())
#define CHECK_TRANSFORMS_NEAR(_out, _expected, _eps)
TFSIMD_FORCE_INLINE const tfScalar & y() const
Return the y value.
static const std::string threading_error
TFSIMD_FORCE_INLINE const tfScalar & x() const
Return the x value.
The data type which will be cross compatable with geometry_msgs This is the tf datatype equivilant of...
TEST(tf, lookupTransform_compount)
static void vector3MsgToTF(const geometry_msgs::Vector3 &msg_v, Vector3 &bt_v)
convert Vector3 msg to Vector3
TFSIMD_FORCE_INLINE const tfScalar & z() const
Return the z value.
static void vector3StampedMsgToTF(const geometry_msgs::Vector3Stamped &msg, Stamped< Vector3 > &bt)
convert Vector3Stamped msg to Stamped<Vector3>
void setRPY(const tfScalar &roll, const tfScalar &pitch, const tfScalar &yaw)
Set the quaternion using fixed axis RPY.
Duration & fromSec(double t)
static void quaternionMsgToTF(const geometry_msgs::Quaternion &msg, Quaternion &bt)
convert Quaternion msg to Quaternion
TFSIMD_FORCE_INLINE Vector3()
No initialization constructor.
static void quaternionTFToMsg(const Quaternion &bt, geometry_msgs::Quaternion &msg)
convert Quaternion to Quaternion msg
TFSIMD_FORCE_INLINE const tfScalar & y() const
Return the y value.
TFSIMD_FORCE_INLINE const tfScalar & x() const
Return the x value.
static void quaternionStampedTFToMsg(const Stamped< Quaternion > &bt, geometry_msgs::QuaternionStamped &msg)
convert Stamped<Quaternion> to QuaternionStamped msg
bool expectInvalidQuaternion(tf::Quaternion q)
void push_back_y(std::vector< std::string > &children, std::vector< std::string > &parents, std::vector< double > &dx, std::vector< double > &dy)
TFSIMD_FORCE_INLINE const tfScalar & z() const
Return the z value.
ros::Time stamp_
The timestamp associated with this data.
void push_back_i(std::vector< std::string > &children, std::vector< std::string > &parents, std::vector< double > &dx, std::vector< double > &dy)
std::string frame_id_
The frame_id associated this data.
static void transformMsgToTF(const geometry_msgs::Transform &msg, Transform &bt)
convert Transform msg to Transform
void push_back_1(std::vector< std::string > &children, std::vector< std::string > &parents, std::vector< double > &dx, std::vector< double > &dy)
static void poseStampedMsgToTF(const geometry_msgs::PoseStamped &msg, Stamped< Pose > &bt)
convert PoseStamped msg to Stamped<Pose>
int main(int argc, char **argv)
static void transformTFToMsg(const Transform &bt, geometry_msgs::Transform &msg)
convert Transform to Transform msg
Vector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-byte...