$search
00001 00002 /********************************************************************* 00003 * Software License Agreement (BSD License) 00004 * 00005 * Copyright (c) 2010, Willow Garage, Inc. 00006 * All rights reserved. 00007 * 00008 * Redistribution and use in source and binary forms, with or without 00009 * modification, are permitted provided that the following conditions 00010 * are met: 00011 * 00012 * * Redistributions of source code must retain the above copyright 00013 * notice, this list of conditions and the following disclaimer. 00014 * * Redistributions in binary form must reproduce the above 00015 * copyright notice, this list of conditions and the following 00016 * disclaimer in the documentation and/or other materials provided 00017 * with the distribution. 00018 * * Neither the name of the Willow Garage nor the names of its 00019 * contributors may be used to endorse or promote products derived 00020 * from this software without specific prior written permission. 00021 * 00022 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00023 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00024 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00025 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00026 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00027 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00028 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00029 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00030 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00031 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00032 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00033 * POSSIBILITY OF SUCH DAMAGE. 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 //#include <pair> 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 // Test Class (for 2 inputs) 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 //printf("Call_back called\n"); 00108 //printf("Call back: <%f, %f>\n", p->header.stamp.toSec(), q->header.stamp.toSec()); 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(new Msg); 00124 p->header.stamp = input_[i].first; 00125 sync_.add<0>(p); 00126 } 00127 else 00128 { 00129 MsgPtr q(new Msg); 00130 q->header.stamp = input_[i].first; 00131 sync_.add<1>(q); 00132 } 00133 } 00134 //printf("Done running test\n"); 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 // Test Class (for 4 inputs) 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 //printf("Call_back called\n"); 00166 //printf("Call back: <%f, %f>\n", p->header.stamp.toSec(), q->header.stamp.toSec()); 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(new 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 //printf("Done running test\n"); 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 // Test Suite 00217 //---------------------------------------------------------- 00218 TEST(ApproxTimeSync, ExactMatch) { 00219 // Input A: a..b..c 00220 // Input B: A..B..C 00221 // Output: a..b..c 00222 // A..B..C 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)); // a 00230 input.push_back(TimeAndTopic(t,1)); // A 00231 input.push_back(TimeAndTopic(t+s*3,0)); // b 00232 input.push_back(TimeAndTopic(t+s*3,1)); // B 00233 input.push_back(TimeAndTopic(t+s*6,0)); // c 00234 input.push_back(TimeAndTopic(t+s*6,1)); // C 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 // Input A: a..b..c. 00246 // Input B: .A..B..C 00247 // Output: ...a..b. 00248 // ...A..B. 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)); // a 00256 input.push_back(TimeAndTopic(t+s,1)); // A 00257 input.push_back(TimeAndTopic(t+s*3,0)); // b 00258 input.push_back(TimeAndTopic(t+s*4,1)); // B 00259 input.push_back(TimeAndTopic(t+s*6,0)); // c 00260 input.push_back(TimeAndTopic(t+s*7,1)); // C 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 // Input A: a.xb..c. 00271 // Input B: .A...B.C 00272 // Output: ..a...c. 00273 // ..A...B. 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)); // a 00281 input.push_back(TimeAndTopic(t+s,1)); // A 00282 input.push_back(TimeAndTopic(t+s*2,0)); // x 00283 input.push_back(TimeAndTopic(t+s*3,0)); // b 00284 input.push_back(TimeAndTopic(t+s*5,1)); // B 00285 input.push_back(TimeAndTopic(t+s*6,0)); // c 00286 input.push_back(TimeAndTopic(t+s*7,1)); // C 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 // Time: 0123456789012345678 00297 // Input A: a...........b....c. 00298 // Input B: .......A.......B..C 00299 // Output: ............b.....c 00300 // ............A.....C 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)); // a 00308 input.push_back(TimeAndTopic(t+s*7,1)); // A 00309 input.push_back(TimeAndTopic(t+s*12,0)); // b 00310 input.push_back(TimeAndTopic(t+s*15,1)); // B 00311 input.push_back(TimeAndTopic(t+s*17,0)); // c 00312 input.push_back(TimeAndTopic(t+s*18,1)); // C 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 // Queue size 1 (too small) 00323 // Time: 012345678901234 00324 // Input A: a...b...c.d..e. 00325 // Input B: .A.B...C...D..E 00326 // Output: .......b.....d. 00327 // .......B.....D. 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)); // a 00335 input.push_back(TimeAndTopic(t+s,1)); // A 00336 input.push_back(TimeAndTopic(t+s*3,1)); // B 00337 input.push_back(TimeAndTopic(t+s*4,0)); // b 00338 input.push_back(TimeAndTopic(t+s*7,1)); // C 00339 input.push_back(TimeAndTopic(t+s*8,0)); // c 00340 input.push_back(TimeAndTopic(t+s*10,0)); // d 00341 input.push_back(TimeAndTopic(t+s*11,1)); // D 00342 input.push_back(TimeAndTopic(t+s*13,0)); // e 00343 input.push_back(TimeAndTopic(t+s*14,1)); // E 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 // Queue size 2 (just enough) 00351 // Time: 012345678901234 00352 // Input A: a...b...c.d..e. 00353 // Input B: .A.B...C...D..E 00354 // Output: ....a..b...c.d. 00355 // ....A..B...C.D. 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 // Queue size 5 00369 // Time: 012345678901234 00370 // Input A: abcdefghiklmnp. 00371 // Input B: ...j......o.... 00372 // Output: ..........l.... 00373 // ..........o.... 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)); // a 00381 input.push_back(TimeAndTopic(t+s,0)); // b 00382 input.push_back(TimeAndTopic(t+s*2,0)); // c 00383 input.push_back(TimeAndTopic(t+s*3,0)); // d 00384 input.push_back(TimeAndTopic(t+s*4,0)); // e 00385 input.push_back(TimeAndTopic(t+s*5,0)); // f 00386 input.push_back(TimeAndTopic(t+s*6,0)); // g 00387 input.push_back(TimeAndTopic(t+s*7,0)); // h 00388 input.push_back(TimeAndTopic(t+s*8,0)); // i 00389 input.push_back(TimeAndTopic(t+s*3,1)); // j 00390 input.push_back(TimeAndTopic(t+s*9,0)); // k 00391 input.push_back(TimeAndTopic(t+s*10,0)); // l 00392 input.push_back(TimeAndTopic(t+s*11,0)); // m 00393 input.push_back(TimeAndTopic(t+s*12,0)); // n 00394 input.push_back(TimeAndTopic(t+s*10,1)); // o 00395 input.push_back(TimeAndTopic(t+s*13,0)); // l 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 // Input A: a..b 00405 // Input B: .A.B 00406 // Output: ...b 00407 // ...B 00408 // + 00409 // a 00410 // A 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)); // a 00418 input.push_back(TimeAndTopic(t+s,1)); // A 00419 input.push_back(TimeAndTopic(t+s*3,1)); // B 00420 input.push_back(TimeAndTopic(t+s*3,0)); // b 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 // Time: 012345678901234 00431 // Input A: a....e..i.m..n. 00432 // Input B: .b....g..j....o 00433 // Input C: ..c...h...k.... 00434 // Input D: ...d.f.....l... 00435 // Output: ......a....e..m 00436 // ......b....g..j 00437 // ......c....h..k 00438 // ......d....f..l 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)); // a 00446 input.push_back(TimeAndTopic(t+s,1)); // b 00447 input.push_back(TimeAndTopic(t+s*2,2)); // c 00448 input.push_back(TimeAndTopic(t+s*3,3)); // d 00449 input.push_back(TimeAndTopic(t+s*5,0)); // e 00450 input.push_back(TimeAndTopic(t+s*5,3)); // f 00451 input.push_back(TimeAndTopic(t+s*6,1)); // g 00452 input.push_back(TimeAndTopic(t+s*6,2)); // h 00453 input.push_back(TimeAndTopic(t+s*8,0)); // i 00454 input.push_back(TimeAndTopic(t+s*9,1)); // j 00455 input.push_back(TimeAndTopic(t+s*10,2)); // k 00456 input.push_back(TimeAndTopic(t+s*11,3)); // l 00457 input.push_back(TimeAndTopic(t+s*10,0)); // m 00458 input.push_back(TimeAndTopic(t+s*13,0)); // n 00459 input.push_back(TimeAndTopic(t+s*14,1)); // o 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 // Time: 012345678901234 00471 // Input A: a......e 00472 // Input B: .b...... 00473 // Input C: ..c..... 00474 // Input D: ...d.... 00475 // Output: .......a 00476 // .......b 00477 // .......c 00478 // .......d 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)); // a 00486 input.push_back(TimeAndTopic(t+s,1)); // b 00487 input.push_back(TimeAndTopic(t+s*2,2)); // c 00488 input.push_back(TimeAndTopic(t+s*3,3)); // d 00489 input.push_back(TimeAndTopic(t+s*7,0)); // e 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 // Rate bound A: 1.5 00499 // Input A: a..b..c. 00500 // Input B: .A..B..C 00501 // Output: .a..b... 00502 // .A..B... 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)); // a 00510 input.push_back(TimeAndTopic(t+s,1)); // A 00511 input.push_back(TimeAndTopic(t+s*3,0)); // b 00512 input.push_back(TimeAndTopic(t+s*4,1)); // B 00513 input.push_back(TimeAndTopic(t+s*6,0)); // c 00514 input.push_back(TimeAndTopic(t+s*7,1)); // C 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 // Rate bound A: 2 00523 // Input A: a..b..c. 00524 // Input B: .A..B..C 00525 // Output: .a..b..c 00526 // .A..B..C 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 }