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>
32 #include <sys/time.h>
34 #include <stdexcept>
35 
36 #include <geometry_msgs/TransformStamped.h>
37 
38 #include <cmath>
39 
40 std::vector<double> values;
41 unsigned int step = 0;
42 
43 void seed_rand()
44 {
45  values.clear();
46  for (unsigned int i = 0; i < 1000; i++)
47  {
48  int pseudo_rand = std::floor(i * M_PI);
49  values.push_back(( pseudo_rand % 100)/50.0 - 1.0);
50  //printf("Seeding with %f\n", values.back());
51  }
52 };
53 
54 
55 double get_rand()
56 {
57  if (values.size() == 0) throw std::runtime_error("you need to call seed_rand first");
58  if (step >= values.size())
59  step = 0;
60  else
61  step++;
62  return values[step];
63 }
64 
65 using namespace tf2;
66 
67 
69 {
70  stor.translation_.setValue(0.0, 0.0, 0.0);
71  stor.rotation_.setValue(0.0, 0.0, 0.0, 1.0);
72 }
73 
74 TEST(TimeCache, Repeatability)
75 {
76  unsigned int runs = 100;
77 
78  tf2::TimeCache cache;
79 
80  TransformStorage stor;
81  setIdentity(stor);
82 
83  for ( uint64_t i = 1; i < runs ; i++ )
84  {
85  stor.frame_id_ = i;
86  stor.stamp_ = ros::Time().fromNSec(i);
87 
88  cache.insertData(stor);
89  }
90 
91  for ( uint64_t i = 1; i < runs ; i++ )
92 
93  {
94  cache.getData(ros::Time().fromNSec(i), stor);
95  EXPECT_EQ(stor.frame_id_, i);
96  EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i));
97  }
98 
99 }
100 
101 TEST(TimeCache, RepeatabilityReverseInsertOrder)
102 {
103  unsigned int runs = 100;
104 
105  tf2::TimeCache cache;
106 
107  TransformStorage stor;
108  setIdentity(stor);
109 
110  for ( int i = runs -1; i >= 0 ; i-- )
111  {
112  stor.frame_id_ = i;
113  stor.stamp_ = ros::Time().fromNSec(i);
114 
115  cache.insertData(stor);
116  }
117  for ( uint64_t i = 1; i < runs ; i++ )
118 
119  {
120  cache.getData(ros::Time().fromNSec(i), stor);
121  EXPECT_EQ(stor.frame_id_, i);
122  EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i));
123  }
124 
125 }
126 
127 #if 0 // jfaust: this doesn't seem to actually be testing random insertion?
128 TEST(TimeCache, RepeatabilityRandomInsertOrder)
129 {
130 
131  seed_rand();
132 
133  tf2::TimeCache cache;
134  double my_vals[] = {13,2,5,4,9,7,3,11,15,14,12,1,6,10,0,8};
135  std::vector<double> values (my_vals, my_vals + sizeof(my_vals)/sizeof(double));
136  unsigned int runs = values.size();
137 
138  TransformStorage stor;
139  setIdentity(stor);
140  for ( uint64_t i = 0; i <runs ; i++ )
141  {
142  values[i] = 10.0 * get_rand();
143  std::stringstream ss;
144  ss << values[i];
145  stor.header.frame_id = ss.str();
146  stor.frame_id_ = i;
147  stor.stamp_ = ros::Time().fromNSec(i);
148 
149  cache.insertData(stor);
150  }
151  for ( uint64_t i = 1; i < runs ; i++ )
152 
153  {
154  cache.getData(ros::Time().fromNSec(i), stor);
155  EXPECT_EQ(stor.frame_id_, i);
156  EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i));
157  std::stringstream ss;
158  ss << values[i];
159  EXPECT_EQ(stor.header.frame_id, ss.str());
160  }
161 
162 }
163 #endif
164 
165 TEST(TimeCache, ZeroAtFront)
166 {
167  uint64_t runs = 100;
168 
169  tf2::TimeCache cache;
170 
171  TransformStorage stor;
172  setIdentity(stor);
173 
174  for ( uint64_t i = 1; i < runs ; i++ )
175  {
176  stor.frame_id_ = i;
177  stor.stamp_ = ros::Time().fromNSec(i);
178 
179  cache.insertData(stor);
180  }
181 
182  stor.frame_id_ = runs;
183  stor.stamp_ = ros::Time().fromNSec(runs);
184  cache.insertData(stor);
185 
186  for ( uint64_t i = 1; i < runs ; i++ )
187 
188  {
189  cache.getData(ros::Time().fromNSec(i), stor);
190  EXPECT_EQ(stor.frame_id_, i);
191  EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i));
192  }
193 
194  cache.getData(ros::Time(), stor);
195  EXPECT_EQ(stor.frame_id_, runs);
196  EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(runs));
197 
198  stor.frame_id_ = runs;
199  stor.stamp_ = ros::Time().fromNSec(runs+1);
200  cache.insertData(stor);
201 
202 
203  //Make sure we get a different value now that a new values is added at the front
204  cache.getData(ros::Time(), stor);
205  EXPECT_EQ(stor.frame_id_, runs);
206  EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(runs+1));
207 
208 }
209 
210 TEST(TimeCache, CartesianInterpolation)
211 {
212  uint64_t runs = 100;
213  double epsilon = 2e-6;
214  seed_rand();
215 
216  tf2::TimeCache cache;
217  std::vector<double> xvalues(2);
218  std::vector<double> yvalues(2);
219  std::vector<double> zvalues(2);
220 
221  uint64_t offset = 200;
222 
223  TransformStorage stor;
224  setIdentity(stor);
225 
226  for ( uint64_t i = 1; i < runs ; i++ )
227  {
228 
229  for (uint64_t step = 0; step < 2 ; step++)
230  {
231  xvalues[step] = 10.0 * get_rand();
232  yvalues[step] = 10.0 * get_rand();
233  zvalues[step] = 10.0 * get_rand();
234 
235  stor.translation_.setValue(xvalues[step], yvalues[step], zvalues[step]);
236  stor.frame_id_ = 2;
237  stor.stamp_ = ros::Time().fromNSec(step * 100 + offset);
238  cache.insertData(stor);
239  }
240 
241  for (int pos = 0; pos < 100 ; pos ++)
242  {
243  uint64_t time = offset + pos;
244  cache.getData(ros::Time().fromNSec(time), stor);
245  uint64_t time_out = stor.stamp_.toNSec();
246  double x_out = stor.translation_.x();
247  double y_out = stor.translation_.y();
248  double z_out = stor.translation_.z();
249  // printf("pose %d, %f %f %f, expected %f %f %f\n", pos, x_out, y_out, z_out,
250  // xvalues[0] + (xvalues[1] - xvalues[0]) * (double)pos/100.,
251  // yvalues[0] + (yvalues[1] - yvalues[0]) * (double)pos/100.0,
252  // zvalues[0] + (xvalues[1] - zvalues[0]) * (double)pos/100.0);
253  EXPECT_EQ(time, time_out);
254  EXPECT_NEAR(xvalues[0] + (xvalues[1] - xvalues[0]) * (double)pos/100.0, x_out, epsilon);
255  EXPECT_NEAR(yvalues[0] + (yvalues[1] - yvalues[0]) * (double)pos/100.0, y_out, epsilon);
256  EXPECT_NEAR(zvalues[0] + (zvalues[1] - zvalues[0]) * (double)pos/100.0, z_out, epsilon);
257  }
258 
259 
260  cache.clearList();
261  }
262 
263 
264 }
265 
267 TEST(TimeCache, ReparentingInterpolationProtection)
268 {
269  double epsilon = 1e-6;
270  uint64_t offset = 555;
271 
272  seed_rand();
273 
274  tf2::TimeCache cache;
275  std::vector<double> xvalues(2);
276  std::vector<double> yvalues(2);
277  std::vector<double> zvalues(2);
278 
279  TransformStorage stor;
280  setIdentity(stor);
281 
282  for (uint64_t step = 0; step < 2 ; step++)
283  {
284  xvalues[step] = 10.0 * get_rand();
285  yvalues[step] = 10.0 * get_rand();
286  zvalues[step] = 10.0 * get_rand();
287 
288  stor.translation_.setValue(xvalues[step], yvalues[step], zvalues[step]);
289  stor.frame_id_ = step + 4;
290  stor.stamp_ = ros::Time().fromNSec(step * 100 + offset);
291  cache.insertData(stor);
292  }
293 
294  for (int pos = 0; pos < 100 ; pos ++)
295  {
296  EXPECT_TRUE(cache.getData(ros::Time().fromNSec(offset + pos), stor));
297  double x_out = stor.translation_.x();
298  double y_out = stor.translation_.y();
299  double z_out = stor.translation_.z();
300  EXPECT_NEAR(xvalues[0], x_out, epsilon);
301  EXPECT_NEAR(yvalues[0], y_out, epsilon);
302  EXPECT_NEAR(zvalues[0], z_out, epsilon);
303  }
304 }
305 
306 TEST(Bullet, Slerp)
307 {
308 
309  uint64_t runs = 100;
310  seed_rand();
311 
312  tf2::Quaternion q1, q2;
313  q1.setEuler(0,0,0);
314 
315  for (uint64_t i = 0 ; i < runs ; i++)
316  {
317  q2.setEuler(1.0 * get_rand(),
318  1.0 * get_rand(),
319  1.0 * get_rand());
320 
321 
322  tf2::Quaternion q3 = slerp(q1,q2,0.5);
323 
324  EXPECT_NEAR(q3.angle(q1), q2.angle(q3), 1e-5);
325  }
326 
327 }
328 
329 
330 TEST(TimeCache, AngularInterpolation)
331 {
332  uint64_t runs = 100;
333  double epsilon = 1e-6;
334  seed_rand();
335 
336  tf2::TimeCache cache;
337  std::vector<double> yawvalues(2);
338  std::vector<double> pitchvalues(2);
339  std::vector<double> rollvalues(2);
340  uint64_t offset = 200;
341 
342  std::vector<tf2::Quaternion> quats(2);
343 
344  TransformStorage stor;
345  setIdentity(stor);
346 
347  for ( uint64_t i = 1; i < runs ; i++ )
348  {
349 
350  for (uint64_t step = 0; step < 2 ; step++)
351  {
352  yawvalues[step] = 10.0 * get_rand() / 100.0;
353  pitchvalues[step] = 0;//10.0 * get_rand();
354  rollvalues[step] = 0;//10.0 * get_rand();
355  quats[step].setRPY(yawvalues[step], pitchvalues[step], rollvalues[step]);
356  stor.rotation_ = quats[step];
357  stor.frame_id_ = 3;
358  stor.stamp_ = ros::Time().fromNSec(offset + (step * 100)); //step = 0 or 1
359  cache.insertData(stor);
360  }
361 
362  for (int pos = 0; pos < 100 ; pos ++)
363  {
364  uint64_t time = offset + pos;
365  cache.getData(ros::Time().fromNSec(time), stor);
366  uint64_t time_out = stor.stamp_.toNSec();
367  tf2::Quaternion quat (stor.rotation_);
368 
369  //Generate a ground truth quaternion directly calling slerp
370  tf2::Quaternion ground_truth = quats[0].slerp(quats[1], pos/100.0);
371 
372  //Make sure the transformed one and the direct call match
373  EXPECT_EQ(time, time_out);
374  EXPECT_NEAR(0, angle(ground_truth, quat), epsilon);
375 
376  }
377 
378  cache.clearList();
379  }
380 
381 
382 }
383 
384 TEST(TimeCache, DuplicateEntries)
385 {
386 
387  TimeCache cache;
388 
389  TransformStorage stor;
390  setIdentity(stor);
391  stor.frame_id_ = 3;
392  stor.stamp_ = ros::Time().fromNSec(1);
393 
394  cache.insertData(stor);
395 
396  cache.insertData(stor);
397 
398 
399  cache.getData(ros::Time().fromNSec(1), stor);
400 
401  //printf(" stor is %f\n", stor.translation_.x());
402  EXPECT_TRUE(!std::isnan(stor.translation_.x()));
403  EXPECT_TRUE(!std::isnan(stor.translation_.y()));
404  EXPECT_TRUE(!std::isnan(stor.translation_.z()));
405  EXPECT_TRUE(!std::isnan(stor.rotation_.x()));
406  EXPECT_TRUE(!std::isnan(stor.rotation_.y()));
407  EXPECT_TRUE(!std::isnan(stor.rotation_.z()));
408  EXPECT_TRUE(!std::isnan(stor.rotation_.w()));
409 }
410 
411 int main(int argc, char **argv){
412  testing::InitGoogleTest(&argc, argv);
413  return RUN_ALL_TESTS();
414 }
415 
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:405
Time & fromNSec(uint64_t t)
std::vector< double > values
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)
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:314
uint64_t toNSec() const
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:73
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
tf2Scalar angle(const Quaternion &q) const
Return the ***half*** angle between this quaternion and the other.
Definition: Quaternion.h:210
void seed_rand()
double get_rand()
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
Definition: Quaternion.h:28
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:430
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 Fri Jun 7 2019 21:45:39