$search
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2008, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 #include <gtest/gtest.h> 00036 00037 #include "ros/time.h" 00038 #include "message_filters/time_synchronizer.h" 00039 #include "message_filters/pass_through.h" 00040 #include <ros/init.h> 00041 00042 using namespace message_filters; 00043 00044 struct Header 00045 { 00046 ros::Time stamp; 00047 }; 00048 00049 00050 struct Msg 00051 { 00052 Header header; 00053 int data; 00054 }; 00055 typedef boost::shared_ptr<Msg> MsgPtr; 00056 typedef boost::shared_ptr<Msg const> MsgConstPtr; 00057 00058 namespace ros 00059 { 00060 namespace message_traits 00061 { 00062 template<> 00063 struct TimeStamp<Msg> 00064 { 00065 static ros::Time value(const Msg& m) 00066 { 00067 return m.header.stamp; 00068 } 00069 }; 00070 } 00071 } 00072 00073 class Helper 00074 { 00075 public: 00076 Helper() 00077 : count_(0) 00078 , drop_count_(0) 00079 {} 00080 00081 void cb() 00082 { 00083 ++count_; 00084 } 00085 00086 void dropcb() 00087 { 00088 ++drop_count_; 00089 } 00090 00091 int32_t count_; 00092 int32_t drop_count_; 00093 }; 00094 00095 TEST(TimeSynchronizer, compile2) 00096 { 00097 NullFilter<Msg> f0, f1; 00098 TimeSynchronizer<Msg, Msg> sync(f0, f1, 1); 00099 } 00100 00101 TEST(TimeSynchronizer, compile3) 00102 { 00103 NullFilter<Msg> f0, f1, f2; 00104 TimeSynchronizer<Msg, Msg, Msg> sync(f0, f1, f2, 1); 00105 } 00106 00107 TEST(TimeSynchronizer, compile4) 00108 { 00109 NullFilter<Msg> f0, f1, f2, f3; 00110 TimeSynchronizer<Msg, Msg, Msg, Msg> sync(f0, f1, f2, f3, 1); 00111 } 00112 00113 TEST(TimeSynchronizer, compile5) 00114 { 00115 NullFilter<Msg> f0, f1, f2, f3, f4; 00116 TimeSynchronizer<Msg, Msg, Msg, Msg, Msg> sync(f0, f1, f2, f3, f4, 1); 00117 } 00118 00119 TEST(TimeSynchronizer, compile6) 00120 { 00121 NullFilter<Msg> f0, f1, f2, f3, f4, f5; 00122 TimeSynchronizer<Msg, Msg, Msg, Msg, Msg, Msg> sync(f0, f1, f2, f3, f4, f5, 1); 00123 } 00124 00125 TEST(TimeSynchronizer, compile7) 00126 { 00127 NullFilter<Msg> f0, f1, f2, f3, f4, f5, f6; 00128 TimeSynchronizer<Msg, Msg, Msg, Msg, Msg, Msg, Msg> sync(f0, f1, f2, f3, f4, f5, f6, 1); 00129 } 00130 00131 TEST(TimeSynchronizer, compile8) 00132 { 00133 NullFilter<Msg> f0, f1, f2, f3, f4, f5, f6, f7; 00134 TimeSynchronizer<Msg, Msg, Msg, Msg, Msg, Msg, Msg, Msg> sync(f0, f1, f2, f3, f4, f5, f6, f7, 1); 00135 } 00136 00137 TEST(TimeSynchronizer, compile9) 00138 { 00139 NullFilter<Msg> f0, f1, f2, f3, f4, f5, f6, f7, f8; 00140 TimeSynchronizer<Msg, Msg, Msg, Msg, Msg, Msg, Msg, Msg, Msg> sync(f0, f1, f2, f3, f4, f5, f6, f7, f8, 1); 00141 } 00142 00143 void function2(const MsgConstPtr&, const MsgConstPtr&) {} 00144 void function3(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} 00145 void function4(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} 00146 void function5(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} 00147 void function6(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} 00148 void function7(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} 00149 void function8(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} 00150 void function9(const MsgConstPtr&, MsgConstPtr, const MsgPtr&, MsgPtr, const Msg&, Msg, const ros::MessageEvent<Msg const>&, const ros::MessageEvent<Msg>&, const MsgConstPtr&) {} 00151 00152 TEST(TimeSynchronizer, compileFunction2) 00153 { 00154 TimeSynchronizer<Msg, Msg> sync(1); 00155 sync.registerCallback(function2); 00156 } 00157 00158 TEST(TimeSynchronizer, compileFunction3) 00159 { 00160 TimeSynchronizer<Msg, Msg, Msg> sync(1); 00161 sync.registerCallback(function3); 00162 } 00163 00164 TEST(TimeSynchronizer, compileFunction4) 00165 { 00166 TimeSynchronizer<Msg, Msg, Msg, Msg> sync(1); 00167 sync.registerCallback(function4); 00168 } 00169 00170 TEST(TimeSynchronizer, compileFunction5) 00171 { 00172 TimeSynchronizer<Msg, Msg, Msg, Msg, Msg> sync(1); 00173 sync.registerCallback(function5); 00174 } 00175 00176 TEST(TimeSynchronizer, compileFunction6) 00177 { 00178 TimeSynchronizer<Msg, Msg, Msg, Msg, Msg, Msg> sync(1); 00179 sync.registerCallback(function6); 00180 } 00181 00182 TEST(TimeSynchronizer, compileFunction7) 00183 { 00184 TimeSynchronizer<Msg, Msg, Msg, Msg, Msg, Msg, Msg> sync(1); 00185 sync.registerCallback(function7); 00186 } 00187 00188 TEST(TimeSynchronizer, compileFunction8) 00189 { 00190 TimeSynchronizer<Msg, Msg, Msg, Msg, Msg, Msg, Msg, Msg> sync(1); 00191 sync.registerCallback(function8); 00192 } 00193 00194 TEST(TimeSynchronizer, compileFunction9) 00195 { 00196 TimeSynchronizer<Msg, Msg, Msg, Msg, Msg, Msg, Msg, Msg, Msg> sync(1); 00197 sync.registerCallback(function9); 00198 } 00199 00200 struct MethodHelper 00201 { 00202 void method2(const MsgConstPtr&, const MsgConstPtr&) {} 00203 void method3(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} 00204 void method4(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} 00205 void method5(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} 00206 void method6(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} 00207 void method7(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} 00208 void method8(const MsgConstPtr&, MsgConstPtr, const MsgPtr&, MsgPtr, const Msg&, Msg, const ros::MessageEvent<Msg const>&, const ros::MessageEvent<Msg>&) {} 00209 // Can only do 8 here because the object instance counts as a parameter and bind only supports 9 00210 }; 00211 00212 TEST(TimeSynchronizer, compileMethod2) 00213 { 00214 MethodHelper h; 00215 TimeSynchronizer<Msg, Msg> sync(1); 00216 sync.registerCallback(&MethodHelper::method2, &h); 00217 } 00218 00219 TEST(TimeSynchronizer, compileMethod3) 00220 { 00221 MethodHelper h; 00222 TimeSynchronizer<Msg, Msg, Msg> sync(1); 00223 sync.registerCallback(&MethodHelper::method3, &h); 00224 } 00225 00226 TEST(TimeSynchronizer, compileMethod4) 00227 { 00228 MethodHelper h; 00229 TimeSynchronizer<Msg, Msg, Msg, Msg> sync(1); 00230 sync.registerCallback(&MethodHelper::method4, &h); 00231 } 00232 00233 TEST(TimeSynchronizer, compileMethod5) 00234 { 00235 MethodHelper h; 00236 TimeSynchronizer<Msg, Msg, Msg, Msg, Msg> sync(1); 00237 sync.registerCallback(&MethodHelper::method5, &h); 00238 } 00239 00240 TEST(TimeSynchronizer, compileMethod6) 00241 { 00242 MethodHelper h; 00243 TimeSynchronizer<Msg, Msg, Msg, Msg, Msg, Msg> sync(1); 00244 sync.registerCallback(&MethodHelper::method6, &h); 00245 } 00246 00247 TEST(TimeSynchronizer, compileMethod7) 00248 { 00249 MethodHelper h; 00250 TimeSynchronizer<Msg, Msg, Msg, Msg, Msg, Msg, Msg> sync(1); 00251 sync.registerCallback(&MethodHelper::method7, &h); 00252 } 00253 00254 TEST(TimeSynchronizer, compileMethod8) 00255 { 00256 MethodHelper h; 00257 TimeSynchronizer<Msg, Msg, Msg, Msg, Msg, Msg, Msg, Msg> sync(1); 00258 sync.registerCallback(&MethodHelper::method8, &h); 00259 } 00260 00261 TEST(TimeSynchronizer, immediate2) 00262 { 00263 TimeSynchronizer<Msg, Msg> sync(1); 00264 Helper h; 00265 sync.registerCallback(boost::bind(&Helper::cb, &h)); 00266 MsgPtr m(new Msg); 00267 m->header.stamp = ros::Time::now(); 00268 00269 sync.add0(m); 00270 ASSERT_EQ(h.count_, 0); 00271 sync.add1(m); 00272 ASSERT_EQ(h.count_, 1); 00273 } 00274 00275 TEST(TimeSynchronizer, immediate3) 00276 { 00277 TimeSynchronizer<Msg, Msg, Msg> sync(1); 00278 Helper h; 00279 sync.registerCallback(boost::bind(&Helper::cb, &h)); 00280 MsgPtr m(new Msg); 00281 m->header.stamp = ros::Time::now(); 00282 00283 sync.add0(m); 00284 ASSERT_EQ(h.count_, 0); 00285 sync.add1(m); 00286 ASSERT_EQ(h.count_, 0); 00287 sync.add2(m); 00288 ASSERT_EQ(h.count_, 1); 00289 } 00290 00291 TEST(TimeSynchronizer, immediate4) 00292 { 00293 TimeSynchronizer<Msg, Msg, Msg, Msg> sync(1); 00294 Helper h; 00295 sync.registerCallback(boost::bind(&Helper::cb, &h)); 00296 MsgPtr m(new Msg); 00297 m->header.stamp = ros::Time::now(); 00298 00299 sync.add0(m); 00300 ASSERT_EQ(h.count_, 0); 00301 sync.add1(m); 00302 ASSERT_EQ(h.count_, 0); 00303 sync.add2(m); 00304 ASSERT_EQ(h.count_, 0); 00305 sync.add3(m); 00306 ASSERT_EQ(h.count_, 1); 00307 } 00308 00309 TEST(TimeSynchronizer, immediate5) 00310 { 00311 TimeSynchronizer<Msg, Msg, Msg, Msg, Msg> sync(1); 00312 Helper h; 00313 sync.registerCallback(boost::bind(&Helper::cb, &h)); 00314 MsgPtr m(new Msg); 00315 m->header.stamp = ros::Time::now(); 00316 00317 sync.add0(m); 00318 ASSERT_EQ(h.count_, 0); 00319 sync.add1(m); 00320 ASSERT_EQ(h.count_, 0); 00321 sync.add2(m); 00322 ASSERT_EQ(h.count_, 0); 00323 sync.add3(m); 00324 ASSERT_EQ(h.count_, 0); 00325 sync.add4(m); 00326 ASSERT_EQ(h.count_, 1); 00327 } 00328 00329 TEST(TimeSynchronizer, immediate6) 00330 { 00331 TimeSynchronizer<Msg, Msg, Msg, Msg, Msg, Msg> sync(1); 00332 Helper h; 00333 sync.registerCallback(boost::bind(&Helper::cb, &h)); 00334 MsgPtr m(new Msg); 00335 m->header.stamp = ros::Time::now(); 00336 00337 sync.add0(m); 00338 ASSERT_EQ(h.count_, 0); 00339 sync.add1(m); 00340 ASSERT_EQ(h.count_, 0); 00341 sync.add2(m); 00342 ASSERT_EQ(h.count_, 0); 00343 sync.add3(m); 00344 ASSERT_EQ(h.count_, 0); 00345 sync.add4(m); 00346 ASSERT_EQ(h.count_, 0); 00347 sync.add5(m); 00348 ASSERT_EQ(h.count_, 1); 00349 } 00350 00351 TEST(TimeSynchronizer, immediate7) 00352 { 00353 TimeSynchronizer<Msg, Msg, Msg, Msg, Msg, Msg, Msg> sync(1); 00354 Helper h; 00355 sync.registerCallback(boost::bind(&Helper::cb, &h)); 00356 MsgPtr m(new Msg); 00357 m->header.stamp = ros::Time::now(); 00358 00359 sync.add0(m); 00360 ASSERT_EQ(h.count_, 0); 00361 sync.add1(m); 00362 ASSERT_EQ(h.count_, 0); 00363 sync.add2(m); 00364 ASSERT_EQ(h.count_, 0); 00365 sync.add3(m); 00366 ASSERT_EQ(h.count_, 0); 00367 sync.add4(m); 00368 ASSERT_EQ(h.count_, 0); 00369 sync.add5(m); 00370 ASSERT_EQ(h.count_, 0); 00371 sync.add6(m); 00372 ASSERT_EQ(h.count_, 1); 00373 } 00374 00375 TEST(TimeSynchronizer, immediate8) 00376 { 00377 TimeSynchronizer<Msg, Msg, Msg, Msg, Msg, Msg, Msg, Msg> sync(1); 00378 Helper h; 00379 sync.registerCallback(boost::bind(&Helper::cb, &h)); 00380 MsgPtr m(new Msg); 00381 m->header.stamp = ros::Time::now(); 00382 00383 sync.add0(m); 00384 ASSERT_EQ(h.count_, 0); 00385 sync.add1(m); 00386 ASSERT_EQ(h.count_, 0); 00387 sync.add2(m); 00388 ASSERT_EQ(h.count_, 0); 00389 sync.add3(m); 00390 ASSERT_EQ(h.count_, 0); 00391 sync.add4(m); 00392 ASSERT_EQ(h.count_, 0); 00393 sync.add5(m); 00394 ASSERT_EQ(h.count_, 0); 00395 sync.add6(m); 00396 ASSERT_EQ(h.count_, 0); 00397 sync.add7(m); 00398 ASSERT_EQ(h.count_, 1); 00399 } 00400 00401 TEST(TimeSynchronizer, immediate9) 00402 { 00403 TimeSynchronizer<Msg, Msg, Msg, Msg, Msg, Msg, Msg, Msg, Msg> sync(1); 00404 Helper h; 00405 sync.registerCallback(boost::bind(&Helper::cb, &h)); 00406 MsgPtr m(new Msg); 00407 m->header.stamp = ros::Time::now(); 00408 00409 sync.add0(m); 00410 ASSERT_EQ(h.count_, 0); 00411 sync.add1(m); 00412 ASSERT_EQ(h.count_, 0); 00413 sync.add2(m); 00414 ASSERT_EQ(h.count_, 0); 00415 sync.add3(m); 00416 ASSERT_EQ(h.count_, 0); 00417 sync.add4(m); 00418 ASSERT_EQ(h.count_, 0); 00419 sync.add5(m); 00420 ASSERT_EQ(h.count_, 0); 00421 sync.add6(m); 00422 ASSERT_EQ(h.count_, 0); 00423 sync.add7(m); 00424 ASSERT_EQ(h.count_, 0); 00425 sync.add8(m); 00426 ASSERT_EQ(h.count_, 1); 00427 } 00428 00430 // From here on we assume that testing the 3-message version is sufficient, so as not to duplicate 00431 // tests for everywhere from 2-9 00433 TEST(TimeSynchronizer, multipleTimes) 00434 { 00435 TimeSynchronizer<Msg, Msg, Msg> sync(2); 00436 Helper h; 00437 sync.registerCallback(boost::bind(&Helper::cb, &h)); 00438 MsgPtr m(new Msg); 00439 m->header.stamp = ros::Time(); 00440 00441 sync.add0(m); 00442 ASSERT_EQ(h.count_, 0); 00443 00444 m.reset(new Msg); 00445 m->header.stamp = ros::Time(0.1); 00446 sync.add1(m); 00447 ASSERT_EQ(h.count_, 0); 00448 sync.add0(m); 00449 ASSERT_EQ(h.count_, 0); 00450 sync.add2(m); 00451 ASSERT_EQ(h.count_, 1); 00452 } 00453 00454 TEST(TimeSynchronizer, queueSize) 00455 { 00456 TimeSynchronizer<Msg, Msg, Msg> sync(1); 00457 Helper h; 00458 sync.registerCallback(boost::bind(&Helper::cb, &h)); 00459 MsgPtr m(new Msg); 00460 m->header.stamp = ros::Time(); 00461 00462 sync.add0(m); 00463 ASSERT_EQ(h.count_, 0); 00464 sync.add1(m); 00465 ASSERT_EQ(h.count_, 0); 00466 00467 m.reset(new Msg); 00468 m->header.stamp = ros::Time(0.1); 00469 sync.add1(m); 00470 ASSERT_EQ(h.count_, 0); 00471 00472 m.reset(new Msg); 00473 m->header.stamp = ros::Time(0); 00474 sync.add1(m); 00475 ASSERT_EQ(h.count_, 0); 00476 sync.add2(m); 00477 ASSERT_EQ(h.count_, 0); 00478 } 00479 00480 TEST(TimeSynchronizer, dropCallback) 00481 { 00482 TimeSynchronizer<Msg, Msg> sync(1); 00483 Helper h; 00484 sync.registerCallback(boost::bind(&Helper::cb, &h)); 00485 sync.registerDropCallback(boost::bind(&Helper::dropcb, &h)); 00486 MsgPtr m(new Msg); 00487 m->header.stamp = ros::Time(); 00488 00489 sync.add0(m); 00490 ASSERT_EQ(h.drop_count_, 0); 00491 m->header.stamp = ros::Time(0.1); 00492 sync.add0(m); 00493 00494 ASSERT_EQ(h.drop_count_, 1); 00495 } 00496 00497 struct EventHelper 00498 { 00499 void callback(const ros::MessageEvent<Msg const>& e1, const ros::MessageEvent<Msg const>& e2) 00500 { 00501 e1_ = e1; 00502 e2_ = e2; 00503 } 00504 00505 ros::MessageEvent<Msg const> e1_; 00506 ros::MessageEvent<Msg const> e2_; 00507 }; 00508 00509 TEST(TimeSynchronizer, eventInEventOut) 00510 { 00511 TimeSynchronizer<Msg, Msg> sync(2); 00512 EventHelper h; 00513 sync.registerCallback(&EventHelper::callback, &h); 00514 ros::MessageEvent<Msg const> evt(MsgPtr(new Msg), ros::Time(4)); 00515 00516 sync.add<0>(evt); 00517 sync.add<1>(evt); 00518 00519 ASSERT_TRUE(h.e1_.getMessage()); 00520 ASSERT_TRUE(h.e2_.getMessage()); 00521 ASSERT_EQ(h.e1_.getReceiptTime(), evt.getReceiptTime()); 00522 ASSERT_EQ(h.e2_.getReceiptTime(), evt.getReceiptTime()); 00523 } 00524 00525 TEST(TimeSynchronizer, connectConstructor) 00526 { 00527 PassThrough<Msg> pt1, pt2; 00528 TimeSynchronizer<Msg, Msg> sync(pt1, pt2, 1); 00529 Helper h; 00530 sync.registerCallback(boost::bind(&Helper::cb, &h)); 00531 MsgPtr m(new Msg); 00532 m->header.stamp = ros::Time::now(); 00533 00534 pt1.add(m); 00535 ASSERT_EQ(h.count_, 0); 00536 pt2.add(m); 00537 ASSERT_EQ(h.count_, 1); 00538 } 00539 00540 //TEST(TimeSynchronizer, connectToSimple) 00541 00542 int main(int argc, char **argv){ 00543 testing::InitGoogleTest(&argc, argv); 00544 ros::init(argc, argv, "blah"); 00545 00546 ros::Time::init(); 00547 ros::Time::setNow(ros::Time()); 00548 00549 return RUN_ALL_TESTS(); 00550 } 00551 00552 00553