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_);
281 std::deque<typename mpl::at_c<Events, i>::type>& deque = boost::get<i>(
deques_);
296 dequeDeleteFront<0>();
299 dequeDeleteFront<1>();
302 dequeDeleteFront<2>();
305 dequeDeleteFront<3>();
308 dequeDeleteFront<4>();
311 dequeDeleteFront<5>();
314 dequeDeleteFront<6>();
317 dequeDeleteFront<7>();
320 dequeDeleteFront<8>();
331 std::deque<typename mpl::at_c<Events, i>::type>& deque = boost::get<i>(
deques_);
332 std::vector<typename mpl::at_c<Events, i>::type>& vector = boost::get<i>(
past_);
334 vector.push_back(deque.front());
347 dequeMoveFrontToPast<0>();
350 dequeMoveFrontToPast<1>();
353 dequeMoveFrontToPast<2>();
356 dequeMoveFrontToPast<3>();
359 dequeMoveFrontToPast<4>();
362 dequeMoveFrontToPast<5>();
365 dequeMoveFrontToPast<6>();
368 dequeMoveFrontToPast<7>();
371 dequeMoveFrontToPast<8>();
385 if (RealTypeCount::value > 2)
388 if (RealTypeCount::value > 3)
391 if (RealTypeCount::value > 4)
394 if (RealTypeCount::value > 5)
397 if (RealTypeCount::value > 6)
400 if (RealTypeCount::value > 7)
403 if (RealTypeCount::value > 8)
414 boost::get<0>(
past_).clear();
415 boost::get<1>(
past_).clear();
416 boost::get<2>(
past_).clear();
417 boost::get<3>(
past_).clear();
418 boost::get<4>(
past_).clear();
419 boost::get<5>(
past_).clear();
420 boost::get<6>(
past_).clear();
421 boost::get<7>(
past_).clear();
422 boost::get<8>(
past_).clear();
431 if (i >= RealTypeCount::value)
436 std::vector<typename mpl::at_c<Events, i>::type>& v = boost::get<i>(
past_);
437 std::deque<typename mpl::at_c<Events, i>::type>& q = boost::get<i>(
deques_);
439 while (num_messages > 0)
441 q.push_front(v.back());
456 if (i >= RealTypeCount::value)
461 std::vector<typename mpl::at_c<Events, i>::type>& v = boost::get<i>(
past_);
462 std::deque<typename mpl::at_c<Events, i>::type>& q = boost::get<i>(
deques_);
465 q.push_front(v.back());
479 if (i >= RealTypeCount::value)
484 std::vector<typename mpl::at_c<Events, i>::type>& v = boost::get<i>(
past_);
485 std::deque<typename mpl::at_c<Events, i>::type>& q = boost::get<i>(
deques_);
488 q.push_front(v.back());
515 recoverAndDelete<0>();
516 recoverAndDelete<1>();
517 recoverAndDelete<2>();
518 recoverAndDelete<3>();
519 recoverAndDelete<4>();
520 recoverAndDelete<5>();
521 recoverAndDelete<6>();
522 recoverAndDelete<7>();
523 recoverAndDelete<8>();
548 M0Event& m0 = boost::get<0>(
deques_).front();
549 time = mt::TimeStamp<M0>::value(*m0.getMessage());
551 if (RealTypeCount::value > 1)
553 M1Event& m1 = boost::get<1>(
deques_).front();
554 if ((mt::TimeStamp<M1>::value(*m1.getMessage()) < time) ^ end)
556 time = mt::TimeStamp<M1>::value(*m1.getMessage());
560 if (RealTypeCount::value > 2)
562 M2Event& m2 = boost::get<2>(
deques_).front();
563 if ((mt::TimeStamp<M2>::value(*m2.getMessage()) < time) ^ end)
565 time = mt::TimeStamp<M2>::value(*m2.getMessage());
569 if (RealTypeCount::value > 3)
571 M3Event& m3 = boost::get<3>(
deques_).front();
572 if ((mt::TimeStamp<M3>::value(*m3.getMessage()) < time) ^ end)
574 time = mt::TimeStamp<M3>::value(*m3.getMessage());
578 if (RealTypeCount::value > 4)
580 M4Event& m4 = boost::get<4>(
deques_).front();
581 if ((mt::TimeStamp<M4>::value(*m4.getMessage()) < time) ^ end)
583 time = mt::TimeStamp<M4>::value(*m4.getMessage());
587 if (RealTypeCount::value > 5)
589 M5Event& m5 = boost::get<5>(
deques_).front();
590 if ((mt::TimeStamp<M5>::value(*m5.getMessage()) < time) ^ end)
592 time = mt::TimeStamp<M5>::value(*m5.getMessage());
596 if (RealTypeCount::value > 6)
598 M6Event& m6 = boost::get<6>(
deques_).front();
599 if ((mt::TimeStamp<M6>::value(*m6.getMessage()) < time) ^ end)
601 time = mt::TimeStamp<M6>::value(*m6.getMessage());
605 if (RealTypeCount::value > 7)
607 M7Event& m7 = boost::get<7>(
deques_).front();
608 if ((mt::TimeStamp<M7>::value(*m7.getMessage()) < time) ^ end)
610 time = mt::TimeStamp<M7>::value(*m7.getMessage());
614 if (RealTypeCount::value > 8)
616 M8Event& m8 = boost::get<8>(
deques_).front();
617 if ((mt::TimeStamp<M8>::value(*m8.getMessage()) < time) ^ end)
619 time = mt::TimeStamp<M8>::value(*m8.getMessage());
632 if (i >= RealTypeCount::value)
638 std::vector<typename mpl::at_c<Events, i>::type>& v = boost::get<i>(
past_);
639 std::deque<typename mpl::at_c<Events, i>::type>& q = boost::get<i>(
deques_);
643 ros::Time last_msg_time = mt::TimeStamp<typename mpl::at_c<Messages, i>::type>::value(*(v.back()).getMessage());
647 return msg_time_lower_bound;
651 ros::Time current_msg_time = mt::TimeStamp<typename mpl::at_c<Messages, i>::type>::value(*(q.front()).getMessage());
652 return current_msg_time;
675 std::vector<ros::Time> virtual_times(9);
676 virtual_times[0] = getVirtualTime<0>();
677 virtual_times[1] = getVirtualTime<1>();
678 virtual_times[2] = getVirtualTime<2>();
679 virtual_times[3] = getVirtualTime<3>();
680 virtual_times[4] = getVirtualTime<4>();
681 virtual_times[5] = getVirtualTime<5>();
682 virtual_times[6] = getVirtualTime<6>();
683 virtual_times[7] = getVirtualTime<7>();
684 virtual_times[8] = getVirtualTime<8>();
686 time = virtual_times[0];
688 for (
int i = 0; i < RealTypeCount::value; i++)
690 if ((virtual_times[i] < time) ^ end)
692 time = virtual_times[i];
710 uint32_t end_index, start_index;
713 for (uint32_t i = 0; i < (uint32_t)RealTypeCount::value; i++)
770 if (start_index ==
pivot_)
789 std::vector<int> num_virtual_moves(9,0);
793 uint32_t end_index, start_index;
811 recover<0>(num_virtual_moves[0]);
812 recover<1>(num_virtual_moves[1]);
813 recover<2>(num_virtual_moves[2]);
814 recover<3>(num_virtual_moves[3]);
815 recover<4>(num_virtual_moves[4]);
816 recover<5>(num_virtual_moves[5]);
817 recover<6>(num_virtual_moves[6]);
818 recover<7>(num_virtual_moves[7]);
819 recover<8>(num_virtual_moves[8]);
820 (void)num_non_empty_deques_before_virtual_search;
830 num_virtual_moves[start_index]++;
862 #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
void setInterMessageLowerBound(ros::Duration lower_bound)
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
ROSTIME_DECL const Duration DURATION_MAX
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)