message_filters: synchronizer.h Source File

Go to the documentation of this file.
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_.addCallback(callback);
00305   }
00306 
00307   template<class C>
00308   Connection registerCallback(const C& callback)
00309   {
00310     return signal_.addCallback(callback);
00311   }
00312 
00313   template<class C, typename T>
00314   Connection registerCallback(const C& callback, T* t)
00315   {
00316     return signal_.addCallback(callback, t);
00317   }
00318 
00319   template<class C, typename T>
00320   Connection registerCallback(C& callback, T* t)
00321   {
00322     return signal_.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