cache_unittest.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <gtest/gtest.h>
31 #include <tf2/time_cache.h>
33 #include <stdexcept>
34 
35 #include <geometry_msgs/TransformStamped.h>
36 
37 #include <cmath>
38 
39 std::vector<double> values;
40 unsigned int step = 0;
41 
42 void seed_rand()
43 {
44  values.clear();
45  for (unsigned int i = 0; i < 1000; i++)
46  {
47  int pseudo_rand = std::floor(i * M_PI);
48  values.push_back(( pseudo_rand % 100)/50.0 - 1.0);
49  //printf("Seeding with %f\n", values.back());
50  }
51 };
52 
53 
54 double get_rand()
55 {
56  if (values.size() == 0) throw std::runtime_error("you need to call seed_rand first");
57  if (step >= values.size())
58  step = 0;
59  else
60  step++;
61  return values[step];
62 }
63 
64 using namespace tf2;
65 
66 
68 {
69  stor.translation_.setValue(0.0, 0.0, 0.0);
70  stor.rotation_.setValue(0.0, 0.0, 0.0, 1.0);
71 }
72 
73 TEST(TimeCache, Repeatability)
74 {
75  unsigned int runs = 100;
76 
77  tf2::TimeCache cache;
78 
79  TransformStorage stor;
80  setIdentity(stor);
81 
82  for ( uint64_t i = 1; i < runs ; i++ )
83  {
84  stor.frame_id_ = i;
85  stor.stamp_ = ros::Time().fromNSec(i);
86 
87  cache.insertData(stor);
88  }
89 
90  for ( uint64_t i = 1; i < runs ; i++ )
91 
92  {
93  cache.getData(ros::Time().fromNSec(i), stor);
94  EXPECT_EQ(stor.frame_id_, i);
95  EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i));
96  }
97 
98 }
99 
100 TEST(TimeCache, RepeatabilityReverseInsertOrder)
101 {
102  unsigned int runs = 100;
103 
104  tf2::TimeCache cache;
105 
106  TransformStorage stor;
107  setIdentity(stor);
108 
109  for ( int i = runs -1; i >= 0 ; i-- )
110  {
111  stor.frame_id_ = i;
112  stor.stamp_ = ros::Time().fromNSec(i);
113 
114  cache.insertData(stor);
115  }
116  for ( uint64_t i = 1; i < runs ; i++ )
117 
118  {
119  cache.getData(ros::Time().fromNSec(i), stor);
120  EXPECT_EQ(stor.frame_id_, i);
121  EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i));
122  }
123 
124 }
125 
126 #if 0 // jfaust: this doesn't seem to actually be testing random insertion?
127 TEST(TimeCache, RepeatabilityRandomInsertOrder)
128 {
129 
130  seed_rand();
131 
132  tf2::TimeCache cache;
133  double my_vals[] = {13,2,5,4,9,7,3,11,15,14,12,1,6,10,0,8};
134  std::vector<double> values (my_vals, my_vals + sizeof(my_vals)/sizeof(double));
135  unsigned int runs = values.size();
136 
137  TransformStorage stor;
138  setIdentity(stor);
139  for ( uint64_t i = 0; i <runs ; i++ )
140  {
141  values[i] = 10.0 * get_rand();
142  std::stringstream ss;
143  ss << values[i];
144  stor.header.frame_id = ss.str();
145  stor.frame_id_ = i;
146  stor.stamp_ = ros::Time().fromNSec(i);
147 
148  cache.insertData(stor);
149  }
150  for ( uint64_t i = 1; i < runs ; i++ )
151 
152  {
153  cache.getData(ros::Time().fromNSec(i), stor);
154  EXPECT_EQ(stor.frame_id_, i);
155  EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i));
156  std::stringstream ss;
157  ss << values[i];
158  EXPECT_EQ(stor.header.frame_id, ss.str());
159  }
160 
161 }
162 #endif
163 
164 TEST(TimeCache, ZeroAtFront)
165 {
166  uint64_t runs = 100;
167 
168  tf2::TimeCache cache;
169 
170  TransformStorage stor;
171  setIdentity(stor);
172 
173  for ( uint64_t i = 1; i < runs ; i++ )
174  {
175  stor.frame_id_ = i;
176  stor.stamp_ = ros::Time().fromNSec(i);
177 
178  cache.insertData(stor);
179  }
180 
181  stor.frame_id_ = runs;
182  stor.stamp_ = ros::Time().fromNSec(runs);
183  cache.insertData(stor);
184 
185  for ( uint64_t i = 1; i < runs ; i++ )
186 
187  {
188  cache.getData(ros::Time().fromNSec(i), stor);
189  EXPECT_EQ(stor.frame_id_, i);
190  EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i));
191  }
192 
193  cache.getData(ros::Time(), stor);
194  EXPECT_EQ(stor.frame_id_, runs);
195  EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(runs));
196 
197  stor.frame_id_ = runs;
198  stor.stamp_ = ros::Time().fromNSec(runs+1);
199  cache.insertData(stor);
200 
201 
202  //Make sure we get a different value now that a new values is added at the front
203  cache.getData(ros::Time(), stor);
204  EXPECT_EQ(stor.frame_id_, runs);
205  EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(runs+1));
206 
207 }
208 
209 TEST(TimeCache, CartesianInterpolation)
210 {
211  uint64_t runs = 100;
212  double epsilon = 2e-6;
213  seed_rand();
214 
215  tf2::TimeCache cache;
216  std::vector<double> xvalues(2);
217  std::vector<double> yvalues(2);
218  std::vector<double> zvalues(2);
219 
220  uint64_t offset = 200;
221 
222  TransformStorage stor;
223  setIdentity(stor);
224 
225  for ( uint64_t i = 1; i < runs ; i++ )
226  {
227 
228  for (uint64_t step = 0; step < 2 ; step++)
229  {
230  xvalues[step] = 10.0 * get_rand();
231  yvalues[step] = 10.0 * get_rand();
232  zvalues[step] = 10.0 * get_rand();
233 
234  stor.translation_.setValue(xvalues[step], yvalues[step], zvalues[step]);
235  stor.frame_id_ = 2;
236  stor.stamp_ = ros::Time().fromNSec(step * 100 + offset);
237  cache.insertData(stor);
238  }
239 
240  for (int pos = 0; pos < 100 ; pos ++)
241  {
242  uint64_t time = offset + pos;
243  cache.getData(ros::Time().fromNSec(time), stor);
244  uint64_t time_out = stor.stamp_.toNSec();
245  double x_out = stor.translation_.x();
246  double y_out = stor.translation_.y();
247  double z_out = stor.translation_.z();
248  // printf("pose %d, %f %f %f, expected %f %f %f\n", pos, x_out, y_out, z_out,
249  // xvalues[0] + (xvalues[1] - xvalues[0]) * (double)pos/100.,
250  // yvalues[0] + (yvalues[1] - yvalues[0]) * (double)pos/100.0,
251  // zvalues[0] + (xvalues[1] - zvalues[0]) * (double)pos/100.0);
252  EXPECT_EQ(time, time_out);
253  EXPECT_NEAR(xvalues[0] + (xvalues[1] - xvalues[0]) * (double)pos/100.0, x_out, epsilon);
254  EXPECT_NEAR(yvalues[0] + (yvalues[1] - yvalues[0]) * (double)pos/100.0, y_out, epsilon);
255  EXPECT_NEAR(zvalues[0] + (zvalues[1] - zvalues[0]) * (double)pos/100.0, z_out, epsilon);
256  }
257 
258 
259  cache.clearList();
260  }
261 
262 
263 }
264 
266 TEST(TimeCache, ReparentingInterpolationProtection)
267 {
268  double epsilon = 1e-6;
269  uint64_t offset = 555;
270 
271  seed_rand();
272 
273  tf2::TimeCache cache;
274  std::vector<double> xvalues(2);
275  std::vector<double> yvalues(2);
276  std::vector<double> zvalues(2);
277 
278  TransformStorage stor;
279  setIdentity(stor);
280 
281  for (uint64_t step = 0; step < 2 ; step++)
282  {
283  xvalues[step] = 10.0 * get_rand();
284  yvalues[step] = 10.0 * get_rand();
285  zvalues[step] = 10.0 * get_rand();
286 
287  stor.translation_.setValue(xvalues[step], yvalues[step], zvalues[step]);
288  stor.frame_id_ = step + 4;
289  stor.stamp_ = ros::Time().fromNSec(step * 100 + offset);
290  cache.insertData(stor);
291  }
292 
293  for (int pos = 0; pos < 100 ; pos ++)
294  {
295  EXPECT_TRUE(cache.getData(ros::Time().fromNSec(offset + pos), stor));
296  double x_out = stor.translation_.x();
297  double y_out = stor.translation_.y();
298  double z_out = stor.translation_.z();
299  EXPECT_NEAR(xvalues[0], x_out, epsilon);
300  EXPECT_NEAR(yvalues[0], y_out, epsilon);
301  EXPECT_NEAR(zvalues[0], z_out, epsilon);
302  }
303 }
304 
305 TEST(Bullet, Slerp)
306 {
307 
308  uint64_t runs = 100;
309  seed_rand();
310 
311  tf2::Quaternion q1, q2;
312  q1.setEuler(0,0,0);
313 
314  for (uint64_t i = 0 ; i < runs ; i++)
315  {
316  q2.setEuler(1.0 * get_rand(),
317  1.0 * get_rand(),
318  1.0 * get_rand());
319 
320 
321  tf2::Quaternion q3 = slerp(q1,q2,0.5);
322 
323  EXPECT_NEAR(q3.angle(q1), q2.angle(q3), 1e-5);
324  }
325 
326 }
327 
328 
329 TEST(TimeCache, AngularInterpolation)
330 {
331  uint64_t runs = 100;
332  double epsilon = 1e-6;
333  seed_rand();
334 
335  tf2::TimeCache cache;
336  std::vector<double> yawvalues(2);
337  std::vector<double> pitchvalues(2);
338  std::vector<double> rollvalues(2);
339  uint64_t offset = 200;
340 
341  std::vector<tf2::Quaternion> quats(2);
342 
343  TransformStorage stor;
344  setIdentity(stor);
345 
346  for ( uint64_t i = 1; i < runs ; i++ )
347  {
348 
349  for (uint64_t step = 0; step < 2 ; step++)
350  {
351  yawvalues[step] = 10.0 * get_rand() / 100.0;
352  pitchvalues[step] = 0;//10.0 * get_rand();
353  rollvalues[step] = 0;//10.0 * get_rand();
354  quats[step].setRPY(yawvalues[step], pitchvalues[step], rollvalues[step]);
355  stor.rotation_ = quats[step];
356  stor.frame_id_ = 3;
357  stor.stamp_ = ros::Time().fromNSec(offset + (step * 100)); //step = 0 or 1
358  cache.insertData(stor);
359  }
360 
361  for (int pos = 0; pos < 100 ; pos ++)
362  {
363  uint64_t time = offset + pos;
364  cache.getData(ros::Time().fromNSec(time), stor);
365  uint64_t time_out = stor.stamp_.toNSec();
366  tf2::Quaternion quat (stor.rotation_);
367 
368  //Generate a ground truth quaternion directly calling slerp
369  tf2::Quaternion ground_truth = quats[0].slerp(quats[1], pos/100.0);
370 
371  //Make sure the transformed one and the direct call match
372  EXPECT_EQ(time, time_out);
373  EXPECT_NEAR(0, angle(ground_truth, quat), epsilon);
374 
375  }
376 
377  cache.clearList();
378  }
379 
380 
381 }
382 
383 TEST(TimeCache, DuplicateEntries)
384 {
385 
386  TimeCache cache;
387 
388  TransformStorage stor;
389  setIdentity(stor);
390  stor.frame_id_ = 3;
391  stor.stamp_ = ros::Time().fromNSec(1);
392 
393  cache.insertData(stor);
394 
395  cache.insertData(stor);
396 
397 
398  cache.getData(ros::Time().fromNSec(1), stor);
399 
400  //printf(" stor is %f\n", stor.translation_.x());
401  EXPECT_TRUE(!std::isnan(stor.translation_.x()));
402  EXPECT_TRUE(!std::isnan(stor.translation_.y()));
403  EXPECT_TRUE(!std::isnan(stor.translation_.z()));
404  EXPECT_TRUE(!std::isnan(stor.rotation_.x()));
405  EXPECT_TRUE(!std::isnan(stor.rotation_.y()));
406  EXPECT_TRUE(!std::isnan(stor.rotation_.z()));
407  EXPECT_TRUE(!std::isnan(stor.rotation_.w()));
408 }
409 
410 int main(int argc, char **argv){
411  testing::InitGoogleTest(&argc, argv);
412  return RUN_ALL_TESTS();
413 }
414 
void setIdentity(geometry_msgs::Transform &tx)
Definition: buffer_core.cpp:92
double epsilon
TEST(TimeCache, Repeatability)
TF2SIMD_FORCE_INLINE tf2Scalar angle(const Quaternion &q1, const Quaternion &q2)
Return the half angle between two quaternions.
Definition: Quaternion.h:407
Time & fromNSec(uint64_t t)
std::vector< double > values
tf2Scalar angle(const Quaternion &q) const
Return the half angle between this quaternion and the other.
Definition: Quaternion.h:212
uint64_t toNSec() const
Quaternion slerp(const Quaternion &q, const tf2Scalar &t) const
Return the quaternion which is the result of Spherical Linear Interpolation between this and the othe...
Definition: Quaternion.h:316
virtual bool insertData(const TransformStorage &new_data)
Insert data into the cache.
Definition: cache.cpp:245
tf2::Quaternion rotation_
int main(int argc, char **argv)
Storage for transforms and their parent.
void setEuler(const tf2Scalar &yaw, const tf2Scalar &pitch, const tf2Scalar &roll)
Set the quaternion using Euler angles.
Definition: Quaternion.h:75
CompactFrameID frame_id_
unsigned int step
virtual void clearList()
Clear the list of stored values.
Definition: cache.cpp:270
A class to keep a sorted linked list in time This builds and maintains a list of timestamped data...
Definition: time_cache.h:94
void seed_rand()
double get_rand()
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
Definition: Quaternion.h:30
TF2SIMD_FORCE_INLINE Quaternion slerp(const Quaternion &q1, const Quaternion &q2, const tf2Scalar &t)
Return the result of spherical linear interpolation betwen two quaternions.
Definition: Quaternion.h:432
virtual bool getData(ros::Time time, TransformStorage &data_out, std::string *error_str=0)
Virtual methods.
Definition: cache.cpp:198


tf2
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Mon Jun 27 2022 02:43:06