35 #ifndef MESSAGE_FILTERS_SYNC_APPROXIMATE_TIME_H 36 #define MESSAGE_FILTERS_SYNC_APPROXIMATE_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;
107 typedef boost::tuple<M0Event, M1Event, M2Event, M3Event, M4Event, M5Event, M6Event, M7Event, M8Event>
Tuple;
108 typedef boost::tuple<M0Deque, M1Deque, M2Deque, M3Deque, M4Deque, M5Deque, M6Deque, M7Deque, M8Deque>
DequeTuple;
109 typedef boost::tuple<M0Vector, M1Vector, M2Vector, M3Vector, M4Vector, M5Vector, M6Vector, M7Vector, M8Vector>
VectorTuple;
163 std::deque<typename mpl::at_c<Events, i>::type>& deque = boost::get<i>(
deques_);
164 std::vector<typename mpl::at_c<Events, i>::type>& v = boost::get<i>(
past_);
166 const typename mpl::at_c<Messages, i>::type &msg = *(deque.back()).getMessage();
167 ros::Time msg_time = mt::TimeStamp<typename mpl::at_c<Messages, i>::type>::value(msg);
169 if (deque.size() == (size_t) 1)
176 const typename mpl::at_c<Messages, i>::type &previous_msg = *(v.back()).getMessage();
177 previous_msg_time = mt::TimeStamp<typename mpl::at_c<Messages, i>::type>::value(previous_msg);
182 const typename mpl::at_c<Messages, i>::type &previous_msg = *(deque[deque.size()-2]).getMessage();
183 previous_msg_time = mt::TimeStamp<typename mpl::at_c<Messages, i>::type>::value(previous_msg);
185 if (msg_time < previous_msg_time)
187 ROS_WARN_STREAM(
"Messages of type " << i <<
" arrived out of order (will print only once)");
192 ROS_WARN_STREAM(
"Messages of type " << i <<
" arrived closer (" << (msg_time - previous_msg_time)
194 <<
") (will print only once)");
201 void add(
const typename mpl::at_c<Events, i>::type& evt)
205 std::deque<typename mpl::at_c<Events, i>::type>& deque = boost::get<i>(
deques_);
206 deque.push_back(evt);
207 if (deque.size() == (size_t)1) {
218 checkInterMessageBound<i>();
222 std::vector<typename mpl::at_c<Events, i>::type>& past = boost::get<i>(
past_);
275 std::deque<typename mpl::at_c<Events, i>::type>& deque = boost::get<i>(
deques_);
290 dequeDeleteFront<0>();
293 dequeDeleteFront<1>();
296 dequeDeleteFront<2>();
299 dequeDeleteFront<3>();
302 dequeDeleteFront<4>();
305 dequeDeleteFront<5>();
308 dequeDeleteFront<6>();
311 dequeDeleteFront<7>();
314 dequeDeleteFront<8>();
325 std::deque<typename mpl::at_c<Events, i>::type>& deque = boost::get<i>(
deques_);
326 std::vector<typename mpl::at_c<Events, i>::type>& vector = boost::get<i>(
past_);
328 vector.push_back(deque.front());
341 dequeMoveFrontToPast<0>();
344 dequeMoveFrontToPast<1>();
347 dequeMoveFrontToPast<2>();
350 dequeMoveFrontToPast<3>();
353 dequeMoveFrontToPast<4>();
356 dequeMoveFrontToPast<5>();
359 dequeMoveFrontToPast<6>();
362 dequeMoveFrontToPast<7>();
365 dequeMoveFrontToPast<8>();
379 if (RealTypeCount::value > 2)
382 if (RealTypeCount::value > 3)
385 if (RealTypeCount::value > 4)
388 if (RealTypeCount::value > 5)
391 if (RealTypeCount::value > 6)
394 if (RealTypeCount::value > 7)
397 if (RealTypeCount::value > 8)
408 boost::get<0>(
past_).clear();
409 boost::get<1>(
past_).clear();
410 boost::get<2>(
past_).clear();
411 boost::get<3>(
past_).clear();
412 boost::get<4>(
past_).clear();
413 boost::get<5>(
past_).clear();
414 boost::get<6>(
past_).clear();
415 boost::get<7>(
past_).clear();
416 boost::get<8>(
past_).clear();
425 if (i >= RealTypeCount::value)
430 std::vector<typename mpl::at_c<Events, i>::type>& v = boost::get<i>(
past_);
431 std::deque<typename mpl::at_c<Events, i>::type>& q = boost::get<i>(
deques_);
433 while (num_messages > 0)
435 q.push_front(v.back());
450 if (i >= RealTypeCount::value)
455 std::vector<typename mpl::at_c<Events, i>::type>& v = boost::get<i>(
past_);
456 std::deque<typename mpl::at_c<Events, i>::type>& q = boost::get<i>(
deques_);
459 q.push_front(v.back());
473 if (i >= RealTypeCount::value)
478 std::vector<typename mpl::at_c<Events, i>::type>& v = boost::get<i>(
past_);
479 std::deque<typename mpl::at_c<Events, i>::type>& q = boost::get<i>(
deques_);
482 q.push_front(v.back());
509 recoverAndDelete<0>();
510 recoverAndDelete<1>();
511 recoverAndDelete<2>();
512 recoverAndDelete<3>();
513 recoverAndDelete<4>();
514 recoverAndDelete<5>();
515 recoverAndDelete<6>();
516 recoverAndDelete<7>();
517 recoverAndDelete<8>();
542 M0Event& m0 = boost::get<0>(
deques_).front();
543 time = mt::TimeStamp<M0>::value(*m0.getMessage());
545 if (RealTypeCount::value > 1)
547 M1Event& m1 = boost::get<1>(
deques_).front();
548 if ((mt::TimeStamp<M1>::value(*m1.getMessage()) < time) ^ end)
550 time = mt::TimeStamp<M1>::value(*m1.getMessage());
554 if (RealTypeCount::value > 2)
556 M2Event& m2 = boost::get<2>(
deques_).front();
557 if ((mt::TimeStamp<M2>::value(*m2.getMessage()) < time) ^ end)
559 time = mt::TimeStamp<M2>::value(*m2.getMessage());
563 if (RealTypeCount::value > 3)
565 M3Event& m3 = boost::get<3>(
deques_).front();
566 if ((mt::TimeStamp<M3>::value(*m3.getMessage()) < time) ^ end)
568 time = mt::TimeStamp<M3>::value(*m3.getMessage());
572 if (RealTypeCount::value > 4)
574 M4Event& m4 = boost::get<4>(
deques_).front();
575 if ((mt::TimeStamp<M4>::value(*m4.getMessage()) < time) ^ end)
577 time = mt::TimeStamp<M4>::value(*m4.getMessage());
581 if (RealTypeCount::value > 5)
583 M5Event& m5 = boost::get<5>(
deques_).front();
584 if ((mt::TimeStamp<M5>::value(*m5.getMessage()) < time) ^ end)
586 time = mt::TimeStamp<M5>::value(*m5.getMessage());
590 if (RealTypeCount::value > 6)
592 M6Event& m6 = boost::get<6>(
deques_).front();
593 if ((mt::TimeStamp<M6>::value(*m6.getMessage()) < time) ^ end)
595 time = mt::TimeStamp<M6>::value(*m6.getMessage());
599 if (RealTypeCount::value > 7)
601 M7Event& m7 = boost::get<7>(
deques_).front();
602 if ((mt::TimeStamp<M7>::value(*m7.getMessage()) < time) ^ end)
604 time = mt::TimeStamp<M7>::value(*m7.getMessage());
608 if (RealTypeCount::value > 8)
610 M8Event& m8 = boost::get<8>(
deques_).front();
611 if ((mt::TimeStamp<M8>::value(*m8.getMessage()) < time) ^ end)
613 time = mt::TimeStamp<M8>::value(*m8.getMessage());
626 if (i >= RealTypeCount::value)
632 std::vector<typename mpl::at_c<Events, i>::type>& v = boost::get<i>(
past_);
633 std::deque<typename mpl::at_c<Events, i>::type>& q = boost::get<i>(
deques_);
637 ros::Time last_msg_time = mt::TimeStamp<typename mpl::at_c<Messages, i>::type>::value(*(v.back()).getMessage());
641 return msg_time_lower_bound;
645 ros::Time current_msg_time = mt::TimeStamp<typename mpl::at_c<Messages, i>::type>::value(*(q.front()).getMessage());
646 return current_msg_time;
669 std::vector<ros::Time> virtual_times(9);
670 virtual_times[0] = getVirtualTime<0>();
671 virtual_times[1] = getVirtualTime<1>();
672 virtual_times[2] = getVirtualTime<2>();
673 virtual_times[3] = getVirtualTime<3>();
674 virtual_times[4] = getVirtualTime<4>();
675 virtual_times[5] = getVirtualTime<5>();
676 virtual_times[6] = getVirtualTime<6>();
677 virtual_times[7] = getVirtualTime<7>();
678 virtual_times[8] = getVirtualTime<8>();
680 time = virtual_times[0];
682 for (
int i = 0; i < RealTypeCount::value; i++)
684 if ((virtual_times[i] < time) ^ end)
686 time = virtual_times[i];
704 uint32_t end_index, start_index;
707 for (uint32_t i = 0; i < (uint32_t)RealTypeCount::value; i++)
764 if (start_index ==
pivot_)
783 std::vector<int> num_virtual_moves(9,0);
787 uint32_t end_index, start_index;
805 recover<0>(num_virtual_moves[0]);
806 recover<1>(num_virtual_moves[1]);
807 recover<2>(num_virtual_moves[2]);
808 recover<3>(num_virtual_moves[3]);
809 recover<4>(num_virtual_moves[4]);
810 recover<5>(num_virtual_moves[5]);
811 recover<6>(num_virtual_moves[6]);
812 recover<7>(num_virtual_moves[7]);
813 recover<8>(num_virtual_moves[8]);
814 (void)num_non_empty_deques_before_virtual_search;
824 num_virtual_moves[start_index]++;
856 #endif // MESSAGE_FILTERS_SYNC_APPROXIMATE_TIME_H
void setInterMessageLowerBound(int i, ros::Duration lower_bound)
void dequeMoveFrontToPast(uint32_t index)
std::vector< M6Event > M6Vector
std::deque< M0Event > M0Deque
boost::tuple< M0Event, M1Event, M2Event, M3Event, M4Event, M5Event, M6Event, M7Event, M8Event > Tuple
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
std::vector< bool > has_dropped_messages_
static const uint32_t NO_PIVOT
void getCandidateEnd(uint32_t &end_index, ros::Time &end_time)
std::deque< M1Event > M1Deque
std::deque< M8Event > M8Deque
std::deque< M5Event > M5Deque
std::vector< ros::Duration > inter_message_lower_bounds_
ros::Time getVirtualTime()
Synchronizer< ApproximateTime > Sync
void setAgePenalty(double age_penalty)
std::deque< M7Event > M7Deque
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 getVirtualCandidateStart(uint32_t &start_index, ros::Time &start_time)
void dequeMoveFrontToPast()
mpl::at_c< Events, 1 >::type M1Event
std::deque< M2Event > M2Deque
ApproximateTime(const ApproximateTime &e)
std::vector< M3Event > M3Vector
mpl::fold< Messages, mpl::int_< 0 >, mpl::if_< mpl::not_< boost::is_same< mpl::_2, NullType > >, mpl::next< mpl::_1 >, mpl::_1 > >::type RealTypeCount
void checkInterMessageBound()
std::vector< M1Event > M1Vector
void getCandidateStart(uint32_t &start_index, ros::Time &start_time)
ros::Duration max_interval_duration_
mpl::at_c< Events, 5 >::type M5Event
mpl::at_c< Events, 2 >::type M2Event
std::vector< M0Event > M0Vector
std::vector< M7Event > M7Vector
void initParent(Sync *parent)
std::vector< M8Event > M8Vector
mpl::vector< M0, M1, M2, M3, M4, M5, M6, M7, M8 > Messages
Super::RealTypeCount RealTypeCount
void setMaxIntervalDuration(ros::Duration max_interval_duration)
std::vector< M2Event > M2Vector
void getVirtualCandidateEnd(uint32_t &end_index, ros::Time &end_time)
std::vector< bool > warned_about_incorrect_bound_
std::deque< M4Event > M4Deque
mpl::at_c< Events, 0 >::type M0Event
void dequeDeleteFront(uint32_t index)
#define ROS_WARN_STREAM(args)
mpl::at_c< Events, 3 >::type M3Event
std::vector< M5Event > M5Vector
boost::tuple< M0Deque, M1Deque, M2Deque, M3Deque, M4Deque, M5Deque, M6Deque, M7Deque, M8Deque > DequeTuple
uint32_t num_non_empty_deques_
void add(const typename mpl::at_c< Events, i >::type &evt)
void getCandidateBoundary(uint32_t &index, ros::Time &time, bool end)
std::deque< M6Event > M6Deque
PolicyBase< M0, M1, M2, M3, M4, M5, M6, M7, M8 > Super
std::deque< M3Event > M3Deque
ros::Time candidate_start_
mpl::at_c< Events, 7 >::type M7Event
mpl::at_c< Events, 4 >::type M4Event
std::vector< M4Event > M4Vector
void getVirtualCandidateBoundary(uint32_t &index, ros::Time &time, bool end)
boost::tuple< M0Vector, M1Vector, M2Vector, M3Vector, M4Vector, M5Vector, M6Vector, M7Vector, M8Vector > VectorTuple
ApproximateTime & operator=(const ApproximateTime &rhs)
mpl::at_c< Events, 8 >::type M8Event
void recover(size_t num_messages)
mpl::at_c< Events, 6 >::type M6Event
ApproximateTime(uint32_t queue_size)