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 #include <gtest/gtest.h>
00036
00037 #include "ros/time.h"
00038 #include <ros/init.h>
00039 #include "message_filters/synchronizer.h"
00040
00041 using namespace message_filters;
00042
00043 struct Header
00044 {
00045 ros::Time stamp;
00046 };
00047
00048
00049 struct Msg
00050 {
00051 Header header;
00052 int data;
00053 };
00054 typedef boost::shared_ptr<Msg> MsgPtr;
00055 typedef boost::shared_ptr<Msg const> MsgConstPtr;
00056
00057
00058 template<typename M0, typename M1, typename M2 = NullType, typename M3 = NullType, typename M4 = NullType,
00059 typename M5 = NullType, typename M6 = NullType, typename M7 = NullType, typename M8 = NullType>
00060 struct NullPolicy : public PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8>
00061 {
00062 typedef Synchronizer<NullPolicy> Sync;
00063 typedef PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8> Super;
00064 typedef typename Super::Messages Messages;
00065 typedef typename Super::Signal Signal;
00066 typedef typename Super::Events Events;
00067 typedef typename Super::RealTypeCount RealTypeCount;
00068
00069 NullPolicy()
00070 {
00071 for (int i = 0; i < RealTypeCount::value; ++i)
00072 {
00073 added_[i] = 0;
00074 }
00075 }
00076
00077 void initParent(Sync* parent)
00078 {
00079 }
00080
00081 template<int i>
00082 void add(const typename mpl::at_c<Events, i>::type& evt)
00083 {
00084 ++added_.at(i);
00085 }
00086
00087 boost::array<int32_t, RealTypeCount::value> added_;
00088 };
00089 typedef NullPolicy<Msg, Msg> Policy2;
00090 typedef NullPolicy<Msg, Msg, Msg> Policy3;
00091 typedef NullPolicy<Msg, Msg, Msg, Msg> Policy4;
00092 typedef NullPolicy<Msg, Msg, Msg, Msg, Msg> Policy5;
00093 typedef NullPolicy<Msg, Msg, Msg, Msg, Msg, Msg> Policy6;
00094 typedef NullPolicy<Msg, Msg, Msg, Msg, Msg, Msg, Msg> Policy7;
00095 typedef NullPolicy<Msg, Msg, Msg, Msg, Msg, Msg, Msg, Msg> Policy8;
00096 typedef NullPolicy<Msg, Msg, Msg, Msg, Msg, Msg, Msg, Msg, Msg> Policy9;
00097
00098 TEST(Synchronizer, compile2)
00099 {
00100 NullFilter<Msg> f0, f1;
00101 Synchronizer<Policy2> sync(f0, f1);
00102 }
00103
00104 TEST(Synchronizer, compile3)
00105 {
00106 NullFilter<Msg> f0, f1, f2;
00107 Synchronizer<Policy3> sync(f0, f1, f2);
00108 }
00109
00110 TEST(Synchronizer, compile4)
00111 {
00112 NullFilter<Msg> f0, f1, f2, f3;
00113 Synchronizer<Policy4> sync(f0, f1, f2, f3);
00114 }
00115
00116 TEST(Synchronizer, compile5)
00117 {
00118 NullFilter<Msg> f0, f1, f2, f3, f4;
00119 Synchronizer<Policy5> sync(f0, f1, f2, f3, f4);
00120 }
00121
00122 TEST(Synchronizer, compile6)
00123 {
00124 NullFilter<Msg> f0, f1, f2, f3, f4, f5;
00125 Synchronizer<Policy6> sync(f0, f1, f2, f3, f4, f5);
00126 }
00127
00128 TEST(Synchronizer, compile7)
00129 {
00130 NullFilter<Msg> f0, f1, f2, f3, f4, f5, f6;
00131 Synchronizer<Policy7> sync(f0, f1, f2, f3, f4, f5, f6);
00132 }
00133
00134 TEST(Synchronizer, compile8)
00135 {
00136 NullFilter<Msg> f0, f1, f2, f3, f4, f5, f6, f7;
00137 Synchronizer<Policy8> sync(f0, f1, f2, f3, f4, f5, f6, f7);
00138 }
00139
00140 TEST(Synchronizer, compile9)
00141 {
00142 NullFilter<Msg> f0, f1, f2, f3, f4, f5, f6, f7, f8;
00143 Synchronizer<Policy9> sync(f0, f1, f2, f3, f4, f5, f6, f7, f8);
00144 }
00145
00146 void function2(const MsgConstPtr&, const MsgConstPtr&) {}
00147 void function3(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
00148 void function4(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
00149 void function5(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
00150 void function6(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
00151 void function7(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
00152 void function8(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
00153 void function9(const MsgConstPtr&, MsgConstPtr, const MsgPtr&, MsgPtr, const Msg&, Msg, const ros::MessageEvent<Msg const>&, const ros::MessageEvent<Msg>&, const MsgConstPtr&) {}
00154
00155 TEST(Synchronizer, compileFunction2)
00156 {
00157 Synchronizer<Policy2> sync;
00158 sync.registerCallback(function2);
00159 }
00160
00161 TEST(Synchronizer, compileFunction3)
00162 {
00163 Synchronizer<Policy3> sync;
00164 sync.registerCallback(function3);
00165 }
00166
00167 TEST(Synchronizer, compileFunction4)
00168 {
00169 Synchronizer<Policy4> sync;
00170 sync.registerCallback(function4);
00171 }
00172
00173 TEST(Synchronizer, compileFunction5)
00174 {
00175 Synchronizer<Policy5> sync;
00176 sync.registerCallback(function5);
00177 }
00178
00179 TEST(Synchronizer, compileFunction6)
00180 {
00181 Synchronizer<Policy6> sync;
00182 sync.registerCallback(function6);
00183 }
00184
00185 TEST(Synchronizer, compileFunction7)
00186 {
00187 Synchronizer<Policy7> sync;
00188 sync.registerCallback(function7);
00189 }
00190
00191 TEST(Synchronizer, compileFunction8)
00192 {
00193 Synchronizer<Policy8> sync;
00194 sync.registerCallback(function8);
00195 }
00196
00197 TEST(Synchronizer, compileFunction9)
00198 {
00199 Synchronizer<Policy9> sync;
00200 sync.registerCallback(function9);
00201 }
00202
00203 struct MethodHelper
00204 {
00205 void method2(const MsgConstPtr&, const MsgConstPtr&) {}
00206 void method3(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
00207 void method4(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
00208 void method5(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
00209 void method6(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
00210 void method7(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {}
00211 void method8(const MsgConstPtr&, MsgConstPtr, const MsgPtr&, MsgPtr, const Msg&, Msg, const ros::MessageEvent<Msg const>&, const ros::MessageEvent<Msg>&) {}
00212
00213 };
00214
00215 TEST(Synchronizer, compileMethod2)
00216 {
00217 MethodHelper h;
00218 Synchronizer<Policy2> sync;
00219 sync.registerCallback(&MethodHelper::method2, &h);
00220 }
00221
00222 TEST(Synchronizer, compileMethod3)
00223 {
00224 MethodHelper h;
00225 Synchronizer<Policy3> sync;
00226 sync.registerCallback(&MethodHelper::method3, &h);
00227 }
00228
00229 TEST(Synchronizer, compileMethod4)
00230 {
00231 MethodHelper h;
00232 Synchronizer<Policy4> sync;
00233 sync.registerCallback(&MethodHelper::method4, &h);
00234 }
00235
00236 TEST(Synchronizer, compileMethod5)
00237 {
00238 MethodHelper h;
00239 Synchronizer<Policy5> sync;
00240 sync.registerCallback(&MethodHelper::method5, &h);
00241 }
00242
00243 TEST(Synchronizer, compileMethod6)
00244 {
00245 MethodHelper h;
00246 Synchronizer<Policy6> sync;
00247 sync.registerCallback(&MethodHelper::method6, &h);
00248 }
00249
00250 TEST(Synchronizer, compileMethod7)
00251 {
00252 MethodHelper h;
00253 Synchronizer<Policy7> sync;
00254 sync.registerCallback(&MethodHelper::method7, &h);
00255 }
00256
00257 TEST(Synchronizer, compileMethod8)
00258 {
00259 MethodHelper h;
00260 Synchronizer<Policy8> sync;
00261 sync.registerCallback(&MethodHelper::method8, &h);
00262 }
00263
00264 TEST(Synchronizer, add2)
00265 {
00266 Synchronizer<Policy2> sync;
00267 MsgPtr m(new Msg);
00268
00269 ASSERT_EQ(sync.added_[0], 0);
00270 sync.add<0>(m);
00271 ASSERT_EQ(sync.added_[0], 1);
00272 ASSERT_EQ(sync.added_[1], 0);
00273 sync.add<1>(m);
00274 ASSERT_EQ(sync.added_[1], 1);
00275 }
00276
00277 TEST(Synchronizer, add3)
00278 {
00279 Synchronizer<Policy3> sync;
00280 MsgPtr m(new Msg);
00281
00282 ASSERT_EQ(sync.added_[0], 0);
00283 sync.add<0>(m);
00284 ASSERT_EQ(sync.added_[0], 1);
00285 ASSERT_EQ(sync.added_[1], 0);
00286 sync.add<1>(m);
00287 ASSERT_EQ(sync.added_[1], 1);
00288 ASSERT_EQ(sync.added_[2], 0);
00289 sync.add<2>(m);
00290 ASSERT_EQ(sync.added_[2], 1);
00291 }
00292
00293 TEST(Synchronizer, add4)
00294 {
00295 Synchronizer<Policy4> sync;
00296 MsgPtr m(new Msg);
00297
00298 ASSERT_EQ(sync.added_[0], 0);
00299 sync.add<0>(m);
00300 ASSERT_EQ(sync.added_[0], 1);
00301 ASSERT_EQ(sync.added_[1], 0);
00302 sync.add<1>(m);
00303 ASSERT_EQ(sync.added_[1], 1);
00304 ASSERT_EQ(sync.added_[2], 0);
00305 sync.add<2>(m);
00306 ASSERT_EQ(sync.added_[2], 1);
00307 ASSERT_EQ(sync.added_[3], 0);
00308 sync.add<3>(m);
00309 ASSERT_EQ(sync.added_[3], 1);
00310 }
00311
00312 TEST(Synchronizer, add5)
00313 {
00314 Synchronizer<Policy5> sync;
00315 MsgPtr m(new Msg);
00316
00317 ASSERT_EQ(sync.added_[0], 0);
00318 sync.add<0>(m);
00319 ASSERT_EQ(sync.added_[0], 1);
00320 ASSERT_EQ(sync.added_[1], 0);
00321 sync.add<1>(m);
00322 ASSERT_EQ(sync.added_[1], 1);
00323 ASSERT_EQ(sync.added_[2], 0);
00324 sync.add<2>(m);
00325 ASSERT_EQ(sync.added_[2], 1);
00326 ASSERT_EQ(sync.added_[3], 0);
00327 sync.add<3>(m);
00328 ASSERT_EQ(sync.added_[3], 1);
00329 ASSERT_EQ(sync.added_[4], 0);
00330 sync.add<4>(m);
00331 ASSERT_EQ(sync.added_[4], 1);
00332 }
00333
00334 TEST(Synchronizer, add6)
00335 {
00336 Synchronizer<Policy6> sync;
00337 MsgPtr m(new Msg);
00338
00339 ASSERT_EQ(sync.added_[0], 0);
00340 sync.add<0>(m);
00341 ASSERT_EQ(sync.added_[0], 1);
00342 ASSERT_EQ(sync.added_[1], 0);
00343 sync.add<1>(m);
00344 ASSERT_EQ(sync.added_[1], 1);
00345 ASSERT_EQ(sync.added_[2], 0);
00346 sync.add<2>(m);
00347 ASSERT_EQ(sync.added_[2], 1);
00348 ASSERT_EQ(sync.added_[3], 0);
00349 sync.add<3>(m);
00350 ASSERT_EQ(sync.added_[3], 1);
00351 ASSERT_EQ(sync.added_[4], 0);
00352 sync.add<4>(m);
00353 ASSERT_EQ(sync.added_[4], 1);
00354 ASSERT_EQ(sync.added_[5], 0);
00355 sync.add<5>(m);
00356 ASSERT_EQ(sync.added_[5], 1);
00357 }
00358
00359 TEST(Synchronizer, add7)
00360 {
00361 Synchronizer<Policy7> sync;
00362 MsgPtr m(new Msg);
00363
00364 ASSERT_EQ(sync.added_[0], 0);
00365 sync.add<0>(m);
00366 ASSERT_EQ(sync.added_[0], 1);
00367 ASSERT_EQ(sync.added_[1], 0);
00368 sync.add<1>(m);
00369 ASSERT_EQ(sync.added_[1], 1);
00370 ASSERT_EQ(sync.added_[2], 0);
00371 sync.add<2>(m);
00372 ASSERT_EQ(sync.added_[2], 1);
00373 ASSERT_EQ(sync.added_[3], 0);
00374 sync.add<3>(m);
00375 ASSERT_EQ(sync.added_[3], 1);
00376 ASSERT_EQ(sync.added_[4], 0);
00377 sync.add<4>(m);
00378 ASSERT_EQ(sync.added_[4], 1);
00379 ASSERT_EQ(sync.added_[5], 0);
00380 sync.add<5>(m);
00381 ASSERT_EQ(sync.added_[5], 1);
00382 ASSERT_EQ(sync.added_[6], 0);
00383 sync.add<6>(m);
00384 ASSERT_EQ(sync.added_[6], 1);
00385 }
00386
00387 TEST(Synchronizer, add8)
00388 {
00389 Synchronizer<Policy8> sync;
00390 MsgPtr m(new Msg);
00391
00392 ASSERT_EQ(sync.added_[0], 0);
00393 sync.add<0>(m);
00394 ASSERT_EQ(sync.added_[0], 1);
00395 ASSERT_EQ(sync.added_[1], 0);
00396 sync.add<1>(m);
00397 ASSERT_EQ(sync.added_[1], 1);
00398 ASSERT_EQ(sync.added_[2], 0);
00399 sync.add<2>(m);
00400 ASSERT_EQ(sync.added_[2], 1);
00401 ASSERT_EQ(sync.added_[3], 0);
00402 sync.add<3>(m);
00403 ASSERT_EQ(sync.added_[3], 1);
00404 ASSERT_EQ(sync.added_[4], 0);
00405 sync.add<4>(m);
00406 ASSERT_EQ(sync.added_[4], 1);
00407 ASSERT_EQ(sync.added_[5], 0);
00408 sync.add<5>(m);
00409 ASSERT_EQ(sync.added_[5], 1);
00410 ASSERT_EQ(sync.added_[6], 0);
00411 sync.add<6>(m);
00412 ASSERT_EQ(sync.added_[6], 1);
00413 ASSERT_EQ(sync.added_[7], 0);
00414 sync.add<7>(m);
00415 ASSERT_EQ(sync.added_[7], 1);
00416 }
00417
00418 TEST(Synchronizer, add9)
00419 {
00420 Synchronizer<Policy9> sync;
00421 MsgPtr m(new Msg);
00422
00423 ASSERT_EQ(sync.added_[0], 0);
00424 sync.add<0>(m);
00425 ASSERT_EQ(sync.added_[0], 1);
00426 ASSERT_EQ(sync.added_[1], 0);
00427 sync.add<1>(m);
00428 ASSERT_EQ(sync.added_[1], 1);
00429 ASSERT_EQ(sync.added_[2], 0);
00430 sync.add<2>(m);
00431 ASSERT_EQ(sync.added_[2], 1);
00432 ASSERT_EQ(sync.added_[3], 0);
00433 sync.add<3>(m);
00434 ASSERT_EQ(sync.added_[3], 1);
00435 ASSERT_EQ(sync.added_[4], 0);
00436 sync.add<4>(m);
00437 ASSERT_EQ(sync.added_[4], 1);
00438 ASSERT_EQ(sync.added_[5], 0);
00439 sync.add<5>(m);
00440 ASSERT_EQ(sync.added_[5], 1);
00441 ASSERT_EQ(sync.added_[6], 0);
00442 sync.add<6>(m);
00443 ASSERT_EQ(sync.added_[6], 1);
00444 ASSERT_EQ(sync.added_[7], 0);
00445 sync.add<7>(m);
00446 ASSERT_EQ(sync.added_[7], 1);
00447 ASSERT_EQ(sync.added_[8], 0);
00448 sync.add<8>(m);
00449 ASSERT_EQ(sync.added_[8], 1);
00450 }
00451
00452 int main(int argc, char **argv){
00453 testing::InitGoogleTest(&argc, argv);
00454 ros::init(argc, argv, "blah");
00455
00456 ros::Time::init();
00457 ros::Time::setNow(ros::Time());
00458
00459 return RUN_ALL_TESTS();
00460 }
00461
00462