time_synchronizer_unittest.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 #include <gtest/gtest.h>
36 
37 #include "ros/time.h"
40 #include <ros/init.h>
41 
42 using namespace message_filters;
43 
44 struct Header
45 {
46  ros::Time stamp;
47 };
48 
49 
50 struct Msg
51 {
52  Header header;
53  int data;
54 };
57 
58 namespace ros
59 {
60 namespace message_traits
61 {
62 template<>
63 struct TimeStamp<Msg>
64 {
65  static ros::Time value(const Msg& m)
66  {
67  return m.header.stamp;
68  }
69 };
70 }
71 }
72 
73 class Helper
74 {
75 public:
77  : count_(0)
78  , drop_count_(0)
79  {}
80 
81  void cb()
82  {
83  ++count_;
84  }
85 
86  void dropcb()
87  {
88  ++drop_count_;
89  }
90 
91  int32_t count_;
92  int32_t drop_count_;
93 };
94 
96 {
97  NullFilter<Msg> f0, f1;
98  TimeSynchronizer<Msg, Msg> sync(f0, f1, 1);
99 }
100 
102 {
103  NullFilter<Msg> f0, f1, f2;
104  TimeSynchronizer<Msg, Msg, Msg> sync(f0, f1, f2, 1);
105 }
106 
108 {
109  NullFilter<Msg> f0, f1, f2, f3;
110  TimeSynchronizer<Msg, Msg, Msg, Msg> sync(f0, f1, f2, f3, 1);
111 }
112 
114 {
115  NullFilter<Msg> f0, f1, f2, f3, f4;
116  TimeSynchronizer<Msg, Msg, Msg, Msg, Msg> sync(f0, f1, f2, f3, f4, 1);
117 }
118 
120 {
121  NullFilter<Msg> f0, f1, f2, f3, f4, f5;
122  TimeSynchronizer<Msg, Msg, Msg, Msg, Msg, Msg> sync(f0, f1, f2, f3, f4, f5, 1);
123 }
124 
126 {
127  NullFilter<Msg> f0, f1, f2, f3, f4, f5, f6;
128  TimeSynchronizer<Msg, Msg, Msg, Msg, Msg, Msg, Msg> sync(f0, f1, f2, f3, f4, f5, f6, 1);
129 }
130 
132 {
133  NullFilter<Msg> f0, f1, f2, f3, f4, f5, f6, f7;
134  TimeSynchronizer<Msg, Msg, Msg, Msg, Msg, Msg, Msg, Msg> sync(f0, f1, f2, f3, f4, f5, f6, f7, 1);
135 }
136 
138 {
139  NullFilter<Msg> f0, f1, f2, f3, f4, f5, f6, f7, f8;
140  TimeSynchronizer<Msg, Msg, Msg, Msg, Msg, Msg, Msg, Msg, Msg> sync(f0, f1, f2, f3, f4, f5, f6, f7, f8, 1);
141 }
142 
143 void function2(const MsgConstPtr&, const MsgConstPtr&) {}
144 void function3(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
145 void function4(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
146 void function5(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
147 void function6(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
148 void function7(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
149 void function8(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
151 
152 TEST(TimeSynchronizer, compileFunction2)
153 {
156 }
157 
158 TEST(TimeSynchronizer, compileFunction3)
159 {
162 }
163 
164 TEST(TimeSynchronizer, compileFunction4)
165 {
168 }
169 
170 TEST(TimeSynchronizer, compileFunction5)
171 {
174 }
175 
176 TEST(TimeSynchronizer, compileFunction6)
177 {
180 }
181 
182 TEST(TimeSynchronizer, compileFunction7)
183 {
186 }
187 
188 TEST(TimeSynchronizer, compileFunction8)
189 {
192 }
193 
194 TEST(TimeSynchronizer, compileFunction9)
195 {
198 }
199 
200 struct MethodHelper
201 {
202  void method2(const MsgConstPtr&, const MsgConstPtr&) {}
203  void method3(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
204  void method4(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
205  void method5(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
206  void method6(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
207  void method7(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
209  // Can only do 8 here because the object instance counts as a parameter and bind only supports 9
210 };
211 
212 TEST(TimeSynchronizer, compileMethod2)
213 {
214  MethodHelper h;
217 }
218 
219 TEST(TimeSynchronizer, compileMethod3)
220 {
221  MethodHelper h;
224 }
225 
226 TEST(TimeSynchronizer, compileMethod4)
227 {
228  MethodHelper h;
231 }
232 
233 TEST(TimeSynchronizer, compileMethod5)
234 {
235  MethodHelper h;
238 }
239 
240 TEST(TimeSynchronizer, compileMethod6)
241 {
242  MethodHelper h;
245 }
246 
247 TEST(TimeSynchronizer, compileMethod7)
248 {
249  MethodHelper h;
252 }
253 
254 TEST(TimeSynchronizer, compileMethod8)
255 {
256  MethodHelper h;
259 }
260 
261 TEST(TimeSynchronizer, immediate2)
262 {
264  Helper h;
265  sync.registerCallback(boost::bind(&Helper::cb, &h));
266  MsgPtr m(boost::make_shared<Msg>());
267  m->header.stamp = ros::Time::now();
268 
269  sync.add0(m);
270  ASSERT_EQ(h.count_, 0);
271  sync.add1(m);
272  ASSERT_EQ(h.count_, 1);
273 }
274 
275 TEST(TimeSynchronizer, immediate3)
276 {
278  Helper h;
279  sync.registerCallback(boost::bind(&Helper::cb, &h));
280  MsgPtr m(boost::make_shared<Msg>());
281  m->header.stamp = ros::Time::now();
282 
283  sync.add0(m);
284  ASSERT_EQ(h.count_, 0);
285  sync.add1(m);
286  ASSERT_EQ(h.count_, 0);
287  sync.add2(m);
288  ASSERT_EQ(h.count_, 1);
289 }
290 
291 TEST(TimeSynchronizer, immediate4)
292 {
294  Helper h;
295  sync.registerCallback(boost::bind(&Helper::cb, &h));
296  MsgPtr m(boost::make_shared<Msg>());
297  m->header.stamp = ros::Time::now();
298 
299  sync.add0(m);
300  ASSERT_EQ(h.count_, 0);
301  sync.add1(m);
302  ASSERT_EQ(h.count_, 0);
303  sync.add2(m);
304  ASSERT_EQ(h.count_, 0);
305  sync.add3(m);
306  ASSERT_EQ(h.count_, 1);
307 }
308 
309 TEST(TimeSynchronizer, immediate5)
310 {
312  Helper h;
313  sync.registerCallback(boost::bind(&Helper::cb, &h));
314  MsgPtr m(boost::make_shared<Msg>());
315  m->header.stamp = ros::Time::now();
316 
317  sync.add0(m);
318  ASSERT_EQ(h.count_, 0);
319  sync.add1(m);
320  ASSERT_EQ(h.count_, 0);
321  sync.add2(m);
322  ASSERT_EQ(h.count_, 0);
323  sync.add3(m);
324  ASSERT_EQ(h.count_, 0);
325  sync.add4(m);
326  ASSERT_EQ(h.count_, 1);
327 }
328 
329 TEST(TimeSynchronizer, immediate6)
330 {
332  Helper h;
333  sync.registerCallback(boost::bind(&Helper::cb, &h));
334  MsgPtr m(boost::make_shared<Msg>());
335  m->header.stamp = ros::Time::now();
336 
337  sync.add0(m);
338  ASSERT_EQ(h.count_, 0);
339  sync.add1(m);
340  ASSERT_EQ(h.count_, 0);
341  sync.add2(m);
342  ASSERT_EQ(h.count_, 0);
343  sync.add3(m);
344  ASSERT_EQ(h.count_, 0);
345  sync.add4(m);
346  ASSERT_EQ(h.count_, 0);
347  sync.add5(m);
348  ASSERT_EQ(h.count_, 1);
349 }
350 
351 TEST(TimeSynchronizer, immediate7)
352 {
354  Helper h;
355  sync.registerCallback(boost::bind(&Helper::cb, &h));
356  MsgPtr m(boost::make_shared<Msg>());
357  m->header.stamp = ros::Time::now();
358 
359  sync.add0(m);
360  ASSERT_EQ(h.count_, 0);
361  sync.add1(m);
362  ASSERT_EQ(h.count_, 0);
363  sync.add2(m);
364  ASSERT_EQ(h.count_, 0);
365  sync.add3(m);
366  ASSERT_EQ(h.count_, 0);
367  sync.add4(m);
368  ASSERT_EQ(h.count_, 0);
369  sync.add5(m);
370  ASSERT_EQ(h.count_, 0);
371  sync.add6(m);
372  ASSERT_EQ(h.count_, 1);
373 }
374 
375 TEST(TimeSynchronizer, immediate8)
376 {
378  Helper h;
379  sync.registerCallback(boost::bind(&Helper::cb, &h));
380  MsgPtr m(boost::make_shared<Msg>());
381  m->header.stamp = ros::Time::now();
382 
383  sync.add0(m);
384  ASSERT_EQ(h.count_, 0);
385  sync.add1(m);
386  ASSERT_EQ(h.count_, 0);
387  sync.add2(m);
388  ASSERT_EQ(h.count_, 0);
389  sync.add3(m);
390  ASSERT_EQ(h.count_, 0);
391  sync.add4(m);
392  ASSERT_EQ(h.count_, 0);
393  sync.add5(m);
394  ASSERT_EQ(h.count_, 0);
395  sync.add6(m);
396  ASSERT_EQ(h.count_, 0);
397  sync.add7(m);
398  ASSERT_EQ(h.count_, 1);
399 }
400 
401 TEST(TimeSynchronizer, immediate9)
402 {
404  Helper h;
405  sync.registerCallback(boost::bind(&Helper::cb, &h));
406  MsgPtr m(boost::make_shared<Msg>());
407  m->header.stamp = ros::Time::now();
408 
409  sync.add0(m);
410  ASSERT_EQ(h.count_, 0);
411  sync.add1(m);
412  ASSERT_EQ(h.count_, 0);
413  sync.add2(m);
414  ASSERT_EQ(h.count_, 0);
415  sync.add3(m);
416  ASSERT_EQ(h.count_, 0);
417  sync.add4(m);
418  ASSERT_EQ(h.count_, 0);
419  sync.add5(m);
420  ASSERT_EQ(h.count_, 0);
421  sync.add6(m);
422  ASSERT_EQ(h.count_, 0);
423  sync.add7(m);
424  ASSERT_EQ(h.count_, 0);
425  sync.add8(m);
426  ASSERT_EQ(h.count_, 1);
427 }
428 
430 // From here on we assume that testing the 3-message version is sufficient, so as not to duplicate
431 // tests for everywhere from 2-9
433 TEST(TimeSynchronizer, multipleTimes)
434 {
436  Helper h;
437  sync.registerCallback(boost::bind(&Helper::cb, &h));
438  MsgPtr m(boost::make_shared<Msg>());
439  m->header.stamp = ros::Time();
440 
441  sync.add0(m);
442  ASSERT_EQ(h.count_, 0);
443 
444  m = boost::make_shared<Msg>();
445  m->header.stamp = ros::Time(0.1);
446  sync.add1(m);
447  ASSERT_EQ(h.count_, 0);
448  sync.add0(m);
449  ASSERT_EQ(h.count_, 0);
450  sync.add2(m);
451  ASSERT_EQ(h.count_, 1);
452 }
453 
455 {
457  Helper h;
458  sync.registerCallback(boost::bind(&Helper::cb, &h));
459  MsgPtr m(boost::make_shared<Msg>());
460  m->header.stamp = ros::Time();
461 
462  sync.add0(m);
463  ASSERT_EQ(h.count_, 0);
464  sync.add1(m);
465  ASSERT_EQ(h.count_, 0);
466 
467  m = boost::make_shared<Msg>();
468  m->header.stamp = ros::Time(0.1);
469  sync.add1(m);
470  ASSERT_EQ(h.count_, 0);
471 
472  m = boost::make_shared<Msg>();
473  m->header.stamp = ros::Time(0);
474  sync.add1(m);
475  ASSERT_EQ(h.count_, 0);
476  sync.add2(m);
477  ASSERT_EQ(h.count_, 0);
478 }
479 
480 TEST(TimeSynchronizer, dropCallback)
481 {
483  Helper h;
484  sync.registerCallback(boost::bind(&Helper::cb, &h));
485  sync.registerDropCallback(boost::bind(&Helper::dropcb, &h));
486  MsgPtr m(boost::make_shared<Msg>());
487  m->header.stamp = ros::Time();
488 
489  sync.add0(m);
490  ASSERT_EQ(h.drop_count_, 0);
491  m->header.stamp = ros::Time(0.1);
492  sync.add0(m);
493 
494  ASSERT_EQ(h.drop_count_, 1);
495 }
496 
497 struct EventHelper
498 {
500  {
501  e1_ = e1;
502  e2_ = e2;
503  }
504 
507 };
508 
509 TEST(TimeSynchronizer, eventInEventOut)
510 {
512  EventHelper h;
514  ros::MessageEvent<Msg const> evt(boost::make_shared<Msg>(), ros::Time(4));
515 
516  sync.add<0>(evt);
517  sync.add<1>(evt);
518 
519  ASSERT_TRUE(h.e1_.getMessage());
520  ASSERT_TRUE(h.e2_.getMessage());
521  ASSERT_EQ(h.e1_.getReceiptTime(), evt.getReceiptTime());
522  ASSERT_EQ(h.e2_.getReceiptTime(), evt.getReceiptTime());
523 }
524 
525 TEST(TimeSynchronizer, connectConstructor)
526 {
527  PassThrough<Msg> pt1, pt2;
528  TimeSynchronizer<Msg, Msg> sync(pt1, pt2, 1);
529  Helper h;
530  sync.registerCallback(boost::bind(&Helper::cb, &h));
531  MsgPtr m(boost::make_shared<Msg>());
532  m->header.stamp = ros::Time::now();
533 
534  pt1.add(m);
535  ASSERT_EQ(h.count_, 0);
536  pt2.add(m);
537  ASSERT_EQ(h.count_, 1);
538 }
539 
540 //TEST(TimeSynchronizer, connectToSimple)
541 
542 int main(int argc, char **argv){
543  testing::InitGoogleTest(&argc, argv);
544  ros::init(argc, argv, "blah");
545 
546  ros::Time::init();
548 
549  return RUN_ALL_TESTS();
550 }
551 
552 
553 
void add1(const M1ConstPtr &msg)
void function4(const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &)
void function5(const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &)
Header header
int32_t count_
Definition: test_chain.cpp:61
void function6(const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &)
boost::shared_ptr< M > getMessage() const
void add3(const M3ConstPtr &msg)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void add5(const M5ConstPtr &msg)
void method7(const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &)
void callback(const ros::MessageEvent< Msg const > &e1, const ros::MessageEvent< Msg const > &e2)
static void setNow(const Time &new_now)
boost::shared_ptr< Msg > MsgPtr
void add4(const M4ConstPtr &msg)
void method6(const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &)
Connection registerDropCallback(const C &callback)
Definition: exact_time.h:134
void add2(const M2ConstPtr &msg)
ros::MessageEvent< Msg const > e1_
static void init()
void add0(const M0ConstPtr &msg)
void function3(const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &)
void method2(const MsgConstPtr &, const MsgConstPtr &)
void add8(const M8ConstPtr &msg)
void function2(const MsgConstPtr &, const MsgConstPtr &)
void add7(const M7ConstPtr &msg)
TEST(TimeSynchronizer, compile2)
void function9(const MsgConstPtr &, MsgConstPtr, const MsgPtr &, MsgPtr, const Msg &, Msg, const ros::MessageEvent< Msg const > &, const ros::MessageEvent< Msg > &, const MsgConstPtr &)
void add6(const M6ConstPtr &msg)
void function7(const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &)
ros::Time getReceiptTime() const
boost::shared_ptr< Msg const > MsgConstPtr
ros::MessageEvent< Msg const > e2_
void method8(const MsgConstPtr &, MsgConstPtr, const MsgPtr &, MsgPtr, const Msg &, Msg, const ros::MessageEvent< Msg const > &, const ros::MessageEvent< Msg > &)
void method4(const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &)
void add(const MConstPtr &msg)
Definition: pass_through.h:71
static Time now()
void method3(const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &)
const std::string header
int main(int argc, char **argv)
void add(const boost::shared_ptr< typename mpl::at_c< Messages, i >::type const > &msg)
Definition: synchronizer.h:340
Synchronizes up to 9 messages by their timestamps.
void function8(const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &)
void method5(const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &)
ros::Time stamp


message_filters
Author(s): Josh Faust, Vijay Pradeep, Dirk Thomas
autogenerated on Mon Feb 28 2022 23:33:49