00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #include <gtest/gtest.h>
00037 #include "message_filters/synchronizer.h"
00038 #include "message_filters/sync_policies/approximate_time.h"
00039 #include <vector>
00040 #include <ros/ros.h>
00041
00042
00043 using namespace message_filters;
00044 using namespace message_filters::sync_policies;
00045
00046 struct Header
00047 {
00048 ros::Time stamp;
00049 };
00050
00051
00052 struct Msg
00053 {
00054 Header header;
00055 int data;
00056 };
00057 typedef boost::shared_ptr<Msg> MsgPtr;
00058 typedef boost::shared_ptr<Msg const> MsgConstPtr;
00059
00060 namespace ros
00061 {
00062 namespace message_traits
00063 {
00064 template<>
00065 struct TimeStamp<Msg>
00066 {
00067 static ros::Time value(const Msg& m)
00068 {
00069 return m.header.stamp;
00070 }
00071 };
00072 }
00073 }
00074
00075 typedef std::pair<ros::Time, ros::Time> TimePair;
00076 typedef std::pair<ros::Time, unsigned int> TimeAndTopic;
00077 struct TimeQuad
00078 {
00079 TimeQuad(ros::Time p, ros::Time q, ros::Time r, ros::Time s)
00080 {
00081 time[0] = p;
00082 time[1] = q;
00083 time[2] = r;
00084 time[3] = s;
00085 }
00086 ros::Time time[4];
00087 };
00088
00089
00090
00091
00092
00093 class ApproximateTimeSynchronizerTest
00094 {
00095 public:
00096
00097 ApproximateTimeSynchronizerTest(const std::vector<TimeAndTopic> &input,
00098 const std::vector<TimePair> &output,
00099 uint32_t queue_size) :
00100 input_(input), output_(output), output_position_(0), sync_(queue_size)
00101 {
00102 sync_.registerCallback(boost::bind(&ApproximateTimeSynchronizerTest::callback, this, _1, _2));
00103 }
00104
00105 void callback(const MsgConstPtr& p, const MsgConstPtr& q)
00106 {
00107
00108
00109 ASSERT_TRUE(p);
00110 ASSERT_TRUE(q);
00111 ASSERT_LT(output_position_, output_.size());
00112 EXPECT_EQ(output_[output_position_].first, p->header.stamp);
00113 EXPECT_EQ(output_[output_position_].second, q->header.stamp);
00114 ++output_position_;
00115 }
00116
00117 void run()
00118 {
00119 for (unsigned int i = 0; i < input_.size(); i++)
00120 {
00121 if (input_[i].second == 0)
00122 {
00123 MsgPtr p(boost::make_shared<Msg>());
00124 p->header.stamp = input_[i].first;
00125 sync_.add<0>(p);
00126 }
00127 else
00128 {
00129 MsgPtr q(boost::make_shared<Msg>());
00130 q->header.stamp = input_[i].first;
00131 sync_.add<1>(q);
00132 }
00133 }
00134
00135 EXPECT_EQ(output_.size(), output_position_);
00136 }
00137
00138 private:
00139 const std::vector<TimeAndTopic> &input_;
00140 const std::vector<TimePair> &output_;
00141 unsigned int output_position_;
00142 typedef Synchronizer<ApproximateTime<Msg, Msg> > Sync2;
00143 public:
00144 Sync2 sync_;
00145 };
00146
00147
00148
00149
00150
00151 class ApproximateTimeSynchronizerTestQuad
00152 {
00153 public:
00154
00155 ApproximateTimeSynchronizerTestQuad(const std::vector<TimeAndTopic> &input,
00156 const std::vector<TimeQuad> &output,
00157 uint32_t queue_size) :
00158 input_(input), output_(output), output_position_(0), sync_(queue_size)
00159 {
00160 sync_.registerCallback(boost::bind(&ApproximateTimeSynchronizerTestQuad::callback, this, _1, _2, _3, _4));
00161 }
00162
00163 void callback(const MsgConstPtr& p, const MsgConstPtr& q, const MsgConstPtr& r, const MsgConstPtr& s)
00164 {
00165
00166
00167 ASSERT_TRUE(p);
00168 ASSERT_TRUE(q);
00169 ASSERT_TRUE(r);
00170 ASSERT_TRUE(s);
00171 ASSERT_LT(output_position_, output_.size());
00172 EXPECT_EQ(output_[output_position_].time[0], p->header.stamp);
00173 EXPECT_EQ(output_[output_position_].time[1], q->header.stamp);
00174 EXPECT_EQ(output_[output_position_].time[2], r->header.stamp);
00175 EXPECT_EQ(output_[output_position_].time[3], s->header.stamp);
00176 ++output_position_;
00177 }
00178
00179 void run()
00180 {
00181 for (unsigned int i = 0; i < input_.size(); i++)
00182 {
00183 MsgPtr p(boost::make_shared<Msg>());
00184 p->header.stamp = input_[i].first;
00185 switch (input_[i].second)
00186 {
00187 case 0:
00188 sync_.add<0>(p);
00189 break;
00190 case 1:
00191 sync_.add<1>(p);
00192 break;
00193 case 2:
00194 sync_.add<2>(p);
00195 break;
00196 case 3:
00197 sync_.add<3>(p);
00198 break;
00199 }
00200 }
00201
00202 EXPECT_EQ(output_.size(), output_position_);
00203 }
00204
00205 private:
00206 const std::vector<TimeAndTopic> &input_;
00207 const std::vector<TimeQuad> &output_;
00208 unsigned int output_position_;
00209 typedef Synchronizer<ApproximateTime<Msg, Msg, Msg, Msg> > Sync4;
00210 public:
00211 Sync4 sync_;
00212 };
00213
00214
00215
00216
00217
00218 TEST(ApproxTimeSync, ExactMatch) {
00219
00220
00221
00222
00223 std::vector<TimeAndTopic> input;
00224 std::vector<TimePair> output;
00225
00226 ros::Time t(0, 0);
00227 ros::Duration s(1, 0);
00228
00229 input.push_back(TimeAndTopic(t,0));
00230 input.push_back(TimeAndTopic(t,1));
00231 input.push_back(TimeAndTopic(t+s*3,0));
00232 input.push_back(TimeAndTopic(t+s*3,1));
00233 input.push_back(TimeAndTopic(t+s*6,0));
00234 input.push_back(TimeAndTopic(t+s*6,1));
00235 output.push_back(TimePair(t, t));
00236 output.push_back(TimePair(t+s*3, t+s*3));
00237 output.push_back(TimePair(t+s*6, t+s*6));
00238
00239 ApproximateTimeSynchronizerTest sync_test(input, output, 10);
00240 sync_test.run();
00241 }
00242
00243
00244 TEST(ApproxTimeSync, PerfectMatch) {
00245
00246
00247
00248
00249 std::vector<TimeAndTopic> input;
00250 std::vector<TimePair> output;
00251
00252 ros::Time t(0, 0);
00253 ros::Duration s(1, 0);
00254
00255 input.push_back(TimeAndTopic(t,0));
00256 input.push_back(TimeAndTopic(t+s,1));
00257 input.push_back(TimeAndTopic(t+s*3,0));
00258 input.push_back(TimeAndTopic(t+s*4,1));
00259 input.push_back(TimeAndTopic(t+s*6,0));
00260 input.push_back(TimeAndTopic(t+s*7,1));
00261 output.push_back(TimePair(t, t+s));
00262 output.push_back(TimePair(t+s*3, t+s*4));
00263
00264 ApproximateTimeSynchronizerTest sync_test(input, output, 10);
00265 sync_test.run();
00266 }
00267
00268
00269 TEST(ApproxTimeSync, ImperfectMatch) {
00270
00271
00272
00273
00274 std::vector<TimeAndTopic> input;
00275 std::vector<TimePair> output;
00276
00277 ros::Time t(0, 0);
00278 ros::Duration s(1, 0);
00279
00280 input.push_back(TimeAndTopic(t,0));
00281 input.push_back(TimeAndTopic(t+s,1));
00282 input.push_back(TimeAndTopic(t+s*2,0));
00283 input.push_back(TimeAndTopic(t+s*3,0));
00284 input.push_back(TimeAndTopic(t+s*5,1));
00285 input.push_back(TimeAndTopic(t+s*6,0));
00286 input.push_back(TimeAndTopic(t+s*7,1));
00287 output.push_back(TimePair(t, t+s));
00288 output.push_back(TimePair(t+s*6, t+s*5));
00289
00290 ApproximateTimeSynchronizerTest sync_test(input, output, 10);
00291 sync_test.run();
00292 }
00293
00294
00295 TEST(ApproxTimeSync, Acceleration) {
00296
00297
00298
00299
00300
00301 std::vector<TimeAndTopic> input;
00302 std::vector<TimePair> output;
00303
00304 ros::Time t(0, 0);
00305 ros::Duration s(1, 0);
00306
00307 input.push_back(TimeAndTopic(t,0));
00308 input.push_back(TimeAndTopic(t+s*7,1));
00309 input.push_back(TimeAndTopic(t+s*12,0));
00310 input.push_back(TimeAndTopic(t+s*15,1));
00311 input.push_back(TimeAndTopic(t+s*17,0));
00312 input.push_back(TimeAndTopic(t+s*18,1));
00313 output.push_back(TimePair(t+s*12, t+s*7));
00314 output.push_back(TimePair(t+s*17, t+s*18));
00315
00316 ApproximateTimeSynchronizerTest sync_test(input, output, 10);
00317 sync_test.run();
00318 }
00319
00320
00321 TEST(ApproxTimeSync, DroppedMessages) {
00322
00323
00324
00325
00326
00327
00328 std::vector<TimeAndTopic> input;
00329 std::vector<TimePair> output;
00330
00331 ros::Time t(0, 0);
00332 ros::Duration s(1, 0);
00333
00334 input.push_back(TimeAndTopic(t,0));
00335 input.push_back(TimeAndTopic(t+s,1));
00336 input.push_back(TimeAndTopic(t+s*3,1));
00337 input.push_back(TimeAndTopic(t+s*4,0));
00338 input.push_back(TimeAndTopic(t+s*7,1));
00339 input.push_back(TimeAndTopic(t+s*8,0));
00340 input.push_back(TimeAndTopic(t+s*10,0));
00341 input.push_back(TimeAndTopic(t+s*11,1));
00342 input.push_back(TimeAndTopic(t+s*13,0));
00343 input.push_back(TimeAndTopic(t+s*14,1));
00344 output.push_back(TimePair(t+s*4, t+s*3));
00345 output.push_back(TimePair(t+s*10, t+s*11));
00346
00347 ApproximateTimeSynchronizerTest sync_test(input, output, 1);
00348 sync_test.run();
00349
00350
00351
00352
00353
00354
00355
00356 std::vector<TimePair> output2;
00357 output2.push_back(TimePair(t, t+s));
00358 output2.push_back(TimePair(t+s*4, t+s*3));
00359 output2.push_back(TimePair(t+s*8, t+s*7));
00360 output2.push_back(TimePair(t+s*10, t+s*11));
00361
00362 ApproximateTimeSynchronizerTest sync_test2(input, output2, 2);
00363 sync_test2.run();
00364 }
00365
00366
00367 TEST(ApproxTimeSync, LongQueue) {
00368
00369
00370
00371
00372
00373
00374 std::vector<TimeAndTopic> input;
00375 std::vector<TimePair> output;
00376
00377 ros::Time t(0, 0);
00378 ros::Duration s(1, 0);
00379
00380 input.push_back(TimeAndTopic(t,0));
00381 input.push_back(TimeAndTopic(t+s,0));
00382 input.push_back(TimeAndTopic(t+s*2,0));
00383 input.push_back(TimeAndTopic(t+s*3,0));
00384 input.push_back(TimeAndTopic(t+s*4,0));
00385 input.push_back(TimeAndTopic(t+s*5,0));
00386 input.push_back(TimeAndTopic(t+s*6,0));
00387 input.push_back(TimeAndTopic(t+s*7,0));
00388 input.push_back(TimeAndTopic(t+s*8,0));
00389 input.push_back(TimeAndTopic(t+s*3,1));
00390 input.push_back(TimeAndTopic(t+s*9,0));
00391 input.push_back(TimeAndTopic(t+s*10,0));
00392 input.push_back(TimeAndTopic(t+s*11,0));
00393 input.push_back(TimeAndTopic(t+s*12,0));
00394 input.push_back(TimeAndTopic(t+s*10,1));
00395 input.push_back(TimeAndTopic(t+s*13,0));
00396 output.push_back(TimePair(t+s*10, t+s*10));
00397
00398 ApproximateTimeSynchronizerTest sync_test(input, output, 5);
00399 sync_test.run();
00400 }
00401
00402
00403 TEST(ApproxTimeSync, DoublePublish) {
00404
00405
00406
00407
00408
00409
00410
00411 std::vector<TimeAndTopic> input;
00412 std::vector<TimePair> output;
00413
00414 ros::Time t(0, 0);
00415 ros::Duration s(1, 0);
00416
00417 input.push_back(TimeAndTopic(t,0));
00418 input.push_back(TimeAndTopic(t+s,1));
00419 input.push_back(TimeAndTopic(t+s*3,1));
00420 input.push_back(TimeAndTopic(t+s*3,0));
00421 output.push_back(TimePair(t, t+s));
00422 output.push_back(TimePair(t+s*3, t+s*3));
00423
00424 ApproximateTimeSynchronizerTest sync_test(input, output, 10);
00425 sync_test.run();
00426 }
00427
00428
00429 TEST(ApproxTimeSync, FourTopics) {
00430
00431
00432
00433
00434
00435
00436
00437
00438
00439 std::vector<TimeAndTopic> input;
00440 std::vector<TimeQuad> output;
00441
00442 ros::Time t(0, 0);
00443 ros::Duration s(1, 0);
00444
00445 input.push_back(TimeAndTopic(t,0));
00446 input.push_back(TimeAndTopic(t+s,1));
00447 input.push_back(TimeAndTopic(t+s*2,2));
00448 input.push_back(TimeAndTopic(t+s*3,3));
00449 input.push_back(TimeAndTopic(t+s*5,0));
00450 input.push_back(TimeAndTopic(t+s*5,3));
00451 input.push_back(TimeAndTopic(t+s*6,1));
00452 input.push_back(TimeAndTopic(t+s*6,2));
00453 input.push_back(TimeAndTopic(t+s*8,0));
00454 input.push_back(TimeAndTopic(t+s*9,1));
00455 input.push_back(TimeAndTopic(t+s*10,2));
00456 input.push_back(TimeAndTopic(t+s*11,3));
00457 input.push_back(TimeAndTopic(t+s*10,0));
00458 input.push_back(TimeAndTopic(t+s*13,0));
00459 input.push_back(TimeAndTopic(t+s*14,1));
00460 output.push_back(TimeQuad(t, t+s, t+s*2, t+s*3));
00461 output.push_back(TimeQuad(t+s*5, t+s*6, t+s*6, t+s*5));
00462 output.push_back(TimeQuad(t+s*10, t+s*9, t+s*10, t+s*11));
00463
00464 ApproximateTimeSynchronizerTestQuad sync_test(input, output, 10);
00465 sync_test.run();
00466 }
00467
00468
00469 TEST(ApproxTimeSync, EarlyPublish) {
00470
00471
00472
00473
00474
00475
00476
00477
00478
00479 std::vector<TimeAndTopic> input;
00480 std::vector<TimeQuad> output;
00481
00482 ros::Time t(0, 0);
00483 ros::Duration s(1, 0);
00484
00485 input.push_back(TimeAndTopic(t,0));
00486 input.push_back(TimeAndTopic(t+s,1));
00487 input.push_back(TimeAndTopic(t+s*2,2));
00488 input.push_back(TimeAndTopic(t+s*3,3));
00489 input.push_back(TimeAndTopic(t+s*7,0));
00490 output.push_back(TimeQuad(t, t+s, t+s*2, t+s*3));
00491
00492 ApproximateTimeSynchronizerTestQuad sync_test(input, output, 10);
00493 sync_test.run();
00494 }
00495
00496
00497 TEST(ApproxTimeSync, RateBound) {
00498
00499
00500
00501
00502
00503 std::vector<TimeAndTopic> input;
00504 std::vector<TimePair> output;
00505
00506 ros::Time t(0, 0);
00507 ros::Duration s(1, 0);
00508
00509 input.push_back(TimeAndTopic(t,0));
00510 input.push_back(TimeAndTopic(t+s,1));
00511 input.push_back(TimeAndTopic(t+s*3,0));
00512 input.push_back(TimeAndTopic(t+s*4,1));
00513 input.push_back(TimeAndTopic(t+s*6,0));
00514 input.push_back(TimeAndTopic(t+s*7,1));
00515 output.push_back(TimePair(t, t+s));
00516 output.push_back(TimePair(t+s*3, t+s*4));
00517
00518 ApproximateTimeSynchronizerTest sync_test(input, output, 10);
00519 sync_test.sync_.setInterMessageLowerBound(0, s*1.5);
00520 sync_test.run();
00521
00522
00523
00524
00525
00526
00527
00528 output.push_back(TimePair(t+s*6, t+s*7));
00529
00530 ApproximateTimeSynchronizerTest sync_test2(input, output, 10);
00531 sync_test2.sync_.setInterMessageLowerBound(0, s*2);
00532 sync_test2.run();
00533
00534 }
00535
00536
00537 int main(int argc, char **argv) {
00538 testing::InitGoogleTest(&argc, argv);
00539 ros::init(argc, argv, "blah");
00540 ros::Time::init();
00541
00542 return RUN_ALL_TESTS();
00543 }