test_approximate_time_policy.cpp
Go to the documentation of this file.
1 
2 /*********************************************************************
3 * Software License Agreement (BSD License)
4 *
5 * Copyright (c) 2010, Willow Garage, Inc.
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of the Willow Garage nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *********************************************************************/
35 
36 #include <gtest/gtest.h>
39 #include <vector>
40 #include <ros/ros.h>
41 //#include <pair>
42 
43 using namespace message_filters;
44 using namespace message_filters::sync_policies;
45 
46 struct Header
47 {
48  ros::Time stamp;
49 };
50 
51 
52 struct Msg
53 {
54  Header header;
55  int data;
56 };
59 
60 namespace ros
61 {
62 namespace message_traits
63 {
64 template<>
65 struct TimeStamp<Msg>
66 {
67  static ros::Time value(const Msg& m)
68  {
69  return m.header.stamp;
70  }
71 };
72 }
73 }
74 
75 typedef std::pair<ros::Time, ros::Time> TimePair;
76 typedef std::pair<ros::Time, unsigned int> TimeAndTopic;
77 struct TimeQuad
78 {
80  {
81  time[0] = p;
82  time[1] = q;
83  time[2] = r;
84  time[3] = s;
85  }
86  ros::Time time[4];
87 };
88 
89 
90 //----------------------------------------------------------
91 // Test Class (for 2 inputs)
92 //----------------------------------------------------------
94 {
95 public:
96 
97  ApproximateTimeSynchronizerTest(const std::vector<TimeAndTopic> &input,
98  const std::vector<TimePair> &output,
99  uint32_t queue_size) :
100  input_(input), output_(output), output_position_(0), sync_(queue_size)
101  {
102  sync_.registerCallback(boost::bind(&ApproximateTimeSynchronizerTest::callback, this, _1, _2));
103  }
104 
105  void callback(const MsgConstPtr& p, const MsgConstPtr& q)
106  {
107  //printf("Call_back called\n");
108  //printf("Call back: <%f, %f>\n", p->header.stamp.toSec(), q->header.stamp.toSec());
109  ASSERT_TRUE(p);
110  ASSERT_TRUE(q);
111  ASSERT_LT(output_position_, output_.size());
112  EXPECT_EQ(output_[output_position_].first, p->header.stamp);
113  EXPECT_EQ(output_[output_position_].second, q->header.stamp);
114  ++output_position_;
115  }
116 
117  void run()
118  {
119  for (unsigned int i = 0; i < input_.size(); i++)
120  {
121  if (input_[i].second == 0)
122  {
123  MsgPtr p(boost::make_shared<Msg>());
124  p->header.stamp = input_[i].first;
125  sync_.add<0>(p);
126  }
127  else
128  {
129  MsgPtr q(boost::make_shared<Msg>());
130  q->header.stamp = input_[i].first;
131  sync_.add<1>(q);
132  }
133  }
134  //printf("Done running test\n");
135  EXPECT_EQ(output_.size(), output_position_);
136  }
137 
138 private:
139  const std::vector<TimeAndTopic> &input_;
140  const std::vector<TimePair> &output_;
141  unsigned int output_position_;
143 public:
144  Sync2 sync_;
145 };
146 
147 
148 //----------------------------------------------------------
149 // Test Class (for 4 inputs)
150 //----------------------------------------------------------
152 {
153 public:
154 
155  ApproximateTimeSynchronizerTestQuad(const std::vector<TimeAndTopic> &input,
156  const std::vector<TimeQuad> &output,
157  uint32_t queue_size) :
158  input_(input), output_(output), output_position_(0), sync_(queue_size)
159  {
160  sync_.registerCallback(boost::bind(&ApproximateTimeSynchronizerTestQuad::callback, this, _1, _2, _3, _4));
161  }
162 
163  void callback(const MsgConstPtr& p, const MsgConstPtr& q, const MsgConstPtr& r, const MsgConstPtr& s)
164  {
165  //printf("Call_back called\n");
166  //printf("Call back: <%f, %f>\n", p->header.stamp.toSec(), q->header.stamp.toSec());
167  ASSERT_TRUE(p);
168  ASSERT_TRUE(q);
169  ASSERT_TRUE(r);
170  ASSERT_TRUE(s);
171  ASSERT_LT(output_position_, output_.size());
172  EXPECT_EQ(output_[output_position_].time[0], p->header.stamp);
173  EXPECT_EQ(output_[output_position_].time[1], q->header.stamp);
174  EXPECT_EQ(output_[output_position_].time[2], r->header.stamp);
175  EXPECT_EQ(output_[output_position_].time[3], s->header.stamp);
176  ++output_position_;
177  }
178 
179  void run()
180  {
181  for (unsigned int i = 0; i < input_.size(); i++)
182  {
183  MsgPtr p(boost::make_shared<Msg>());
184  p->header.stamp = input_[i].first;
185  switch (input_[i].second)
186  {
187  case 0:
188  sync_.add<0>(p);
189  break;
190  case 1:
191  sync_.add<1>(p);
192  break;
193  case 2:
194  sync_.add<2>(p);
195  break;
196  case 3:
197  sync_.add<3>(p);
198  break;
199  }
200  }
201  //printf("Done running test\n");
202  EXPECT_EQ(output_.size(), output_position_);
203  }
204 
205 private:
206  const std::vector<TimeAndTopic> &input_;
207  const std::vector<TimeQuad> &output_;
208  unsigned int output_position_;
210 public:
211  Sync4 sync_;
212 };
213 
214 
215 //----------------------------------------------------------
216 // Test Suite
217 //----------------------------------------------------------
218 TEST(ApproxTimeSync, ExactMatch) {
219  // Input A: a..b..c
220  // Input B: A..B..C
221  // Output: a..b..c
222  // A..B..C
223  std::vector<TimeAndTopic> input;
224  std::vector<TimePair> output;
225 
226  ros::Time t(0, 0);
227  ros::Duration s(1, 0);
228 
229  input.push_back(TimeAndTopic(t,0)); // a
230  input.push_back(TimeAndTopic(t,1)); // A
231  input.push_back(TimeAndTopic(t+s*3,0)); // b
232  input.push_back(TimeAndTopic(t+s*3,1)); // B
233  input.push_back(TimeAndTopic(t+s*6,0)); // c
234  input.push_back(TimeAndTopic(t+s*6,1)); // C
235  output.push_back(TimePair(t, t));
236  output.push_back(TimePair(t+s*3, t+s*3));
237  output.push_back(TimePair(t+s*6, t+s*6));
238 
239  ApproximateTimeSynchronizerTest sync_test(input, output, 10);
240  sync_test.run();
241 }
242 
243 
244 TEST(ApproxTimeSync, PerfectMatch) {
245  // Input A: a..b..c.
246  // Input B: .A..B..C
247  // Output: ...a..b.
248  // ...A..B.
249  std::vector<TimeAndTopic> input;
250  std::vector<TimePair> output;
251 
252  ros::Time t(0, 0);
253  ros::Duration s(1, 0);
254 
255  input.push_back(TimeAndTopic(t,0)); // a
256  input.push_back(TimeAndTopic(t+s,1)); // A
257  input.push_back(TimeAndTopic(t+s*3,0)); // b
258  input.push_back(TimeAndTopic(t+s*4,1)); // B
259  input.push_back(TimeAndTopic(t+s*6,0)); // c
260  input.push_back(TimeAndTopic(t+s*7,1)); // C
261  output.push_back(TimePair(t, t+s));
262  output.push_back(TimePair(t+s*3, t+s*4));
263 
264  ApproximateTimeSynchronizerTest sync_test(input, output, 10);
265  sync_test.run();
266 }
267 
268 
269 TEST(ApproxTimeSync, ImperfectMatch) {
270  // Input A: a.xb..c.
271  // Input B: .A...B.C
272  // Output: ..a...c.
273  // ..A...B.
274  std::vector<TimeAndTopic> input;
275  std::vector<TimePair> output;
276 
277  ros::Time t(0, 0);
278  ros::Duration s(1, 0);
279 
280  input.push_back(TimeAndTopic(t,0)); // a
281  input.push_back(TimeAndTopic(t+s,1)); // A
282  input.push_back(TimeAndTopic(t+s*2,0)); // x
283  input.push_back(TimeAndTopic(t+s*3,0)); // b
284  input.push_back(TimeAndTopic(t+s*5,1)); // B
285  input.push_back(TimeAndTopic(t+s*6,0)); // c
286  input.push_back(TimeAndTopic(t+s*7,1)); // C
287  output.push_back(TimePair(t, t+s));
288  output.push_back(TimePair(t+s*6, t+s*5));
289 
290  ApproximateTimeSynchronizerTest sync_test(input, output, 10);
291  sync_test.run();
292 }
293 
294 
295 TEST(ApproxTimeSync, Acceleration) {
296  // Time: 0123456789012345678
297  // Input A: a...........b....c.
298  // Input B: .......A.......B..C
299  // Output: ............b.....c
300  // ............A.....C
301  std::vector<TimeAndTopic> input;
302  std::vector<TimePair> output;
303 
304  ros::Time t(0, 0);
305  ros::Duration s(1, 0);
306 
307  input.push_back(TimeAndTopic(t,0)); // a
308  input.push_back(TimeAndTopic(t+s*7,1)); // A
309  input.push_back(TimeAndTopic(t+s*12,0)); // b
310  input.push_back(TimeAndTopic(t+s*15,1)); // B
311  input.push_back(TimeAndTopic(t+s*17,0)); // c
312  input.push_back(TimeAndTopic(t+s*18,1)); // C
313  output.push_back(TimePair(t+s*12, t+s*7));
314  output.push_back(TimePair(t+s*17, t+s*18));
315 
316  ApproximateTimeSynchronizerTest sync_test(input, output, 10);
317  sync_test.run();
318 }
319 
320 
321 TEST(ApproxTimeSync, DroppedMessages) {
322  // Queue size 1 (too small)
323  // Time: 012345678901234
324  // Input A: a...b...c.d..e.
325  // Input B: .A.B...C...D..E
326  // Output: .......b.....d.
327  // .......B.....D.
328  std::vector<TimeAndTopic> input;
329  std::vector<TimePair> output;
330 
331  ros::Time t(0, 0);
332  ros::Duration s(1, 0);
333 
334  input.push_back(TimeAndTopic(t,0)); // a
335  input.push_back(TimeAndTopic(t+s,1)); // A
336  input.push_back(TimeAndTopic(t+s*3,1)); // B
337  input.push_back(TimeAndTopic(t+s*4,0)); // b
338  input.push_back(TimeAndTopic(t+s*7,1)); // C
339  input.push_back(TimeAndTopic(t+s*8,0)); // c
340  input.push_back(TimeAndTopic(t+s*10,0)); // d
341  input.push_back(TimeAndTopic(t+s*11,1)); // D
342  input.push_back(TimeAndTopic(t+s*13,0)); // e
343  input.push_back(TimeAndTopic(t+s*14,1)); // E
344  output.push_back(TimePair(t+s*4, t+s*3));
345  output.push_back(TimePair(t+s*10, t+s*11));
346 
347  ApproximateTimeSynchronizerTest sync_test(input, output, 1);
348  sync_test.run();
349 
350  // Queue size 2 (just enough)
351  // Time: 012345678901234
352  // Input A: a...b...c.d..e.
353  // Input B: .A.B...C...D..E
354  // Output: ....a..b...c.d.
355  // ....A..B...C.D.
356  std::vector<TimePair> output2;
357  output2.push_back(TimePair(t, t+s));
358  output2.push_back(TimePair(t+s*4, t+s*3));
359  output2.push_back(TimePair(t+s*8, t+s*7));
360  output2.push_back(TimePair(t+s*10, t+s*11));
361 
362  ApproximateTimeSynchronizerTest sync_test2(input, output2, 2);
363  sync_test2.run();
364 }
365 
366 
367 TEST(ApproxTimeSync, LongQueue) {
368  // Queue size 5
369  // Time: 012345678901234
370  // Input A: abcdefghiklmnp.
371  // Input B: ...j......o....
372  // Output: ..........l....
373  // ..........o....
374  std::vector<TimeAndTopic> input;
375  std::vector<TimePair> output;
376 
377  ros::Time t(0, 0);
378  ros::Duration s(1, 0);
379 
380  input.push_back(TimeAndTopic(t,0)); // a
381  input.push_back(TimeAndTopic(t+s,0)); // b
382  input.push_back(TimeAndTopic(t+s*2,0)); // c
383  input.push_back(TimeAndTopic(t+s*3,0)); // d
384  input.push_back(TimeAndTopic(t+s*4,0)); // e
385  input.push_back(TimeAndTopic(t+s*5,0)); // f
386  input.push_back(TimeAndTopic(t+s*6,0)); // g
387  input.push_back(TimeAndTopic(t+s*7,0)); // h
388  input.push_back(TimeAndTopic(t+s*8,0)); // i
389  input.push_back(TimeAndTopic(t+s*3,1)); // j
390  input.push_back(TimeAndTopic(t+s*9,0)); // k
391  input.push_back(TimeAndTopic(t+s*10,0)); // l
392  input.push_back(TimeAndTopic(t+s*11,0)); // m
393  input.push_back(TimeAndTopic(t+s*12,0)); // n
394  input.push_back(TimeAndTopic(t+s*10,1)); // o
395  input.push_back(TimeAndTopic(t+s*13,0)); // l
396  output.push_back(TimePair(t+s*10, t+s*10));
397 
398  ApproximateTimeSynchronizerTest sync_test(input, output, 5);
399  sync_test.run();
400 }
401 
402 
403 TEST(ApproxTimeSync, DoublePublish) {
404  // Input A: a..b
405  // Input B: .A.B
406  // Output: ...b
407  // ...B
408  // +
409  // a
410  // A
411  std::vector<TimeAndTopic> input;
412  std::vector<TimePair> output;
413 
414  ros::Time t(0, 0);
415  ros::Duration s(1, 0);
416 
417  input.push_back(TimeAndTopic(t,0)); // a
418  input.push_back(TimeAndTopic(t+s,1)); // A
419  input.push_back(TimeAndTopic(t+s*3,1)); // B
420  input.push_back(TimeAndTopic(t+s*3,0)); // b
421  output.push_back(TimePair(t, t+s));
422  output.push_back(TimePair(t+s*3, t+s*3));
423 
424  ApproximateTimeSynchronizerTest sync_test(input, output, 10);
425  sync_test.run();
426 }
427 
428 
429 TEST(ApproxTimeSync, FourTopics) {
430  // Time: 012345678901234
431  // Input A: a....e..i.m..n.
432  // Input B: .b....g..j....o
433  // Input C: ..c...h...k....
434  // Input D: ...d.f.....l...
435  // Output: ......a....e..m
436  // ......b....g..j
437  // ......c....h..k
438  // ......d....f..l
439  std::vector<TimeAndTopic> input;
440  std::vector<TimeQuad> output;
441 
442  ros::Time t(0, 0);
443  ros::Duration s(1, 0);
444 
445  input.push_back(TimeAndTopic(t,0)); // a
446  input.push_back(TimeAndTopic(t+s,1)); // b
447  input.push_back(TimeAndTopic(t+s*2,2)); // c
448  input.push_back(TimeAndTopic(t+s*3,3)); // d
449  input.push_back(TimeAndTopic(t+s*5,0)); // e
450  input.push_back(TimeAndTopic(t+s*5,3)); // f
451  input.push_back(TimeAndTopic(t+s*6,1)); // g
452  input.push_back(TimeAndTopic(t+s*6,2)); // h
453  input.push_back(TimeAndTopic(t+s*8,0)); // i
454  input.push_back(TimeAndTopic(t+s*9,1)); // j
455  input.push_back(TimeAndTopic(t+s*10,2)); // k
456  input.push_back(TimeAndTopic(t+s*11,3)); // l
457  input.push_back(TimeAndTopic(t+s*10,0)); // m
458  input.push_back(TimeAndTopic(t+s*13,0)); // n
459  input.push_back(TimeAndTopic(t+s*14,1)); // o
460  output.push_back(TimeQuad(t, t+s, t+s*2, t+s*3));
461  output.push_back(TimeQuad(t+s*5, t+s*6, t+s*6, t+s*5));
462  output.push_back(TimeQuad(t+s*10, t+s*9, t+s*10, t+s*11));
463 
464  ApproximateTimeSynchronizerTestQuad sync_test(input, output, 10);
465  sync_test.run();
466 }
467 
468 
469 TEST(ApproxTimeSync, EarlyPublish) {
470  // Time: 012345678901234
471  // Input A: a......e
472  // Input B: .b......
473  // Input C: ..c.....
474  // Input D: ...d....
475  // Output: .......a
476  // .......b
477  // .......c
478  // .......d
479  std::vector<TimeAndTopic> input;
480  std::vector<TimeQuad> output;
481 
482  ros::Time t(0, 0);
483  ros::Duration s(1, 0);
484 
485  input.push_back(TimeAndTopic(t,0)); // a
486  input.push_back(TimeAndTopic(t+s,1)); // b
487  input.push_back(TimeAndTopic(t+s*2,2)); // c
488  input.push_back(TimeAndTopic(t+s*3,3)); // d
489  input.push_back(TimeAndTopic(t+s*7,0)); // e
490  output.push_back(TimeQuad(t, t+s, t+s*2, t+s*3));
491 
492  ApproximateTimeSynchronizerTestQuad sync_test(input, output, 10);
493  sync_test.run();
494 }
495 
496 
497 TEST(ApproxTimeSync, RateBound) {
498  // Rate bound A: 1.5
499  // Input A: a..b..c.
500  // Input B: .A..B..C
501  // Output: .a..b...
502  // .A..B...
503  std::vector<TimeAndTopic> input;
504  std::vector<TimePair> output;
505 
506  ros::Time t(0, 0);
507  ros::Duration s(1, 0);
508 
509  input.push_back(TimeAndTopic(t,0)); // a
510  input.push_back(TimeAndTopic(t+s,1)); // A
511  input.push_back(TimeAndTopic(t+s*3,0)); // b
512  input.push_back(TimeAndTopic(t+s*4,1)); // B
513  input.push_back(TimeAndTopic(t+s*6,0)); // c
514  input.push_back(TimeAndTopic(t+s*7,1)); // C
515  output.push_back(TimePair(t, t+s));
516  output.push_back(TimePair(t+s*3, t+s*4));
517 
518  ApproximateTimeSynchronizerTest sync_test(input, output, 10);
519  sync_test.sync_.setInterMessageLowerBound(0, s*1.5);
520  sync_test.run();
521 
522  // Rate bound A: 2
523  // Input A: a..b..c.
524  // Input B: .A..B..C
525  // Output: .a..b..c
526  // .A..B..C
527 
528  output.push_back(TimePair(t+s*6, t+s*7));
529 
530  ApproximateTimeSynchronizerTest sync_test2(input, output, 10);
531  sync_test2.sync_.setInterMessageLowerBound(0, s*2);
532  sync_test2.run();
533 
534 }
535 
536 
537 int main(int argc, char **argv) {
538  testing::InitGoogleTest(&argc, argv);
539  ros::init(argc, argv, "blah");
540  ros::Time::init();
541 
542  return RUN_ALL_TESTS();
543 }
std::pair< ros::Time, unsigned int > TimeAndTopic
TEST(ApproxTimeSync, ExactMatch)
void callback(const MsgConstPtr &p, const MsgConstPtr &q)
Header header
boost::shared_ptr< Msg const > MsgConstPtr
Synchronizer< ApproximateTime< Msg, Msg > > Sync2
XmlRpcServer s
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
const std::vector< TimeAndTopic > & input_
ApproximateTimeSynchronizerTestQuad(const std::vector< TimeAndTopic > &input, const std::vector< TimeQuad > &output, uint32_t queue_size)
const std::vector< TimeAndTopic > & input_
static void init()
TimeQuad(ros::Time p, ros::Time q, ros::Time r, ros::Time s)
boost::shared_ptr< Msg > MsgPtr
const std::vector< TimePair > & output_
Synchronizer< ApproximateTime< Msg, Msg, Msg, Msg > > Sync4
int main(int argc, char **argv)
std::pair< ros::Time, ros::Time > TimePair
const std::string header
void callback(const MsgConstPtr &p, const MsgConstPtr &q, const MsgConstPtr &r, const MsgConstPtr &s)
ApproximateTimeSynchronizerTest(const std::vector< TimeAndTopic > &input, const std::vector< TimePair > &output, uint32_t queue_size)
ros::Time stamp


message_filters
Author(s): Josh Faust, Vijay Pradeep
autogenerated on Wed Oct 16 2019 03:27:59