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

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