$search
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2009, 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 #ifndef MESSAGE_FILTERS_SYNCHRONIZER_H 00036 #define MESSAGE_FILTERS_SYNCHRONIZER_H 00037 00038 #include <boost/tuple/tuple.hpp> 00039 #include <boost/shared_ptr.hpp> 00040 #include <boost/function.hpp> 00041 #include <boost/thread/mutex.hpp> 00042 #include <boost/signals.hpp> 00043 #include <boost/bind.hpp> 00044 #include <boost/type_traits/is_same.hpp> 00045 #include <boost/noncopyable.hpp> 00046 #include <boost/mpl/or.hpp> 00047 #include <boost/mpl/at.hpp> 00048 #include <boost/mpl/vector.hpp> 00049 #include <boost/function_types/function_arity.hpp> 00050 #include <boost/function_types/is_nonmember_callable_builtin.hpp> 00051 00052 #include "connection.h" 00053 #include "null_types.h" 00054 #include "signal9.h" 00055 #include <ros/message_traits.h> 00056 #include <ros/message_event.h> 00057 00058 #include <deque> 00059 #include <vector> 00060 #include <string> 00061 00062 namespace message_filters 00063 { 00064 00065 namespace mpl = boost::mpl; 00066 00067 template<class Policy> 00068 class Synchronizer : public boost::noncopyable, public Policy 00069 { 00070 public: 00071 typedef typename Policy::Messages Messages; 00072 typedef typename Policy::Events Events; 00073 typedef typename Policy::Signal Signal; 00074 typedef typename mpl::at_c<Messages, 0>::type M0; 00075 typedef typename mpl::at_c<Messages, 1>::type M1; 00076 typedef typename mpl::at_c<Messages, 2>::type M2; 00077 typedef typename mpl::at_c<Messages, 3>::type M3; 00078 typedef typename mpl::at_c<Messages, 4>::type M4; 00079 typedef typename mpl::at_c<Messages, 5>::type M5; 00080 typedef typename mpl::at_c<Messages, 6>::type M6; 00081 typedef typename mpl::at_c<Messages, 7>::type M7; 00082 typedef typename mpl::at_c<Messages, 8>::type M8; 00083 typedef typename mpl::at_c<Events, 0>::type M0Event; 00084 typedef typename mpl::at_c<Events, 1>::type M1Event; 00085 typedef typename mpl::at_c<Events, 2>::type M2Event; 00086 typedef typename mpl::at_c<Events, 3>::type M3Event; 00087 typedef typename mpl::at_c<Events, 4>::type M4Event; 00088 typedef typename mpl::at_c<Events, 5>::type M5Event; 00089 typedef typename mpl::at_c<Events, 6>::type M6Event; 00090 typedef typename mpl::at_c<Events, 7>::type M7Event; 00091 typedef typename mpl::at_c<Events, 8>::type M8Event; 00092 00093 static const uint8_t MAX_MESSAGES = 9; 00094 00095 template<class F0, class F1> 00096 Synchronizer(F0& f0, F1& f1) 00097 { 00098 connectInput(f0, f1); 00099 init(); 00100 } 00101 00102 template<class F0, class F1, class F2> 00103 Synchronizer(F0& f0, F1& f1, F2& f2) 00104 { 00105 connectInput(f0, f1, f2); 00106 init(); 00107 } 00108 00109 template<class F0, class F1, class F2, class F3> 00110 Synchronizer(F0& f0, F1& f1, F2& f2, F3& f3) 00111 { 00112 connectInput(f0, f1, f2, f3); 00113 init(); 00114 } 00115 00116 template<class F0, class F1, class F2, class F3, class F4> 00117 Synchronizer(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4) 00118 { 00119 connectInput(f0, f1, f2, f3, f4); 00120 init(); 00121 } 00122 00123 template<class F0, class F1, class F2, class F3, class F4, class F5> 00124 Synchronizer(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5) 00125 { 00126 connectInput(f0, f1, f2, f3, f4, f5); 00127 init(); 00128 } 00129 00130 template<class F0, class F1, class F2, class F3, class F4, class F5, class F6> 00131 Synchronizer(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, F6& f6) 00132 { 00133 connectInput(f0, f1, f2, f3, f4, f5, f6); 00134 init(); 00135 } 00136 00137 template<class F0, class F1, class F2, class F3, class F4, class F5, class F6, class F7> 00138 Synchronizer(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, F6& f6, F7& f7) 00139 { 00140 connectInput(f0, f1, f2, f3, f4, f5, f6, f7); 00141 init(); 00142 } 00143 00144 template<class F0, class F1, class F2, class F3, class F4, class F5, class F6, class F7, class F8> 00145 Synchronizer(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, F6& f6, F7& f7, F8& f8) 00146 { 00147 connectInput(f0, f1, f2, f3, f4, f5, f6, f7, f8); 00148 init(); 00149 } 00150 00151 Synchronizer() 00152 { 00153 init(); 00154 } 00155 00156 template<class F0, class F1> 00157 Synchronizer(const Policy& policy, F0& f0, F1& f1) 00158 : Policy(policy) 00159 { 00160 connectInput(f0, f1); 00161 init(); 00162 } 00163 00164 template<class F0, class F1, class F2> 00165 Synchronizer(const Policy& policy, F0& f0, F1& f1, F2& f2) 00166 : Policy(policy) 00167 { 00168 connectInput(f0, f1, f2); 00169 init(); 00170 } 00171 00172 template<class F0, class F1, class F2, class F3> 00173 Synchronizer(const Policy& policy, F0& f0, F1& f1, F2& f2, F3& f3) 00174 : Policy(policy) 00175 { 00176 connectInput(f0, f1, f2, f3); 00177 init(); 00178 } 00179 00180 template<class F0, class F1, class F2, class F3, class F4> 00181 Synchronizer(const Policy& policy, F0& f0, F1& f1, F2& f2, F3& f3, F4& f4) 00182 : Policy(policy) 00183 { 00184 connectInput(f0, f1, f2, f3, f4); 00185 init(); 00186 } 00187 00188 template<class F0, class F1, class F2, class F3, class F4, class F5> 00189 Synchronizer(const Policy& policy, F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5) 00190 : Policy(policy) 00191 { 00192 connectInput(f0, f1, f2, f3, f4, f5); 00193 init(); 00194 } 00195 00196 template<class F0, class F1, class F2, class F3, class F4, class F5, class F6> 00197 Synchronizer(const Policy& policy, F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, F6& f6) 00198 : Policy(policy) 00199 { 00200 connectInput(f0, f1, f2, f3, f4, f5, f6); 00201 init(); 00202 } 00203 00204 template<class F0, class F1, class F2, class F3, class F4, class F5, class F6, class F7> 00205 Synchronizer(const Policy& policy, F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, F6& f6, F7& f7) 00206 : Policy(policy) 00207 { 00208 connectInput(f0, f1, f2, f3, f4, f5, f6, f7); 00209 init(); 00210 } 00211 00212 template<class F0, class F1, class F2, class F3, class F4, class F5, class F6, class F7, class F8> 00213 Synchronizer(const Policy& policy, F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, F6& f6, F7& f7, F8& f8) 00214 : Policy(policy) 00215 { 00216 connectInput(f0, f1, f2, f3, f4, f5, f6, f7, f8); 00217 init(); 00218 } 00219 00220 Synchronizer(const Policy& policy) 00221 : Policy(policy) 00222 { 00223 init(); 00224 } 00225 00226 ~Synchronizer() 00227 { 00228 disconnectAll(); 00229 } 00230 00231 void init() 00232 { 00233 Policy::initParent(this); 00234 } 00235 00236 template<class F0, class F1> 00237 void connectInput(F0& f0, F1& f1) 00238 { 00239 NullFilter<M2> f2; 00240 connectInput(f0, f1, f2); 00241 } 00242 00243 template<class F0, class F1, class F2> 00244 void connectInput(F0& f0, F1& f1, F2& f2) 00245 { 00246 NullFilter<M3> f3; 00247 connectInput(f0, f1, f2, f3); 00248 } 00249 00250 template<class F0, class F1, class F2, class F3> 00251 void connectInput(F0& f0, F1& f1, F2& f2, F3& f3) 00252 { 00253 NullFilter<M4> f4; 00254 connectInput(f0, f1, f2, f3, f4); 00255 } 00256 00257 template<class F0, class F1, class F2, class F3, class F4> 00258 void connectInput(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4) 00259 { 00260 NullFilter<M5> f5; 00261 connectInput(f0, f1, f2, f3, f4, f5); 00262 } 00263 00264 template<class F0, class F1, class F2, class F3, class F4, class F5> 00265 void connectInput(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5) 00266 { 00267 NullFilter<M6> f6; 00268 connectInput(f0, f1, f2, f3, f4, f5, f6); 00269 } 00270 00271 template<class F0, class F1, class F2, class F3, class F4, class F5, class F6> 00272 void connectInput(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, F6& f6) 00273 { 00274 NullFilter<M7> f7; 00275 connectInput(f0, f1, f2, f3, f4, f5, f6, f7); 00276 } 00277 00278 template<class F0, class F1, class F2, class F3, class F4, class F5, class F6, class F7> 00279 void connectInput(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, F6& f6, F7& f7) 00280 { 00281 NullFilter<M8> f8; 00282 connectInput(f0, f1, f2, f3, f4, f5, f6, f7, f8); 00283 } 00284 00285 template<class F0, class F1, class F2, class F3, class F4, class F5, class F6, class F7, class F8> 00286 void connectInput(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, F6& f6, F7& f7, F8& f8) 00287 { 00288 disconnectAll(); 00289 00290 input_connections_[0] = f0.registerCallback(boost::function<void(const M0Event&)>(boost::bind(&Synchronizer::template cb<0>, this, _1))); 00291 input_connections_[1] = f1.registerCallback(boost::function<void(const M1Event&)>(boost::bind(&Synchronizer::template cb<1>, this, _1))); 00292 input_connections_[2] = f2.registerCallback(boost::function<void(const M2Event&)>(boost::bind(&Synchronizer::template cb<2>, this, _1))); 00293 input_connections_[3] = f3.registerCallback(boost::function<void(const M3Event&)>(boost::bind(&Synchronizer::template cb<3>, this, _1))); 00294 input_connections_[4] = f4.registerCallback(boost::function<void(const M4Event&)>(boost::bind(&Synchronizer::template cb<4>, this, _1))); 00295 input_connections_[5] = f5.registerCallback(boost::function<void(const M5Event&)>(boost::bind(&Synchronizer::template cb<5>, this, _1))); 00296 input_connections_[6] = f6.registerCallback(boost::function<void(const M6Event&)>(boost::bind(&Synchronizer::template cb<6>, this, _1))); 00297 input_connections_[7] = f7.registerCallback(boost::function<void(const M7Event&)>(boost::bind(&Synchronizer::template cb<7>, this, _1))); 00298 input_connections_[8] = f8.registerCallback(boost::function<void(const M8Event&)>(boost::bind(&Synchronizer::template cb<8>, this, _1))); 00299 } 00300 00301 template<class C> 00302 Connection registerCallback(C& callback) 00303 { 00304 return signal_.template addCallback(callback); 00305 } 00306 00307 template<class C> 00308 Connection registerCallback(const C& callback) 00309 { 00310 return signal_.template addCallback(callback); 00311 } 00312 00313 template<class C, typename T> 00314 Connection registerCallback(const C& callback, T* t) 00315 { 00316 return signal_.template addCallback(callback, t); 00317 } 00318 00319 template<class C, typename T> 00320 Connection registerCallback(C& callback, T* t) 00321 { 00322 return signal_.template addCallback(callback, t); 00323 } 00324 00325 void setName(const std::string& name) { name_ = name; } 00326 const std::string& getName() { return name_; } 00327 00328 00329 void signal(const M0Event& e0, const M1Event& e1, const M2Event& e2, const M3Event& e3, const M4Event& e4, 00330 const M5Event& e5, const M6Event& e6, const M7Event& e7, const M8Event& e8) 00331 { 00332 signal_.call(e0, e1, e2, e3, e4, e5, e6, e7, e8); 00333 } 00334 00335 Policy* getPolicy() { return static_cast<Policy*>(this); } 00336 00337 using Policy::add; 00338 00339 template<int i> 00340 void add(const boost::shared_ptr<typename mpl::at_c<Messages, i>::type const>& msg) 00341 { 00342 this->template add<i>(typename mpl::at_c<Events, i>::type(msg)); 00343 } 00344 00345 private: 00346 00347 void disconnectAll() 00348 { 00349 for (int i = 0; i < MAX_MESSAGES; ++i) 00350 { 00351 input_connections_[i].disconnect(); 00352 } 00353 } 00354 00355 template<int i> 00356 void cb(const typename mpl::at_c<Events, i>::type& evt) 00357 { 00358 this->add<i>(evt); 00359 } 00360 00361 uint32_t queue_size_; 00362 00363 Signal signal_; 00364 00365 Connection input_connections_[MAX_MESSAGES]; 00366 00367 std::string name_; 00368 }; 00369 00370 template<typename M0, typename M1, typename M2, typename M3, typename M4, 00371 typename M5, typename M6, typename M7, typename M8> 00372 struct PolicyBase 00373 { 00374 typedef mpl::vector<M0, M1, M2, M3, M4, M5, M6, M7, M8> Messages; 00375 typedef Signal9<M0, M1, M2, M3, M4, M5, M6, M7, M8> Signal; 00376 typedef mpl::vector<ros::MessageEvent<M0 const>, ros::MessageEvent<M1 const>, ros::MessageEvent<M2 const>, ros::MessageEvent<M3 const>, 00377 ros::MessageEvent<M4 const>, ros::MessageEvent<M5 const>, ros::MessageEvent<M6 const>, ros::MessageEvent<M7 const>, 00378 ros::MessageEvent<M8 const> > Events; 00379 typedef typename mpl::fold<Messages, mpl::int_<0>, mpl::if_<mpl::not_<boost::is_same<mpl::_2, NullType> >, mpl::next<mpl::_1>, mpl::_1> >::type RealTypeCount; 00380 typedef typename mpl::at_c<Events, 0>::type M0Event; 00381 typedef typename mpl::at_c<Events, 1>::type M1Event; 00382 typedef typename mpl::at_c<Events, 2>::type M2Event; 00383 typedef typename mpl::at_c<Events, 3>::type M3Event; 00384 typedef typename mpl::at_c<Events, 4>::type M4Event; 00385 typedef typename mpl::at_c<Events, 5>::type M5Event; 00386 typedef typename mpl::at_c<Events, 6>::type M6Event; 00387 typedef typename mpl::at_c<Events, 7>::type M7Event; 00388 typedef typename mpl::at_c<Events, 8>::type M8Event; 00389 }; 00390 00391 } // namespace message_filters 00392 00393 #endif // MESSAGE_FILTERS_SYNCHRONIZER_H