tf_unittest_future.cpp
Go to the documentation of this file.
1 #include <ctime>
2 #include <cstdlib>
3 #include <gtest/gtest.h>
4 #include <tf/tf.h>
5 
7 
8 using namespace tf;
9 
10 void seed_rand()
11 {
12  //Seed random number generator with current microseond count
13  std::srand(std::time(0));
14 };
15 
16 
17 
18 TEST(tf, SignFlipExtrapolate)
19 {
20  double epsilon = 1e-3;
21 
22  double truex, truey, trueyaw1, trueyaw2;
23 
24  truex = 5.220;
25  truey = 1.193;
26  trueyaw1 = 2.094;
27  trueyaw2 = 2.199;
28  ros::Time ts0;
29  ts0.fromSec(46.6);
30  ros::Time ts1;
31  ts1.fromSec(46.7);
32  ros::Time ts2;
33  ts2.fromSec(46.8);
34 
35  TransformStorage tout;
36  double yaw, pitch, roll;
37 
39  (tf::Transform(tf::Quaternion(0.000, 0.000, -0.8386707128751809, 0.5446388118427071),
40  tf::Vector3(1.0330764266905630, 5.2545257423922198, -0.000)),
41  ts0, "odom", "other0"), 3);
43  (tf::Transform(tf::Quaternion(0.000, 0.000, 0.8660255375641606, -0.4999997682866531),
44  tf::Vector3(1.5766646418987809, 5.1177550046707436, -0.000)),
45  ts1, "odom", "other1"), 3);
47  (tf::Transform(tf::Quaternion(0.000, 0.000, 0.8910066733792211, -0.4539902069358919),
48  tf::Vector3(2.1029791754869160, 4.9249128183465967, -0.000)),
49  ts2, "odom", "other2"), 3);
50 
51  tf::TimeCache tc;
52  tf::Transform res;
53 
54  tc.interpolate(t0, t1, ts1, tout);
55  res = tout.inverse();
56  res.getBasis().getEulerZYX(yaw,pitch,roll);
57 
58  EXPECT_NEAR(res.getOrigin().x(), truex, epsilon);
59  EXPECT_NEAR(res.getOrigin().y(), truey, epsilon);
60  EXPECT_NEAR(yaw, trueyaw1, epsilon);
61 
62  tc.interpolate(t0, t1, ts2, tout);
63  res = tout.inverse();
64  res.getBasis().getEulerZYX(yaw,pitch,roll);
65 
66  EXPECT_NEAR(res.getOrigin().x(), truex, epsilon);
67  EXPECT_NEAR(res.getOrigin().y(), truey, epsilon);
68  EXPECT_NEAR(yaw, trueyaw2, epsilon);
69 
70  tc.interpolate(t1, t2, ts2, tout);
71  res = tout.inverse();
72  res.getBasis().getEulerZYX(yaw,pitch,roll);
73 
74  EXPECT_NEAR(res.getOrigin().x(), truex, epsilon);
75  EXPECT_NEAR(res.getOrigin().y(), truey, epsilon);
76  EXPECT_NEAR(yaw, trueyaw2, epsilon);
77 }
78 
79 
80 
83 int main(int argc, char **argv){
84  testing::InitGoogleTest(&argc, argv);
85  return RUN_ALL_TESTS();
86 }
87 
double epsilon
Definition: quaternion.cpp:38
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
Definition: Quaternion.h:30
TFSIMD_FORCE_INLINE Matrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: Transform.h:110
void interpolate(const TransformStorage &one, const TransformStorage &two, ros::Time time, TransformStorage &output)
Definition: cache.cpp:170
A class to keep a sorted linked list in time This builds and maintains a list of timestamped data...
Definition: time_cache.h:97
Definition: exceptions.h:38
Time & fromSec(double t)
TEST(tf, SignFlipExtrapolate)
The Transform class supports rigid transforms with only translation and rotation and no scaling/shear...
Definition: Transform.h:31
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
Return the origin vector translation.
Definition: Transform.h:115
ROS_DEPRECATED void getEulerZYX(tfScalar &yaw, tfScalar &pitch, tfScalar &roll, unsigned int solution_number=1) const
Get the matrix represented as euler angles around ZYX.
Definition: Matrix3x3.h:284
void seed_rand()
Storage for transforms and their parent.
Definition: time_cache.h:54
int main(int argc, char **argv)
The Stamped Transform datatype used by tf.


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Sat Sep 4 2021 02:27:56