transform_twist_test.cpp
Go to the documentation of this file.
1 #include <gtest/gtest.h>
3 #include <sys/time.h>
4 
6 
7 using namespace tf;
8 
9 
10 // The fixture for testing class Foo.
11 class TransformTwistLinearTest : public ::testing::Test {
12 protected:
13  // You can remove any or all of the following functions if its body
14  // is empty.
15 
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  tw_.header.stamp = ros::Time();
35  tw_.header.frame_id = "bar";
36  tw_.twist.linear.x = 1;
37  tw_.twist.linear.y = 2;
38  tw_.twist.linear.z = 3;
39  tw_.twist.angular.x = 0;
40  tw_.twist.angular.y = 0;
41  tw_.twist.angular.z = 0;
42 
43  }
44 
46  // You can do clean-up work that doesn't throw exceptions here.
47  }
48 
49  // If the constructor and destructor are not enough for setting up
50  // and cleaning up each test, you can define the following methods:
51 
52  virtual void SetUp() {
53  // Code here will be called immediately after the constructor (right
54  // before each test).
55  }
56 
57  virtual void TearDown() {
58  // Code here will be called immediately after each test (right
59  // before the destructor).
60  }
61 
62  // Objects declared here can be used by all tests in the test case for LinearVelocity.
63 
65 
66  geometry_msgs::TwistStamped tw_;
67 
68 };
69 
70 // The fixture for testing class Foo.
71 class TransformTwistAngularTest : public ::testing::Test {
72 protected:
73  // You can remove any or all of the following functions if its body
74  // is empty.
75 
77  double x = -.1;
78  double y = 0;
79  double z = 0;
80  for (double t = 0.1; t < 6; t += 0.1)
81  {
82  if (t < 1) x += .1;
83  else if (t < 2) x -= .1;
84  else if (t < 3) y += .1;
85  else if (t < 4) y -= .1;
86  else if (t < 5) z += .1;
87  else z -= .1;
88  tf_.setTransform(StampedTransform(tf::Transform(tf::createQuaternionFromRPY(x, y, z), tf::Vector3(0,0,0)), ros::Time(t), "foo", "bar"));
89  tf_.setTransform(StampedTransform(tf::Transform(tf::createIdentityQuaternion(), tf::Vector3(1,0,0)), ros::Time(t), "foo", "stationary_offset_child"));
90  tf_.setTransform(StampedTransform(tf::Transform(tf::createIdentityQuaternion(), tf::Vector3(0,0,-1)), ros::Time(t), "stationary_offset_parent", "foo"));
91  }
92 
93  // You can do set-up work for each test here.
94  // You can do set-up work for each test here.
95  tw_x.header.stamp = ros::Time();
96  tw_x.header.frame_id = "bar";
97  tw_x.twist.angular.x = 1;
98 
99  tw_y.header.stamp = ros::Time();
100  tw_y.header.frame_id = "bar";
101  tw_y.twist.angular.y = 1;
102 
103  tw_z.header.stamp = ros::Time();
104  tw_z.header.frame_id = "bar";
105  tw_z.twist.angular.z = 1;
106  }
107 
109  // You can do clean-up work that doesn't throw exceptions here.
110  }
111 
112  // If the constructor and destructor are not enough for setting up
113  // and cleaning up each test, you can define the following methods:
114 
115  virtual void SetUp() {
116  // Code here will be called immediately after the constructor (right
117  // before each test).
118  }
119 
120  virtual void TearDown() {
121  // Code here will be called immediately after each test (right
122  // before the destructor).
123  }
124 
125  // Objects declared here can be used by all tests in the test case for AngularVelocity.
126 
128  geometry_msgs::TwistStamped tw_x, tw_y, tw_z;
129 };
130 
131 TEST_F(TransformTwistLinearTest, LinearVelocityToThreeFrames)
132 {
133  geometry_msgs::TwistStamped tw;
134  geometry_msgs::TwistStamped tw_in =tw_;
135 
136  std::vector<std::string> offset_frames;
137 
138  offset_frames.push_back("foo");
139  offset_frames.push_back("stationary_offset_child");
140  offset_frames.push_back("stationary_offset_parent");
141 
142 
143  for (std::vector<std::string>::iterator it = offset_frames.begin(); it != offset_frames.end(); ++it)
144  {
145  try
146  {
147  tw_in.header.stamp = ros::Time(0.5);
148  tf_.transformTwist(*it, tw_in, tw);
149  EXPECT_FLOAT_EQ(11.0, tw.twist.linear.x);
150  EXPECT_FLOAT_EQ(2.0 , tw.twist.linear.y);
151  EXPECT_FLOAT_EQ(3.0, tw.twist.linear.z);
152  EXPECT_FLOAT_EQ(0.0, tw.twist.angular.x);
153  EXPECT_FLOAT_EQ(0.0, tw.twist.angular.y);
154  EXPECT_FLOAT_EQ(0.0, tw.twist.angular.z);
155 
156  tw_in.header.stamp = ros::Time(1.5);
157  tf_.transformTwist(*it, tw_in, tw);
158  EXPECT_FLOAT_EQ(1.0, tw.twist.linear.x);
159  EXPECT_FLOAT_EQ(12.0, tw.twist.linear.y);
160  EXPECT_FLOAT_EQ(3.0, tw.twist.linear.z);
161  EXPECT_FLOAT_EQ(0.0, tw.twist.angular.x);
162  EXPECT_FLOAT_EQ(0.0, tw.twist.angular.y);
163  EXPECT_FLOAT_EQ(0.0, tw.twist.angular.z);
164 
165  tw_in.header.stamp = ros::Time(2.5);
166  tf_.transformTwist(*it, tw_in, tw);
167  EXPECT_FLOAT_EQ(-9.0, tw.twist.linear.x);
168  EXPECT_FLOAT_EQ(2.0, tw.twist.linear.y);
169  EXPECT_FLOAT_EQ(3.0, tw.twist.linear.z);
170  EXPECT_FLOAT_EQ(tw.twist.angular.x, 0.0);
171  EXPECT_FLOAT_EQ(tw.twist.angular.y, 0.0);
172  EXPECT_FLOAT_EQ(tw.twist.angular.z, 0.0);
173 
174  tw_in.header.stamp = ros::Time(3.5);
175  tf_.transformTwist(*it, tw_in, tw);
176  EXPECT_FLOAT_EQ(tw.twist.linear.x, 1.0);
177  EXPECT_FLOAT_EQ(tw.twist.linear.y, -8.0);
178  EXPECT_FLOAT_EQ(tw.twist.linear.z, 3.0);
179  EXPECT_FLOAT_EQ(tw.twist.angular.x, 0.0);
180  EXPECT_FLOAT_EQ(tw.twist.angular.y, 0.0);
181  EXPECT_FLOAT_EQ(tw.twist.angular.z, 0.0);
182 
183  tw_in.header.stamp = ros::Time(4.5);
184  tf_.transformTwist(*it, tw_in, tw);
185  EXPECT_FLOAT_EQ(tw.twist.linear.x, 1.0);
186  EXPECT_FLOAT_EQ(tw.twist.linear.y, 2.0);
187  EXPECT_FLOAT_EQ(tw.twist.linear.z, 13.0);
188  EXPECT_FLOAT_EQ(tw.twist.angular.x, 0.0);
189  EXPECT_FLOAT_EQ(tw.twist.angular.y, 0.0);
190  EXPECT_FLOAT_EQ(tw.twist.angular.z, 0.0);
191 
192  tw_in.header.stamp = ros::Time(5.5);
193  tf_.transformTwist(*it, tw_in, tw);
194  EXPECT_FLOAT_EQ(tw.twist.linear.x, 1.0);
195  EXPECT_FLOAT_EQ(tw.twist.linear.y, 2.0);
196  EXPECT_FLOAT_EQ(tw.twist.linear.z, -7.0);
197  EXPECT_FLOAT_EQ(tw.twist.angular.x, 0.0);
198  EXPECT_FLOAT_EQ(tw.twist.angular.y, 0.0);
199  EXPECT_FLOAT_EQ(tw.twist.angular.z, 0.0);
200  }
201  catch(tf::TransformException &ex)
202  {
203  EXPECT_STREQ("", ex.what());
204  }
205  }
206 };
207 
208 TEST_F(TransformTwistAngularTest, AngularVelocityAlone)
209 {
210  double epsilon = 1e-14;
211  geometry_msgs::TwistStamped tw;
212  geometry_msgs::TwistStamped tw_in;
213 
214  try
215  {
216  //tf_.lookupVelocity("foo", "bar", ros::Time(0.5), ros::Duration(0.1), tw);
217  tw_in =tw_x;
218  tw_in.header.stamp = ros::Time(0.5);
219  tf_.transformTwist("foo", tw_in, tw);
220  EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon);
221  EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon);
222  EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon);
223  EXPECT_NEAR(tw.twist.angular.x, 2.0, epsilon);
224  EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon);
225  EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon);
226 
227  //tf_.lookupVelocity("foo", "bar", ros::Time(1.5), ros::Duration(0.1), tw);
228  tw_in =tw_x;
229  tw_in.header.stamp = ros::Time(1.5);
230  tf_.transformTwist("foo", tw_in, tw);
231  EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon);
232  EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon);
233  EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon);
234  EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon);
235  EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon);
236  EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon);
237 
238  // tf_.lookupVelocity("foo", "bar", ros::Time(2.5), ros::Duration(0.1), tw);
239  tw_in =tw_y;
240  tw_in.header.stamp = ros::Time(2.5);
241  tf_.transformTwist("foo", tw_in, tw);
242  EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon);
243  EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon);
244  EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon);
245  EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon);
246  EXPECT_NEAR(tw.twist.angular.y, 2.0, epsilon);
247  EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon);
248 
249  //tf_.lookupVelocity("foo", "bar", ros::Time(3.5), ros::Duration(0.1), tw);
250  tw_in =tw_y;
251  tw_in.header.stamp = ros::Time(3.5);
252  tf_.transformTwist("foo", tw_in, tw);
253  EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon);
254  EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon);
255  EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon);
256  EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon);
257  EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon);
258  EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon);
259 
260  //tf_.lookupVelocity("foo", "bar", ros::Time(4.5), ros::Duration(0.1), tw);
261  tw_in =tw_z;
262  tw_in.header.stamp = ros::Time(4.5);
263  tf_.transformTwist("foo", tw_in, tw);
264  EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon);
265  EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon);
266  EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon);
267  EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon);
268  EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon);
269  EXPECT_NEAR(tw.twist.angular.z, 2.0, epsilon);
270 
271  // tf_.lookupVelocity("foo", "bar", ros::Time(5.5), ros::Duration(0.1), tw);
272  tw_in =tw_z;
273  tw_in.header.stamp = ros::Time(5.5);
274  tf_.transformTwist("foo", tw_in, tw);
275  EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon);
276  EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon);
277  EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon);
278  EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon);
279  EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon);
280  EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon);
281  }
282  catch(tf::TransformException &ex)
283  {
284  EXPECT_STREQ("", ex.what());
285  }
286 };
287 /*
288 TEST_F(TransformTwistAngularTest, AngularVelocityOffsetChildFrameInX)
289 {
290  double epsilon = 1e-14;
291  geometry_msgs::TwistStamped tw;
292  try
293  {
294  tf_.lookupVelocity("stationary_offset_child", "bar", ros::Time(0.5), ros::Duration(0.1), tw);
295  EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon);
296  EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon);
297  EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon);
298  EXPECT_NEAR(tw.twist.angular.x, 1.0, epsilon);
299  EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon);
300  EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon);
301 
302  tf_.lookupVelocity("stationary_offset_child", "bar", ros::Time(1.5), ros::Duration(0.1), tw);
303  EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon);
304  EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon);
305  EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon);
306  EXPECT_NEAR(tw.twist.angular.x, -1.0, epsilon);
307  EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon);
308  EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon);
309 
310  tf_.lookupVelocity("stationary_offset_child", "bar", ros::Time(2.5), ros::Duration(0.1), tw);
311  EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon);
312  EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon);
313  EXPECT_NEAR(tw.twist.linear.z, -1.0, epsilon);
314  EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon);
315  EXPECT_NEAR(tw.twist.angular.y, 1.0, epsilon);
316  EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon);
317 
318  tf_.lookupVelocity("stationary_offset_child", "bar", ros::Time(3.5), ros::Duration(0.1), tw);
319  EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon);
320  EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon);
321  EXPECT_NEAR(tw.twist.linear.z, 1.0, epsilon);
322  EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon);
323  EXPECT_NEAR(tw.twist.angular.y, -1.0, epsilon);
324  EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon);
325 
326  tf_.lookupVelocity("stationary_offset_child", "bar", ros::Time(4.5), ros::Duration(0.1), tw);
327  EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon);
328  EXPECT_NEAR(tw.twist.linear.y, 1.0, epsilon);
329  EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon);
330  EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon);
331  EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon);
332  EXPECT_NEAR(tw.twist.angular.z, 1.0, epsilon);
333 
334  tf_.lookupVelocity("stationary_offset_child", "bar", ros::Time(5.5), ros::Duration(0.1), tw);
335  EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon);
336  EXPECT_NEAR(tw.twist.linear.y, -1.0, epsilon);
337  EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon);
338  EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon);
339  EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon);
340  EXPECT_NEAR(tw.twist.angular.z, -1.0, epsilon);
341  }
342  catch(tf::TransformException &ex)
343  {
344  EXPECT_STREQ("", ex.what());
345  }
346 };
347 
348 TEST_F(TransformTwistAngularTest, AngularVelocityOffsetParentFrameInZ)
349 {
350  double epsilon = 1e-14;
351  geometry_msgs::TwistStamped tw;
352  try
353  {
354  tf_.lookupVelocity("stationary_offset_parent", "bar", ros::Time(0.5), ros::Duration(0.1), tw);
355  EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon);
356  EXPECT_NEAR(tw.twist.linear.y, -1.0, epsilon);
357  EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon);
358  EXPECT_NEAR(tw.twist.angular.x, 1.0, epsilon);
359  EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon);
360  EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon);
361 
362  tf_.lookupVelocity("stationary_offset_parent", "bar", ros::Time(1.5), ros::Duration(0.1), tw);
363  EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon);
364  EXPECT_NEAR(tw.twist.linear.y, 1.0, epsilon);
365  EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon);
366  EXPECT_NEAR(tw.twist.angular.x, -1.0, epsilon);
367  EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon);
368  EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon);
369 
370  tf_.lookupVelocity("stationary_offset_parent", "bar", ros::Time(2.5), ros::Duration(0.1), tw);
371  EXPECT_NEAR(tw.twist.linear.x, 1.0, epsilon);
372  EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon);
373  EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon);
374  EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon);
375  EXPECT_NEAR(tw.twist.angular.y, 1.0, epsilon);
376  EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon);
377 
378  tf_.lookupVelocity("stationary_offset_parent", "bar", ros::Time(3.5), ros::Duration(0.1), tw);
379  EXPECT_NEAR(tw.twist.linear.x, -1.0, epsilon);
380  EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon);
381  EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon);
382  EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon);
383  EXPECT_NEAR(tw.twist.angular.y, -1.0, epsilon);
384  EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon);
385 
386  tf_.lookupVelocity("stationary_offset_parent", "bar", ros::Time(4.5), ros::Duration(0.1), tw);
387  EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon);
388  EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon);
389  EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon);
390  EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon);
391  EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon);
392  EXPECT_NEAR(tw.twist.angular.z, 1.0, epsilon);
393 
394  tf_.lookupVelocity("stationary_offset_parent", "bar", ros::Time(5.5), ros::Duration(0.1), tw);
395  EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon);
396  EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon);
397  EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon);
398  EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon);
399  EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon);
400  EXPECT_NEAR(tw.twist.angular.z, -1.0, epsilon);
401  }
402  catch(tf::TransformException &ex)
403  {
404  EXPECT_STREQ("", ex.what());
405  }
406 };
407 */
408 
409 int main(int argc, char **argv){
410  testing::InitGoogleTest(&argc, argv);
411  ros::init(argc, argv, "testing_transform_twist");
412  return RUN_ALL_TESTS();
413 }
414 
double epsilon
Definition: quaternion.cpp:37
geometry_msgs::TwistStamped tw_z
static tf::Quaternion createQuaternionFromRPY(double roll, double pitch, double yaw)
construct a Quaternion from Fixed angles
TEST_F(TransformTwistLinearTest, LinearVelocityToThreeFrames)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
int main(int argc, char **argv)
Definition: exceptions.h:38
TFSIMD_FORCE_INLINE const tfScalar & y() const
Return the y value.
Definition: Vector3.h:486
static tf::Quaternion createIdentityQuaternion()
construct an Identity Quaternion
The Transform class supports rigid transforms with only translation and rotation and no scaling/shear...
Definition: Transform.h:31
TFSIMD_FORCE_INLINE const tfScalar & x() const
Return the x value.
Definition: Vector3.h:484
TFSIMD_FORCE_INLINE const tfScalar & z() const
Return the z value.
Definition: Vector3.h:488
This class inherits from Transformer and automatically subscribes to ROS transform messages...
geometry_msgs::TwistStamped tw_
The Stamped Transform datatype used by tf.
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 Fri Jun 7 2019 22:00:28