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

tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Mon Feb 28 2022 22:26:19