velocity_test.cpp
Go to the documentation of this file.
1 #include <gtest/gtest.h>
2 #include <tf/tf.h>
3 #include <sys/time.h>
4 
5 #include "tf/LinearMath/Vector3.h"
6 
7 using namespace tf;
8 
9 
10 // The fixture for testing class Foo.
11 class LinearVelocitySquareTest : public ::testing::Test {
12 protected:
13  // You can remove any or all of the following functions if its body
14  // is empty.
15 
16  LinearVelocitySquareTest() {
17  double x = -.1;
18  double y = 0;
19  double z = 0;
20  for (double t = 0.1; t <= 6; t += 0.1)
21  {
22  if (t < 1) x += .1;
23  else if (t < 2) y += .1;
24  else if (t < 3) x -= .1;
25  else if (t < 4) y -= .1;
26  else if (t < 5) z += .1;
27  else z -= .1;
28  tf_.setTransform(StampedTransform(tf::Transform(tf::createIdentityQuaternion(), tf::Vector3(x, y, z)), ros::Time(t), "foo", "bar"));
29  tf_.setTransform(StampedTransform(tf::Transform(tf::createIdentityQuaternion(), tf::Vector3(1,0,0)), ros::Time(t), "foo", "stationary_offset_child"));
30  tf_.setTransform(StampedTransform(tf::Transform(tf::createIdentityQuaternion(), tf::Vector3(0,0,1)), ros::Time(t), "stationary_offset_parent", "foo"));
31  }
32 
33  // You can do set-up work for each test here.
34  }
35 
36  virtual ~LinearVelocitySquareTest() {
37  // You can do clean-up work that doesn't throw exceptions here.
38  }
39 
40  // If the constructor and destructor are not enough for setting up
41  // and cleaning up each test, you can define the following methods:
42 
43  virtual void SetUp() {
44  // Code here will be called immediately after the constructor (right
45  // before each test).
46  }
47 
48  virtual void TearDown() {
49  // Code here will be called immediately after each test (right
50  // before the destructor).
51  }
52 
53  // Objects declared here can be used by all tests in the test case for LinearVelocity.
54 
55  tf::Transformer tf_;
56 
57 };
58 
59 // The fixture for testing class Foo.
60 class AngularVelocitySquareTest : public ::testing::Test {
61 protected:
62  // You can remove any or all of the following functions if its body
63  // is empty.
64 
65  AngularVelocitySquareTest() {
66  double x = -.1;
67  double y = 0;
68  double z = 0;
69  for (double t = 0.1; t < 6; t += 0.1)
70  {
71  if (t < 1) x += .1;
72  else if (t < 2) x -= .1;
73  else if (t < 3) y += .1;
74  else if (t < 4) y -= .1;
75  else if (t < 5) z += .1;
76  else z -= .1;
77  tf_.setTransform(StampedTransform(tf::Transform(tf::createQuaternionFromRPY(x, y, z), tf::Vector3(0,0,0)), ros::Time(t), "foo", "bar"));
78  tf_.setTransform(StampedTransform(tf::Transform(tf::createIdentityQuaternion(), tf::Vector3(1,0,0)), ros::Time(t), "foo", "stationary_offset_child"));
79  tf_.setTransform(StampedTransform(tf::Transform(tf::createIdentityQuaternion(), tf::Vector3(0,0,-1)), ros::Time(t), "stationary_offset_parent", "foo"));
80  }
81 
82  // You can do set-up work for each test here.
83  }
84 
85  virtual ~AngularVelocitySquareTest() {
86  // You can do clean-up work that doesn't throw exceptions here.
87  }
88 
89  // If the constructor and destructor are not enough for setting up
90  // and cleaning up each test, you can define the following methods:
91 
92  virtual void SetUp() {
93  // Code here will be called immediately after the constructor (right
94  // before each test).
95  }
96 
97  virtual void TearDown() {
98  // Code here will be called immediately after each test (right
99  // before the destructor).
100  }
101 
102  // Objects declared here can be used by all tests in the test case for AngularVelocity.
103 
104  tf::Transformer tf_;
105 
106 };
107 
108 TEST_F(LinearVelocitySquareTest, LinearVelocityToThreeFrames)
109 {
110  geometry_msgs::Twist twist;
111  std::vector<std::string> offset_frames;
112 
113  offset_frames.push_back("foo");
114  offset_frames.push_back("stationary_offset_child");
115  offset_frames.push_back("stationary_offset_parent");
116 
117 
118  for (std::vector<std::string>::iterator it = offset_frames.begin(); it != offset_frames.end(); ++it)
119  {
120  try
121  {
122  tf_.lookupTwist("bar", *it, ros::Time(0.5), ros::Duration(0.1), twist);
123  EXPECT_FLOAT_EQ(twist.linear.x, 1.0);
124  EXPECT_FLOAT_EQ(twist.linear.y, 0.0);
125  EXPECT_FLOAT_EQ(twist.linear.z, 0.0);
126  EXPECT_FLOAT_EQ(twist.angular.x, 0.0);
127  EXPECT_FLOAT_EQ(twist.angular.y, 0.0);
128  EXPECT_FLOAT_EQ(twist.angular.z, 0.0);
129 
130  tf_.lookupTwist("bar", *it, ros::Time(1.5), ros::Duration(0.1), twist);
131  EXPECT_FLOAT_EQ(twist.linear.x, 0.0);
132  EXPECT_FLOAT_EQ(twist.linear.y, 1.0);
133  EXPECT_FLOAT_EQ(twist.linear.z, 0.0);
134  EXPECT_FLOAT_EQ(twist.angular.x, 0.0);
135  EXPECT_FLOAT_EQ(twist.angular.y, 0.0);
136  EXPECT_FLOAT_EQ(twist.angular.z, 0.0);
137 
138  tf_.lookupTwist("bar", *it, ros::Time(2.5), ros::Duration(0.1), twist);
139  EXPECT_FLOAT_EQ(twist.linear.x, -1.0);
140  EXPECT_FLOAT_EQ(twist.linear.y, 0.0);
141  EXPECT_FLOAT_EQ(twist.linear.z, 0.0);
142  EXPECT_FLOAT_EQ(twist.angular.x, 0.0);
143  EXPECT_FLOAT_EQ(twist.angular.y, 0.0);
144  EXPECT_FLOAT_EQ(twist.angular.z, 0.0);
145 
146  tf_.lookupTwist("bar", *it, ros::Time(3.5), ros::Duration(0.1), twist);
147  EXPECT_FLOAT_EQ(twist.linear.x, 0.0);
148  EXPECT_FLOAT_EQ(twist.linear.y, -1.0);
149  EXPECT_FLOAT_EQ(twist.linear.z, 0.0);
150  EXPECT_FLOAT_EQ(twist.angular.x, 0.0);
151  EXPECT_FLOAT_EQ(twist.angular.y, 0.0);
152  EXPECT_FLOAT_EQ(twist.angular.z, 0.0);
153 
154  tf_.lookupTwist("bar", *it, ros::Time(4.5), ros::Duration(0.1), twist);
155  EXPECT_FLOAT_EQ(twist.linear.x, 0.0);
156  EXPECT_FLOAT_EQ(twist.linear.y, 0.0);
157  EXPECT_FLOAT_EQ(twist.linear.z, 1.0);
158  EXPECT_FLOAT_EQ(twist.angular.x, 0.0);
159  EXPECT_FLOAT_EQ(twist.angular.y, 0.0);
160  EXPECT_FLOAT_EQ(twist.angular.z, 0.0);
161 
162  tf_.lookupTwist("bar", *it, ros::Time(5.5), ros::Duration(0.1), twist);
163  EXPECT_FLOAT_EQ(twist.linear.x, 0.0);
164  EXPECT_FLOAT_EQ(twist.linear.y, 0.0);
165  EXPECT_FLOAT_EQ(twist.linear.z, -1.0);
166  EXPECT_FLOAT_EQ(twist.angular.x, 0.0);
167  EXPECT_FLOAT_EQ(twist.angular.y, 0.0);
168  EXPECT_FLOAT_EQ(twist.angular.z, 0.0);
169  }
170  catch(tf::TransformException &ex)
171  {
172  EXPECT_STREQ("", ex.what());
173  }
174  }
175 };
176 
177 TEST_F(AngularVelocitySquareTest, AngularVelocityAlone)
178 {
179  double epsilon = 1e-6;
180  geometry_msgs::Twist twist;
181  try
182  {
183  tf_.lookupTwist("bar", "foo", ros::Time(0.5), ros::Duration(0.1), twist);
184  EXPECT_NEAR(twist.linear.x, 0.0, epsilon);
185  EXPECT_NEAR(twist.linear.y, 0.0, epsilon);
186  EXPECT_NEAR(twist.linear.z, 0.0, epsilon);
187  EXPECT_NEAR(twist.angular.x, 1.0, epsilon);
188  EXPECT_NEAR(twist.angular.y, 0.0, epsilon);
189  EXPECT_NEAR(twist.angular.z, 0.0, epsilon);
190 
191  tf_.lookupTwist("bar", "foo", ros::Time(1.5), ros::Duration(0.1), twist);
192  EXPECT_NEAR(twist.linear.x, 0.0, epsilon);
193  EXPECT_NEAR(twist.linear.y, 0.0, epsilon);
194  EXPECT_NEAR(twist.linear.z, 0.0, epsilon);
195  EXPECT_NEAR(twist.angular.x, -1.0, epsilon);
196  EXPECT_NEAR(twist.angular.y, 0.0, epsilon);
197  EXPECT_NEAR(twist.angular.z, 0.0, epsilon);
198 
199  tf_.lookupTwist("bar", "foo", ros::Time(2.5), ros::Duration(0.1), twist);
200  EXPECT_NEAR(twist.linear.x, 0.0, epsilon);
201  EXPECT_NEAR(twist.linear.y, 0.0, epsilon);
202  EXPECT_NEAR(twist.linear.z, 0.0, epsilon);
203  EXPECT_NEAR(twist.angular.x, 0.0, epsilon);
204  EXPECT_NEAR(twist.angular.y, 1.0, epsilon);
205  EXPECT_NEAR(twist.angular.z, 0.0, epsilon);
206 
207  tf_.lookupTwist("bar", "foo", ros::Time(3.5), ros::Duration(0.1), twist);
208  EXPECT_NEAR(twist.linear.x, 0.0, epsilon);
209  EXPECT_NEAR(twist.linear.y, 0.0, epsilon);
210  EXPECT_NEAR(twist.linear.z, 0.0, epsilon);
211  EXPECT_NEAR(twist.angular.x, 0.0, epsilon);
212  EXPECT_NEAR(twist.angular.y, -1.0, epsilon);
213  EXPECT_NEAR(twist.angular.z, 0.0, epsilon);
214 
215  tf_.lookupTwist("bar", "foo", ros::Time(4.5), ros::Duration(0.1), twist);
216  EXPECT_NEAR(twist.linear.x, 0.0, epsilon);
217  EXPECT_NEAR(twist.linear.y, 0.0, epsilon);
218  EXPECT_NEAR(twist.linear.z, 0.0, epsilon);
219  EXPECT_NEAR(twist.angular.x, 0.0, epsilon);
220  EXPECT_NEAR(twist.angular.y, 0.0, epsilon);
221  EXPECT_NEAR(twist.angular.z, 1.0, epsilon);
222 
223  tf_.lookupTwist("bar", "foo", ros::Time(5.5), ros::Duration(0.1), twist);
224  EXPECT_NEAR(twist.linear.x, 0.0, epsilon);
225  EXPECT_NEAR(twist.linear.y, 0.0, epsilon);
226  EXPECT_NEAR(twist.linear.z, 0.0, epsilon);
227  EXPECT_NEAR(twist.angular.x, 0.0, epsilon);
228  EXPECT_NEAR(twist.angular.y, 0.0, epsilon);
229  EXPECT_NEAR(twist.angular.z, -1.0, epsilon);
230  }
231  catch(tf::TransformException &ex)
232  {
233  EXPECT_STREQ("", ex.what());
234  }
235 };
236 
237 TEST_F(AngularVelocitySquareTest, AngularVelocityOffsetChildFrameInX)
238 {
239  double epsilon = 1e-6;
240  geometry_msgs::Twist twist;
241  try
242  {
243  tf_.lookupTwist("bar", "stationary_offset_child", ros::Time(0.5), ros::Duration(0.1), twist);
244  EXPECT_NEAR(twist.linear.x, 0.0, epsilon);
245  EXPECT_NEAR(twist.linear.y, 0.0, epsilon);
246  EXPECT_NEAR(twist.linear.z, 0.0, epsilon);
247  EXPECT_NEAR(twist.angular.x, 1.0, epsilon);
248  EXPECT_NEAR(twist.angular.y, 0.0, epsilon);
249  EXPECT_NEAR(twist.angular.z, 0.0, epsilon);
250 
251  tf_.lookupTwist("bar", "stationary_offset_child", ros::Time(1.5), ros::Duration(0.1), twist);
252  EXPECT_NEAR(twist.linear.x, 0.0, epsilon);
253  EXPECT_NEAR(twist.linear.y, 0.0, epsilon);
254  EXPECT_NEAR(twist.linear.z, 0.0, epsilon);
255  EXPECT_NEAR(twist.angular.x, -1.0, epsilon);
256  EXPECT_NEAR(twist.angular.y, 0.0, epsilon);
257  EXPECT_NEAR(twist.angular.z, 0.0, epsilon);
258 
259  tf_.lookupTwist("bar", "stationary_offset_child", ros::Time(2.5), ros::Duration(0.1), twist);
260  EXPECT_NEAR(twist.linear.x, 0.0, epsilon);
261  EXPECT_NEAR(twist.linear.y, 0.0, epsilon);
262  EXPECT_NEAR(twist.linear.z, -1.0, epsilon);
263  EXPECT_NEAR(twist.angular.x, 0.0, epsilon);
264  EXPECT_NEAR(twist.angular.y, 1.0, epsilon);
265  EXPECT_NEAR(twist.angular.z, 0.0, epsilon);
266 
267  tf_.lookupTwist("bar", "stationary_offset_child", ros::Time(3.5), ros::Duration(0.1), twist);
268  EXPECT_NEAR(twist.linear.x, 0.0, epsilon);
269  EXPECT_NEAR(twist.linear.y, 0.0, epsilon);
270  EXPECT_NEAR(twist.linear.z, 1.0, epsilon);
271  EXPECT_NEAR(twist.angular.x, 0.0, epsilon);
272  EXPECT_NEAR(twist.angular.y, -1.0, epsilon);
273  EXPECT_NEAR(twist.angular.z, 0.0, epsilon);
274 
275  tf_.lookupTwist("bar", "stationary_offset_child", ros::Time(4.5), ros::Duration(0.1), twist);
276  EXPECT_NEAR(twist.linear.x, 0.0, epsilon);
277  EXPECT_NEAR(twist.linear.y, 1.0, epsilon);
278  EXPECT_NEAR(twist.linear.z, 0.0, epsilon);
279  EXPECT_NEAR(twist.angular.x, 0.0, epsilon);
280  EXPECT_NEAR(twist.angular.y, 0.0, epsilon);
281  EXPECT_NEAR(twist.angular.z, 1.0, epsilon);
282 
283  tf_.lookupTwist("bar", "stationary_offset_child", ros::Time(5.5), ros::Duration(0.1), twist);
284  EXPECT_NEAR(twist.linear.x, 0.0, epsilon);
285  EXPECT_NEAR(twist.linear.y, -1.0, epsilon);
286  EXPECT_NEAR(twist.linear.z, 0.0, epsilon);
287  EXPECT_NEAR(twist.angular.x, 0.0, epsilon);
288  EXPECT_NEAR(twist.angular.y, 0.0, epsilon);
289  EXPECT_NEAR(twist.angular.z, -1.0, epsilon);
290  }
291  catch(tf::TransformException &ex)
292  {
293  EXPECT_STREQ("", ex.what());
294  }
295 };
296 
297 TEST_F(AngularVelocitySquareTest, AngularVelocityOffsetParentFrameInZ)
298 {
299  double epsilon = 1e-6;
300  geometry_msgs::Twist twist;
301  try
302  {
303  tf_.lookupTwist("bar", "stationary_offset_parent", ros::Time(0.5), ros::Duration(0.1), twist);
304  EXPECT_NEAR(twist.linear.x, 0.0, epsilon);
305  EXPECT_NEAR(twist.linear.y, -1.0, epsilon);
306  EXPECT_NEAR(twist.linear.z, 0.0, epsilon);
307  EXPECT_NEAR(twist.angular.x, 1.0, epsilon);
308  EXPECT_NEAR(twist.angular.y, 0.0, epsilon);
309  EXPECT_NEAR(twist.angular.z, 0.0, epsilon);
310 
311  tf_.lookupTwist("bar", "stationary_offset_parent", ros::Time(1.5), ros::Duration(0.1), twist);
312  EXPECT_NEAR(twist.linear.x, 0.0, epsilon);
313  EXPECT_NEAR(twist.linear.y, 1.0, epsilon);
314  EXPECT_NEAR(twist.linear.z, 0.0, epsilon);
315  EXPECT_NEAR(twist.angular.x, -1.0, epsilon);
316  EXPECT_NEAR(twist.angular.y, 0.0, epsilon);
317  EXPECT_NEAR(twist.angular.z, 0.0, epsilon);
318 
319  tf_.lookupTwist("bar", "stationary_offset_parent", ros::Time(2.5), ros::Duration(0.1), twist);
320  EXPECT_NEAR(twist.linear.x, 1.0, epsilon);
321  EXPECT_NEAR(twist.linear.y, 0.0, epsilon);
322  EXPECT_NEAR(twist.linear.z, 0.0, epsilon);
323  EXPECT_NEAR(twist.angular.x, 0.0, epsilon);
324  EXPECT_NEAR(twist.angular.y, 1.0, epsilon);
325  EXPECT_NEAR(twist.angular.z, 0.0, epsilon);
326 
327  tf_.lookupTwist("bar", "stationary_offset_parent", ros::Time(3.5), ros::Duration(0.1), twist);
328  EXPECT_NEAR(twist.linear.x, -1.0, epsilon);
329  EXPECT_NEAR(twist.linear.y, 0.0, epsilon);
330  EXPECT_NEAR(twist.linear.z, 0.0, epsilon);
331  EXPECT_NEAR(twist.angular.x, 0.0, epsilon);
332  EXPECT_NEAR(twist.angular.y, -1.0, epsilon);
333  EXPECT_NEAR(twist.angular.z, 0.0, epsilon);
334 
335  tf_.lookupTwist("bar", "stationary_offset_parent", ros::Time(4.5), ros::Duration(0.1), twist);
336  EXPECT_NEAR(twist.linear.x, 0.0, epsilon);
337  EXPECT_NEAR(twist.linear.y, 0.0, epsilon);
338  EXPECT_NEAR(twist.linear.z, 0.0, epsilon);
339  EXPECT_NEAR(twist.angular.x, 0.0, epsilon);
340  EXPECT_NEAR(twist.angular.y, 0.0, epsilon);
341  EXPECT_NEAR(twist.angular.z, 1.0, epsilon);
342 
343  tf_.lookupTwist("bar", "stationary_offset_parent", ros::Time(5.5), ros::Duration(0.1), twist);
344  EXPECT_NEAR(twist.linear.x, 0.0, epsilon);
345  EXPECT_NEAR(twist.linear.y, 0.0, epsilon);
346  EXPECT_NEAR(twist.linear.z, 0.0, epsilon);
347  EXPECT_NEAR(twist.angular.x, 0.0, epsilon);
348  EXPECT_NEAR(twist.angular.y, 0.0, epsilon);
349  EXPECT_NEAR(twist.angular.z, -1.0, epsilon);
350  }
351  catch(tf::TransformException &ex)
352  {
353  EXPECT_STREQ("", ex.what());
354  }
355 };
356 
357 
358 int main(int argc, char **argv){
359  testing::InitGoogleTest(&argc, argv);
360  return RUN_ALL_TESTS();
361 }
362 
epsilon
double epsilon
Definition: quaternion.cpp:37
tf::createQuaternionFromRPY
static tf::Quaternion createQuaternionFromRPY(double roll, double pitch, double yaw)
construct a Quaternion from Fixed angles
Definition: transform_datatypes.h:150
tf
Definition: exceptions.h:38
y
TFSIMD_FORCE_INLINE const tfScalar & y() const
Return the y value.
Definition: Vector3.h:486
tf::createIdentityQuaternion
static tf::Quaternion createIdentityQuaternion()
construct an Identity Quaternion
Definition: transform_datatypes.h:193
main
int main(int argc, char **argv)
Definition: velocity_test.cpp:358
tf::Transform
The Transform class supports rigid transforms with only translation and rotation and no scaling/shear...
Definition: Transform.h:31
x
TFSIMD_FORCE_INLINE const tfScalar & x() const
Return the x value.
Definition: Vector3.h:484
z
TFSIMD_FORCE_INLINE const tfScalar & z() const
Return the z value.
Definition: Vector3.h:488
TEST_F
TEST_F(LinearVelocitySquareTest, LinearVelocityToThreeFrames)
Definition: velocity_test.cpp:108
tf::StampedTransform
The Stamped Transform datatype used by tf.
Definition: transform_datatypes.h:81
tf::Transformer
A Class which provides coordinate transforms between any two frames in a system.
Definition: tf.h:90
tf::Vector3
Vector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-byte...
Definition: Vector3.h:38

tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Mon Jun 10 2019 12:25:26