35 #ifndef MESSAGE_FILTERS_SYNC_EXACT_TIME_H 36 #define MESSAGE_FILTERS_SYNC_EXACT_TIME_H 43 #include <boost/tuple/tuple.hpp> 44 #include <boost/shared_ptr.hpp> 45 #include <boost/function.hpp> 46 #include <boost/thread/mutex.hpp> 48 #include <boost/bind.hpp> 49 #include <boost/type_traits/is_same.hpp> 50 #include <boost/noncopyable.hpp> 51 #include <boost/mpl/or.hpp> 52 #include <boost/mpl/at.hpp> 53 #include <boost/mpl/vector.hpp> 65 namespace sync_policies
68 namespace mpl = boost::mpl;
71 template<
typename M0,
typename M1,
typename M2 = NullType,
typename M3 = NullType,
typename M4 = NullType,
72 typename M5 = NullType,
typename M6 = NullType,
typename M7 = NullType,
typename M8 = NullType>
90 typedef boost::tuple<M0Event, M1Event, M2Event, M3Event, M4Event, M5Event, M6Event, M7Event, M8Event>
Tuple;
119 void add(
const typename mpl::at_c<Events, i>::type& evt)
125 boost::mutex::scoped_lock lock(
mutex_);
127 Tuple& t =
tuples_[mt::TimeStamp<typename mpl::at_c<Messages, i>::type>::value(*evt.getMessage())];
128 boost::get<i>(t) = evt;
153 template<
class C,
typename T>
163 template<
class C,
typename T>
181 full = full && (bool)boost::get<0>(t).getMessage();
182 full = full && (bool)boost::get<1>(t).getMessage();
183 full = full && (RealTypeCount::value > 2 ? (bool)boost::get<2>(t).getMessage() :
true);
184 full = full && (RealTypeCount::value > 3 ? (bool)boost::get<3>(t).getMessage() :
true);
185 full = full && (RealTypeCount::value > 4 ? (bool)boost::get<4>(t).getMessage() :
true);
186 full = full && (RealTypeCount::value > 5 ? (bool)boost::get<5>(t).getMessage() :
true);
187 full = full && (RealTypeCount::value > 6 ? (bool)boost::get<6>(t).getMessage() :
true);
188 full = full && (RealTypeCount::value > 7 ? (bool)boost::get<7>(t).getMessage() :
true);
189 full = full && (RealTypeCount::value > 8 ? (bool)boost::get<8>(t).getMessage() :
true);
193 parent_->
signal(boost::get<0>(t), boost::get<1>(t), boost::get<2>(t),
194 boost::get<3>(t), boost::get<4>(t), boost::get<5>(t),
195 boost::get<6>(t), boost::get<7>(t), boost::get<8>(t));
208 Tuple& t2 =
tuples_.begin()->second;
210 boost::get<3>(t2), boost::get<4>(t2), boost::get<5>(t2),
211 boost::get<6>(t2), boost::get<7>(t2), boost::get<8>(t2));
220 typename M_TimeToTuple::iterator it =
tuples_.begin();
221 typename M_TimeToTuple::iterator end =
tuples_.end();
226 typename M_TimeToTuple::iterator old = it;
229 Tuple& t = old->second;
231 boost::get<3>(t), boost::get<4>(t), boost::get<5>(t),
232 boost::get<6>(t), boost::get<7>(t), boost::get<8>(t));
259 #endif // MESSAGE_FILTERS_SYNC_EXACT_TIME_H
mpl::vector< ros::MessageEvent< M0 const >, ros::MessageEvent< M1 const >, ros::MessageEvent< M2 const >, ros::MessageEvent< M3 const >, ros::MessageEvent< M4 const >, ros::MessageEvent< M5 const >, ros::MessageEvent< M6 const >, ros::MessageEvent< M7 const >, ros::MessageEvent< M8 const > > Events
Connection registerDropCallback(C &callback, T *t)
std::map< ros::Time, Tuple > M_TimeToTuple
Synchronizer< ExactTime > Sync
void signal(const M0Event &e0, const M1Event &e1, const M2Event &e2, const M3Event &e3, const M4Event &e4, const M5Event &e5, const M6Event &e6, const M7Event &e7, const M8Event &e8)
void call(const M0Event &e0, const M1Event &e1, const M2Event &e2, const M3Event &e3, const M4Event &e4, const M5Event &e5, const M6Event &e6, const M7Event &e7, const M8Event &e8)
mpl::at_c< Events, 1 >::type M1Event
mpl::fold< Messages, mpl::int_< 0 >, mpl::if_< mpl::not_< boost::is_same< mpl::_2, NullType > >, mpl::next< mpl::_1 >, mpl::_1 > >::type RealTypeCount
Connection registerDropCallback(const C &callback)
Super::RealTypeCount RealTypeCount
mpl::at_c< Events, 5 >::type M5Event
mpl::at_c< Events, 2 >::type M2Event
void checkTuple(Tuple &t)
ros::Time last_signal_time_
mpl::vector< M0, M1, M2, M3, M4, M5, M6, M7, M8 > Messages
void initParent(Sync *parent)
PolicyBase< M0, M1, M2, M3, M4, M5, M6, M7, M8 > Super
mpl::at_c< Events, 0 >::type M0Event
mpl::at_c< Events, 3 >::type M3Event
ExactTime(const ExactTime &e)
Connection registerDropCallback(C &callback)
ExactTime & operator=(const ExactTime &rhs)
Connection addCallback(const boost::function< void(P0, P1, P2, P3, P4, P5, P6, P7, P8)> &callback)
Connection registerDropCallback(const C &callback, T *t)
Encapsulates a connection from one filter to another (or to a user-specified callback) ...
void add(const typename mpl::at_c< Events, i >::type &evt)
mpl::at_c< Events, 7 >::type M7Event
mpl::at_c< Events, 4 >::type M4Event
mpl::at_c< Events, 8 >::type M8Event
boost::tuple< M0Event, M1Event, M2Event, M3Event, M4Event, M5Event, M6Event, M7Event, M8Event > Tuple
ExactTime(uint32_t queue_size)
mpl::at_c< Events, 6 >::type M6Event