diff --git a/include/message_filters/signal9.hpp b/include/message_filters/signal9.hpp index 31d476f..3c78434 100644 --- a/include/message_filters/signal9.hpp +++ b/include/message_filters/signal9.hpp @@ -43,341 +43,100 @@ namespace message_filters { -template +template class CallbackHelper9 { public: - typedef MessageEvent M0Event; - typedef MessageEvent M1Event; - typedef MessageEvent M2Event; - typedef MessageEvent M3Event; - typedef MessageEvent M4Event; - typedef MessageEvent M5Event; - typedef MessageEvent M6Event; - typedef MessageEvent M7Event; - typedef MessageEvent M8Event; - virtual ~CallbackHelper9() {} - virtual void call( - bool nonconst_force_copy, 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) = 0; + virtual void call(bool nonconst_force_copy, const MessageEvent & ... es) = 0; typedef std::shared_ptr Ptr; }; -template +template class CallbackHelper9T - : public CallbackHelper9::Message, - typename ParameterAdapter::Message, - typename ParameterAdapter::Message, - typename ParameterAdapter::Message, - typename ParameterAdapter::Message, - typename ParameterAdapter::Message, - typename ParameterAdapter::Message, - typename ParameterAdapter::Message, - typename ParameterAdapter::Message> + : public CallbackHelper9::Message...> { -private: - typedef ParameterAdapter A0; - typedef ParameterAdapter A1; - typedef ParameterAdapter A2; - typedef ParameterAdapter A3; - typedef ParameterAdapter A4; - typedef ParameterAdapter A5; - typedef ParameterAdapter A6; - typedef ParameterAdapter A7; - typedef ParameterAdapter A8; - typedef typename A0::Event M0Event; - typedef typename A1::Event M1Event; - typedef typename A2::Event M2Event; - typedef typename A3::Event M3Event; - typedef typename A4::Event M4Event; - typedef typename A5::Event M5Event; - typedef typename A6::Event M6Event; - typedef typename A7::Event M7Event; - typedef typename A8::Event M8Event; - public: - typedef std::function Callback; + typedef std::function::Parameter...)> Callback; CallbackHelper9T(const Callback & cb) // NOLINT(runtime/explicit) : callback_(cb) { } - virtual void call( - bool nonconst_force_copy, 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) + virtual void call(bool nonconst_force_copy, const typename ParameterAdapter::Event &... es) { - M0Event my_e0(e0, nonconst_force_copy || e0.nonConstWillCopy()); - M1Event my_e1(e1, nonconst_force_copy || e0.nonConstWillCopy()); - M2Event my_e2(e2, nonconst_force_copy || e0.nonConstWillCopy()); - M3Event my_e3(e3, nonconst_force_copy || e0.nonConstWillCopy()); - M4Event my_e4(e4, nonconst_force_copy || e0.nonConstWillCopy()); - M5Event my_e5(e5, nonconst_force_copy || e0.nonConstWillCopy()); - M6Event my_e6(e6, nonconst_force_copy || e0.nonConstWillCopy()); - M7Event my_e7(e7, nonconst_force_copy || e0.nonConstWillCopy()); - M8Event my_e8(e8, nonconst_force_copy || e0.nonConstWillCopy()); - callback_( - A0::getParameter(e0), - A1::getParameter(e1), - A2::getParameter(e2), - A3::getParameter(e3), - A4::getParameter(e4), - A5::getParameter(e5), - A6::getParameter(e6), - A7::getParameter(e7), - A8::getParameter(e8)); + auto my_es{std::make_tuple( + typename ParameterAdapter::Event( + es, + nonconst_force_copy || es.nonConstWillCopy())...)}; + callback_(ParameterAdapter::getParameter(es)...); } private: Callback callback_; }; -template +template class Signal9 { - typedef std::shared_ptr> CallbackHelper9Ptr; + typedef std::shared_ptr> CallbackHelper9Ptr; typedef std::vector V_CallbackHelper9; public: - typedef MessageEvent M0Event; - typedef MessageEvent M1Event; - typedef MessageEvent M2Event; - typedef MessageEvent M3Event; - typedef MessageEvent M4Event; - typedef MessageEvent M5Event; - typedef MessageEvent M6Event; - typedef MessageEvent M7Event; - typedef MessageEvent M8Event; - typedef std::shared_ptr M0ConstPtr; - typedef std::shared_ptr M1ConstPtr; - typedef std::shared_ptr M2ConstPtr; - typedef std::shared_ptr M3ConstPtr; - typedef std::shared_ptr M4ConstPtr; - typedef std::shared_ptr M5ConstPtr; - typedef std::shared_ptr M6ConstPtr; - typedef std::shared_ptr M7ConstPtr; - typedef std::shared_ptr M8ConstPtr; typedef const std::shared_ptr & NullP; - - template - Connection addCallback(const std::function & callback) + template + Connection addCallback(const std::function & callback) { - CallbackHelper9T * helper = - new CallbackHelper9T(callback); + CallbackHelper9T * helper = new CallbackHelper9T(callback); std::lock_guard lock(mutex_); callbacks_.push_back(CallbackHelper9Ptr(helper)); return Connection(std::bind(&Signal9::removeCallback, this, callbacks_.back())); } - template - Connection addCallback(void (* callback)(P0, P1)) - { - return addCallback( - std::function( - std::bind( - callback, std::placeholders::_1, std::placeholders::_2))); - } - - template - Connection addCallback(void (* callback)(P0, P1, P2)) + template + Connection addCallback(void (* callback)(Ps...)) { return addCallback( - std::function( - std::bind( - callback, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3))); + std::function( + [callback](auto &&... args) { + callback(args ...); + })); } - template - Connection addCallback(void (* callback)(P0, P1, P2, P3)) + template + Connection addCallback(void (T::* callback)(Ps...), T * t) { return addCallback( - std::function( - std::bind( - callback, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - std::placeholders::_4))); - } - - template - Connection addCallback(void (* callback)(P0, P1, P2, P3, P4)) - { - return addCallback( - std::function( - std::bind( - callback, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - std::placeholders::_4, std::placeholders::_5))); - } - - template - Connection addCallback(void (* callback)(P0, P1, P2, P3, P4, P5)) - { - return addCallback( - std::function( - std::bind( - callback, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - std::placeholders::_4, std::placeholders::_5, std::placeholders::_6))); - } - - template - Connection addCallback(void (* callback)(P0, P1, P2, P3, P4, P5, P6)) - { - return addCallback( - std::function( - std::bind( - callback, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, - std::placeholders::_7))); - } - - template - Connection addCallback(void (* callback)(P0, P1, P2, P3, P4, P5, P6, P7)) - { - return addCallback( - std::function( - std::bind( - callback, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, - std::placeholders::_7, std::placeholders::_8))); - } - - template - Connection addCallback(void (* callback)(P0, P1, P2, P3, P4, P5, P6, P7, P8)) - { - return addCallback( - std::function( - std::bind( - callback, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, - std::placeholders::_7, std::placeholders::_8, std::placeholders::_9))); - } - - template - Connection addCallback(void (T::* callback)(P0, P1), T * t) - { - return addCallback( - std::function( - std::bind( - callback, t, std::placeholders::_1, std::placeholders::_2))); - } - - template - Connection addCallback(void (T::* callback)(P0, P1, P2), T * t) - { - return addCallback( - std::function( - std::bind( - callback, t, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3))); - } - - template - Connection addCallback(void (T::* callback)(P0, P1, P2, P3), T * t) - { - return addCallback( - std::function( - std::bind( - callback, t, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, std::placeholders::_4))); - } - - template - Connection addCallback(void (T::* callback)(P0, P1, P2, P3, P4), T * t) - { - return addCallback( - std::function( - std::bind( - callback, t, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, std::placeholders::_4, std::placeholders::_5))); - } - - template - Connection addCallback(void (T::* callback)(P0, P1, P2, P3, P4, P5), T * t) - { - return addCallback( - std::function( - std::bind( - callback, t, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, - std::placeholders::_6))); - } - - template - Connection addCallback(void (T::* callback)(P0, P1, P2, P3, P4, P5, P6), T * t) - { - return addCallback( - std::function( - std::bind( - callback, t, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, - std::placeholders::_6, std::placeholders::_7))); - } - - template - Connection addCallback(void (T::* callback)(P0, P1, P2, P3, P4, P5, P6, P7), T * t) - { - return addCallback( - std::function( - std::bind( - callback, t, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, - std::placeholders::_6, std::placeholders::_7, std::placeholders::_8))); + std::function( + [ = ](const Ps &... ps) { + (t->*callback)(ps ...); + })); } template Connection addCallback(C & callback) { - return addCallback( - std::bind( - callback, std::placeholders::_1, - std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, - std::placeholders::_5, std::placeholders::_6, std::placeholders::_7, - std::placeholders::_8, std::placeholders::_9)); + return addCallback( + std::function & ...)>( + [callback](const std::shared_ptr & ... msgs) {callback(msgs ...);})); } void removeCallback(const CallbackHelper9Ptr & helper) { std::lock_guard lock(mutex_); - typename V_CallbackHelper9::iterator it = std::find( - callbacks_.begin(), callbacks_.end(), helper); + typename V_CallbackHelper9::iterator it = + std::find(callbacks_.begin(), callbacks_.end(), helper); if (it != callbacks_.end()) { callbacks_.erase(it); } } - 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) + void call(const MessageEvent & ... es) { std::lock_guard lock(mutex_); bool nonconst_force_copy = callbacks_.size() > 1; @@ -385,7 +144,7 @@ class Signal9 typename V_CallbackHelper9::iterator end = callbacks_.end(); for (; it != end; ++it) { const CallbackHelper9Ptr & helper = *it; - helper->call(nonconst_force_copy, e0, e1, e2, e3, e4, e5, e6, e7, e8); + helper->call(nonconst_force_copy, es ...); } } diff --git a/include/message_filters/sync_policies/approximate_epsilon_time.hpp b/include/message_filters/sync_policies/approximate_epsilon_time.hpp index 84324b4..bc65f8f 100644 --- a/include/message_filters/sync_policies/approximate_epsilon_time.hpp +++ b/include/message_filters/sync_policies/approximate_epsilon_time.hpp @@ -51,29 +51,20 @@ namespace message_filters namespace sync_policies { -template -class ApproximateEpsilonTime : public PolicyBase +template +class ApproximateEpsilonTime : public PolicyBase { public: typedef Synchronizer Sync; - typedef PolicyBase Super; + typedef PolicyBase Super; typedef typename Super::Messages Messages; typedef typename Super::Signal Signal; typedef typename Super::Events Events; typedef typename Super::RealTypeCount RealTypeCount; - typedef typename Super::M0Event M0Event; - typedef typename Super::M1Event M1Event; - typedef typename Super::M2Event M2Event; - typedef typename Super::M3Event M3Event; - typedef typename Super::M4Event M4Event; - typedef typename Super::M5Event M5Event; - typedef typename Super::M6Event M6Event; - typedef typename Super::M7Event M7Event; - typedef typename Super::M8Event M8Event; typedef Events Tuple; + using Super::N_MESSAGES; + ApproximateEpsilonTime(uint32_t queue_size, rclcpp::Duration epsilon) : parent_(nullptr) , queue_size_(queue_size) @@ -130,16 +121,14 @@ class ApproximateEpsilonTime : public PolicyBase::type; - if constexpr (Is >= RealTypeCount::value) { - return current; - } const auto & events_of_this_type = std::get(events_); if (0u == events_of_this_type.size()) { // this condition should not happen return current; } auto candidate = mt::TimeStamp::value( - *events_of_this_type.at(0).getMessage()); + *events_of_this_type.at( + 0).getMessage()); if (current.first > candidate) { return std::make_pair(candidate, Is); } @@ -160,7 +149,7 @@ class ApproximateEpsilonTime : public PolicyBase()); + return get_older_timestamp_helper(std::make_index_sequence()); } template @@ -169,9 +158,6 @@ class ApproximateEpsilonTime : public PolicyBase::type; - if constexpr (Is >= RealTypeCount::value) { - return true; - } if (Is == older.second) { return true; } @@ -181,7 +167,8 @@ class ApproximateEpsilonTime : public PolicyBase::value( - *events_of_this_type.at(0).getMessage()); + *events_of_this_type.at( + 0).getMessage()); if (older.first + epsilon_ >= ts) { return true; } @@ -202,16 +189,13 @@ class ApproximateEpsilonTime : public PolicyBase()); + older, std::make_index_sequence()); } template void erase_beginning_of_vector() { - if constexpr (Is >= RealTypeCount::value) { - return; - } auto & this_vector = std::get(events_); if (this_vector.begin() != this_vector.end()) { this_vector.erase(this_vector.begin()); @@ -230,7 +214,7 @@ class ApproximateEpsilonTime : public PolicyBase()); + return erase_beginning_of_vectors_helper(std::make_index_sequence()); } template @@ -239,9 +223,6 @@ class ApproximateEpsilonTime : public PolicyBase::type; - if constexpr (Is >= RealTypeCount::value) { - return; - } auto & this_vector = std::get(events_); if (this_vector.begin() == this_vector.end()) { return; @@ -260,61 +241,28 @@ class ApproximateEpsilonTime : public PolicyBase void erase_old_events_if_on_sync_with_ts_helper( - rclcpp::Time timestamp, std::index_sequence const &) + rclcpp::Time timestamp, + std::index_sequence const &) { ((erase_beginning_of_vector_if_on_sync_with_ts(timestamp)), ...); } void erase_old_events_if_on_sync_with_ts(rclcpp::Time timestamp) { - return erase_old_events_if_on_sync_with_ts_helper(timestamp, std::make_index_sequence<9u>()); + return erase_old_events_if_on_sync_with_ts_helper( + timestamp, + std::make_index_sequence()); + } + + template + void signalImpl(std::index_sequence) + { + parent_->signal(std::get(events_).at(0)...); } void signal() { - if constexpr (RealTypeCount::value == 2) { - parent_->signal( - std::get<0>(events_).at(0), std::get<1>(events_).at(0), - M2Event{}, M3Event{}, M4Event{}, M5Event{}, M6Event{}, M7Event{}, M8Event{}); - } else if constexpr (RealTypeCount::value == 3) { - parent_->signal( - std::get<0>(events_).at(0), std::get<1>(events_).at(0), std::get<2>(events_).at(0), - M3Event{}, M4Event{}, M5Event{}, M6Event{}, M7Event{}, M8Event{}); - } else if constexpr (RealTypeCount::value == 4) { - parent_->signal( - std::get<0>(events_).at(0), std::get<1>(events_).at(0), std::get<2>(events_).at(0), - std::get<3>(events_).at(0), - M4Event{}, M5Event{}, M6Event{}, M7Event{}, M8Event{}); - } else if constexpr (RealTypeCount::value == 5) { - parent_->signal( - std::get<0>(events_).at(0), std::get<1>(events_).at(0), std::get<2>(events_).at(0), - std::get<3>(events_).at(0), std::get<4>(events_).at(0), - M5Event{}, M6Event{}, M7Event{}, M8Event{}); - } else if constexpr (RealTypeCount::value == 6) { - parent_->signal( - std::get<0>(events_).at(0), std::get<1>(events_).at(0), std::get<2>(events_).at(0), - std::get<3>(events_).at(0), std::get<4>(events_).at(0), std::get<5>(events_).at(0), - M6Event{}, M7Event{}, M8Event{}); - } else if constexpr (RealTypeCount::value == 7) { - parent_->signal( - std::get<0>(events_).at(0), std::get<1>(events_).at(0), std::get<2>(events_).at(0), - std::get<3>(events_).at(0), std::get<4>(events_).at(0), std::get<5>(events_).at(0), - std::get<6>(events_).at(0), - M7Event{}, M8Event{}); - } else if constexpr (RealTypeCount::value == 8) { - parent_->signal( - std::get<0>(events_).at(0), std::get<1>(events_).at(0), std::get<2>(events_).at(0), - std::get<3>(events_).at(0), std::get<4>(events_).at(0), std::get<5>(events_).at(0), - std::get<6>(events_).at(0), std::get<7>(events_).at(0), - M8Event{}); - } else if constexpr (RealTypeCount::value == 9) { - parent_->signal( - std::get<0>(events_).at(0), std::get<1>(events_).at(0), std::get<2>(events_).at(0), - std::get<3>(events_).at(0), std::get<4>(events_).at(0), std::get<5>(events_).at(0), - std::get<6>(events_).at(0), std::get<7>(events_).at(0), std::get<8>(events_).at(0)); - } else { - static_assert("RealTypeCount::value should be >=2 and <=9"); - } + signalImpl(std::make_index_sequence{}); } // assumes mutex_ is already locked @@ -336,10 +284,7 @@ class ApproximateEpsilonTime : public PolicyBase, std::vector, std::vector, - std::vector, std::vector, std::vector, - std::vector, std::vector, std::vector>; + using TupleOfVecOfEvents = typename std::tuple>...>; TupleOfVecOfEvents events_; std::mutex mutex_; diff --git a/include/message_filters/sync_policies/approximate_time.hpp b/include/message_filters/sync_policies/approximate_time.hpp index e64671f..19b031d 100644 --- a/include/message_filters/sync_policies/approximate_time.hpp +++ b/include/message_filters/sync_policies/approximate_time.hpp @@ -53,49 +53,19 @@ namespace message_filters namespace sync_policies { -template -struct ApproximateTime : public PolicyBase +template +struct ApproximateTime : public PolicyBase { typedef Synchronizer Sync; - typedef PolicyBase Super; + typedef PolicyBase Super; typedef typename Super::Messages Messages; typedef typename Super::Signal Signal; typedef typename Super::Events Events; - typedef typename Super::RealTypeCount RealTypeCount; - typedef typename Super::M0Event M0Event; - typedef typename Super::M1Event M1Event; - typedef typename Super::M2Event M2Event; - typedef typename Super::M3Event M3Event; - typedef typename Super::M4Event M4Event; - typedef typename Super::M5Event M5Event; - typedef typename Super::M6Event M6Event; - typedef typename Super::M7Event M7Event; - typedef typename Super::M8Event M8Event; - typedef std::deque M0Deque; - typedef std::deque M1Deque; - typedef std::deque M2Deque; - typedef std::deque M3Deque; - typedef std::deque M4Deque; - typedef std::deque M5Deque; - typedef std::deque M6Deque; - typedef std::deque M7Deque; - typedef std::deque M8Deque; - typedef std::vector M0Vector; - typedef std::vector M1Vector; - typedef std::vector M2Vector; - typedef std::vector M3Vector; - typedef std::vector M4Vector; - typedef std::vector M5Vector; - typedef std::vector M6Vector; - typedef std::vector M7Vector; - typedef std::vector M8Vector; typedef Events Tuple; - typedef std::tuple DequeTuple; - typedef std::tuple VectorTuple; + typedef std::tuple>...> DequeTuple; + typedef std::tuple>...> VectorTuple; + + using Super::N_MESSAGES; ApproximateTime(uint32_t queue_size) // NOLINT(runtime/explicit) : parent_(0) @@ -202,7 +172,7 @@ struct ApproximateTime : public PolicyBase if (deque.size() == static_cast(1)) { // We have just added the first message, so it was empty before ++num_non_empty_deques_; - if (num_non_empty_deques_ == (uint32_t)RealTypeCount::value) { + if (num_non_empty_deques_ == (uint32_t)N_MESSAGES) { // All deques have messages process(); } @@ -215,15 +185,7 @@ struct ApproximateTime : public PolicyBase if (deque.size() + past.size() > queue_size_) { // Cancel ongoing candidate search, if any: num_non_empty_deques_ = 0; // We will recompute it from scratch - recover<0>(); - recover<1>(); - recover<2>(); - recover<3>(); - recover<4>(); - recover<5>(); - recover<6>(); - recover<7>(); - recover<8>(); + recoverAll(std::make_index_sequence{}); // Drop the oldest message in the offending topic assert(!deque.empty()); deque.pop_front(); @@ -275,42 +237,26 @@ struct ApproximateTime : public PolicyBase } } - // Assumes that deque number is non empty - void dequeDeleteFront(uint32_t index) + template + void dequeDeleteFrontImpl(std::size_t i) { - switch (index) { - case 0: - dequeDeleteFront<0>(); - break; - case 1: - dequeDeleteFront<1>(); - break; - case 2: - dequeDeleteFront<2>(); - break; - case 3: - dequeDeleteFront<3>(); - break; - case 4: - dequeDeleteFront<4>(); - break; - case 5: - dequeDeleteFront<5>(); - break; - case 6: - dequeDeleteFront<6>(); - break; - case 7: - dequeDeleteFront<7>(); - break; - case 8: - dequeDeleteFront<8>(); - break; - default: + if (I == i) { + dequeDeleteFront(); + } else { + if constexpr (I > 0) { + dequeDeleteFrontImpl(i); + } else { std::abort(); + } } } + // Assumes that deque number is non empty + void dequeDeleteFront(uint32_t index) + { + dequeDeleteFrontImpl(index); + } + // Assumes that deque number is non empty template void dequeMoveFrontToPast() @@ -324,90 +270,59 @@ struct ApproximateTime : public PolicyBase --num_non_empty_deques_; } } - // Assumes that deque number is non empty - void dequeMoveFrontToPast(uint32_t index) + + template + void dequeMoveFrontToPastImpl(std::size_t i) { - switch (index) { - case 0: - dequeMoveFrontToPast<0>(); - break; - case 1: - dequeMoveFrontToPast<1>(); - break; - case 2: - dequeMoveFrontToPast<2>(); - break; - case 3: - dequeMoveFrontToPast<3>(); - break; - case 4: - dequeMoveFrontToPast<4>(); - break; - case 5: - dequeMoveFrontToPast<5>(); - break; - case 6: - dequeMoveFrontToPast<6>(); - break; - case 7: - dequeMoveFrontToPast<7>(); - break; - case 8: - dequeMoveFrontToPast<8>(); - break; - default: + if (I == i) { + dequeMoveFrontToPast(); + } else { + if constexpr (I > 0) { + dequeMoveFrontToPastImpl(i); + } else { std::abort(); + } } } + // Assumes that deque number is non empty + void dequeMoveFrontToPast(uint32_t index) + { + dequeMoveFrontToPastImpl(index); + } + + template + void assignFront(Tuple & candidate, DequeTuple & deques, std::index_sequence) + { + ((std::get(candidate) = std::get(deques).front()), ...); + } + + template + void clear(VectorTuple & tuple, std::index_sequence) + { + (std::get(tuple).clear(), ...); + } + void makeCandidate() { // Create candidate tuple candidate_ = Tuple(); // Discards old one if any - std::get<0>(candidate_) = std::get<0>(deques_).front(); - std::get<1>(candidate_) = std::get<1>(deques_).front(); - if (RealTypeCount::value > 2) { - std::get<2>(candidate_) = std::get<2>(deques_).front(); - if (RealTypeCount::value > 3) { - std::get<3>(candidate_) = std::get<3>(deques_).front(); - if (RealTypeCount::value > 4) { - std::get<4>(candidate_) = std::get<4>(deques_).front(); - if (RealTypeCount::value > 5) { - std::get<5>(candidate_) = std::get<5>(deques_).front(); - if (RealTypeCount::value > 6) { - std::get<6>(candidate_) = std::get<6>(deques_).front(); - if (RealTypeCount::value > 7) { - std::get<7>(candidate_) = std::get<7>(deques_).front(); - if (RealTypeCount::value > 8) { - std::get<8>(candidate_) = std::get<8>(deques_).front(); - } - } - } - } - } - } - } + assignFront(candidate_, deques_, std::make_index_sequence{}); + // Delete all past messages, since we have found a better candidate - std::get<0>(past_).clear(); - std::get<1>(past_).clear(); - std::get<2>(past_).clear(); - std::get<3>(past_).clear(); - std::get<4>(past_).clear(); - std::get<5>(past_).clear(); - std::get<6>(past_).clear(); - std::get<7>(past_).clear(); - std::get<8>(past_).clear(); + clear(past_, std::make_index_sequence{}); } + template + void recoverAll(const std::array & num_messages, std::index_sequence) + { + (recover(num_messages[Is]), ...); + } // ASSUMES: num_messages <= past_[i].size() template void recover(size_t num_messages) { - if (i >= RealTypeCount::value) { - return; - } - std::vector::type> & v = std::get(past_); std::deque::type> & q = std::get(deques_); assert(num_messages <= v.size()); @@ -422,14 +337,15 @@ struct ApproximateTime : public PolicyBase } } + template + void recoverAll(std::index_sequence) + { + (recover(), ...); + } template void recover() { - if (i >= RealTypeCount::value) { - return; - } - std::vector::type> & v = std::get(past_); std::deque::type> & q = std::get(deques_); while (!v.empty()) { @@ -442,14 +358,15 @@ struct ApproximateTime : public PolicyBase } } + template + void recoverAndDeleteAll(std::index_sequence) + { + (recoverAndDelete(), ...); + } template void recoverAndDelete() { - if (i >= RealTypeCount::value) { - return; - } - std::vector::type> & v = std::get(past_); std::deque::type> & q = std::get(deques_); while (!v.empty()) { @@ -465,41 +382,28 @@ struct ApproximateTime : public PolicyBase } } - // Assumes: all deques are non empty, i.e. num_non_empty_deques_ == RealTypeCount::value + // Assumes: all deques are non empty, i.e. num_non_empty_deques_ == N_MESSAGES void publishCandidate() { // Publish - parent_->signal( - std::get<0>(candidate_), std::get<1>(candidate_), std::get<2>(candidate_), - std::get<3>(candidate_), - std::get<4>(candidate_), std::get<5>(candidate_), std::get<6>(candidate_), - std::get<7>(candidate_), - std::get<8>(candidate_)); + std::apply([this](auto &&... args) {this->parent_->signal(args ...);}, candidate_); // Delete this candidate candidate_ = Tuple(); pivot_ = NO_PIVOT; // Recover hidden messages, and delete the ones corresponding to the candidate num_non_empty_deques_ = 0; // We will recompute it from scratch - recoverAndDelete<0>(); - recoverAndDelete<1>(); - recoverAndDelete<2>(); - recoverAndDelete<3>(); - recoverAndDelete<4>(); - recoverAndDelete<5>(); - recoverAndDelete<6>(); - recoverAndDelete<7>(); - recoverAndDelete<8>(); - } - - // Assumes: all deques are non empty, i.e. num_non_empty_deques_ == RealTypeCount::value + recoverAndDeleteAll(std::make_index_sequence{}); + } + + // Assumes: all deques are non empty, i.e. num_non_empty_deques_ == N_MESSAGES // Returns: the oldest message on the deques void getCandidateStart(uint32_t & start_index, rclcpp::Time & start_time) { return getCandidateBoundary(start_index, start_time, false); } - // Assumes: all deques are non empty, i.e. num_non_empty_deques_ == RealTypeCount::value + // Assumes: all deques are non empty, i.e. num_non_empty_deques_ == N_MESSAGES // Returns: the latest message among the heads of the deques, i.e. the minimum // time to end an interval started at getCandidateStart_index() void getCandidateEnd(uint32_t & end_index, rclcpp::Time & end_time) @@ -507,84 +411,41 @@ struct ApproximateTime : public PolicyBase return getCandidateBoundary(end_index, end_time, true); } + template + void checkBoundary(uint32_t & index, rclcpp::Time & time, bool end) + { + namespace mt = message_filters::message_traits; + using MEvent = typename std::tuple_element::type; + MEvent & m = std::get(deques_).front(); + using M = typename MEvent::Message; + if ((I == 0) || ((mt::TimeStamp::value(*m.getMessage()) < time) ^ end)) { + time = mt::TimeStamp::value(*m.getMessage()); + index = I; + } + } + + template + void checkAllBoudnaries( + uint32_t & index, rclcpp::Time & time, bool end, + std::index_sequence) + { + (checkBoundary(index, time, end), ...); + } + // ASSUMES: all deques are non-empty // end = true: look for the latest head of deque // false: look for the earliest head of deque void getCandidateBoundary(uint32_t & index, rclcpp::Time & time, bool end) { - namespace mt = message_filters::message_traits; - - M0Event & m0 = std::get<0>(deques_).front(); - time = mt::TimeStamp::value(*m0.getMessage()); - index = 0; - if (RealTypeCount::value > 1) { - M1Event & m1 = std::get<1>(deques_).front(); - if ((mt::TimeStamp::value(*m1.getMessage()) < time) ^ end) { - time = mt::TimeStamp::value(*m1.getMessage()); - index = 1; - } - } - if (RealTypeCount::value > 2) { - M2Event & m2 = std::get<2>(deques_).front(); - if ((mt::TimeStamp::value(*m2.getMessage()) < time) ^ end) { - time = mt::TimeStamp::value(*m2.getMessage()); - index = 2; - } - } - if (RealTypeCount::value > 3) { - M3Event & m3 = std::get<3>(deques_).front(); - if ((mt::TimeStamp::value(*m3.getMessage()) < time) ^ end) { - time = mt::TimeStamp::value(*m3.getMessage()); - index = 3; - } - } - if (RealTypeCount::value > 4) { - M4Event & m4 = std::get<4>(deques_).front(); - if ((mt::TimeStamp::value(*m4.getMessage()) < time) ^ end) { - time = mt::TimeStamp::value(*m4.getMessage()); - index = 4; - } - } - if (RealTypeCount::value > 5) { - M5Event & m5 = std::get<5>(deques_).front(); - if ((mt::TimeStamp::value(*m5.getMessage()) < time) ^ end) { - time = mt::TimeStamp::value(*m5.getMessage()); - index = 5; - } - } - if (RealTypeCount::value > 6) { - M6Event & m6 = std::get<6>(deques_).front(); - if ((mt::TimeStamp::value(*m6.getMessage()) < time) ^ end) { - time = mt::TimeStamp::value(*m6.getMessage()); - index = 6; - } - } - if (RealTypeCount::value > 7) { - M7Event & m7 = std::get<7>(deques_).front(); - if ((mt::TimeStamp::value(*m7.getMessage()) < time) ^ end) { - time = mt::TimeStamp::value(*m7.getMessage()); - index = 7; - } - } - if (RealTypeCount::value > 8) { - M8Event & m8 = std::get<8>(deques_).front(); - if ((mt::TimeStamp::value(*m8.getMessage()) < time) ^ end) { - time = mt::TimeStamp::value(*m8.getMessage()); - index = 8; - } - } + checkAllBoudnaries(index, time, end, std::make_index_sequence{}); } - // ASSUMES: we have a pivot and candidate template rclcpp::Time getVirtualTime() { namespace mt = message_filters::message_traits; - if (i >= RealTypeCount::value) { - return rclcpp::Time(0, 0); // Dummy return value - } assert(pivot_ != NO_PIVOT); std::vector::type> & v = std::get(past_); @@ -619,25 +480,22 @@ struct ApproximateTime : public PolicyBase return getVirtualCandidateBoundary(end_index, end_time, true); } + template + std::array getVirtualTimes(std::index_sequence) + { + return {getVirtualTime()...}; + } + // ASSUMES: we have a pivot and candidate // end = true: look for the latest head of deque // false: look for the earliest head of deque void getVirtualCandidateBoundary(uint32_t & index, rclcpp::Time & time, bool end) { - std::vector virtual_times(9); - virtual_times[0] = getVirtualTime<0>(); - virtual_times[1] = getVirtualTime<1>(); - virtual_times[2] = getVirtualTime<2>(); - virtual_times[3] = getVirtualTime<3>(); - virtual_times[4] = getVirtualTime<4>(); - virtual_times[5] = getVirtualTime<5>(); - virtual_times[6] = getVirtualTime<6>(); - virtual_times[7] = getVirtualTime<7>(); - virtual_times[8] = getVirtualTime<8>(); + const auto virtual_times{getVirtualTimes(std::make_index_sequence{})}; time = virtual_times[0]; index = 0; - for (int i = 0; i < RealTypeCount::value; i++) { + for (uint32_t i = 0u; i < virtual_times.size(); i++) { if ((virtual_times[i] < time) ^ end) { time = virtual_times[i]; index = i; @@ -650,13 +508,13 @@ struct ApproximateTime : public PolicyBase void process() { // While no deque is empty - while (num_non_empty_deques_ == (uint32_t)RealTypeCount::value) { + while (num_non_empty_deques_ == (uint32_t)N_MESSAGES) { // Find the start and end of the current interval rclcpp::Time end_time, start_time; uint32_t end_index, start_index; getCandidateEnd(end_index, end_time); getCandidateStart(start_index, start_time); - for (uint32_t i = 0; i < (uint32_t)RealTypeCount::value; i++) { + for (uint32_t i = 0; i < (uint32_t)N_MESSAGES; i++) { if (i != end_index) { // No dropped message could have been better to use than the ones we have, // so it becomes ok to use this topic as pivot in the future @@ -715,11 +573,11 @@ struct ApproximateTime : public PolicyBase // Note: this case is subsumed by the next, but it may save some unnecessary work and // it makes things (a little) easier to understand publishCandidate(); - } else if (num_non_empty_deques_ < (uint32_t)RealTypeCount::value) { + } else if (num_non_empty_deques_ < (uint32_t)N_MESSAGES) { uint32_t num_non_empty_deques_before_virtual_search = num_non_empty_deques_; // Before giving up, use the rate bounds, if provided, to further try to prove optimality - std::vector num_virtual_moves(9, 0); + std::array num_virtual_moves{}; while (1) { rclcpp::Time end_time, start_time; uint32_t end_index, start_index; @@ -742,15 +600,7 @@ struct ApproximateTime : public PolicyBase // candidate // Cleanup the virtual search: num_non_empty_deques_ = 0; // We will recompute it from scratch - recover<0>(num_virtual_moves[0]); - recover<1>(num_virtual_moves[1]); - recover<2>(num_virtual_moves[2]); - recover<3>(num_virtual_moves[3]); - recover<4>(num_virtual_moves[4]); - recover<5>(num_virtual_moves[5]); - recover<6>(num_virtual_moves[6]); - recover<7>(num_virtual_moves[7]); - recover<8>(num_virtual_moves[8]); + recoverAll(num_virtual_moves, std::make_index_sequence{}); (void)num_non_empty_deques_before_virtual_search; // unused variable warning stopper assert(num_non_empty_deques_before_virtual_search == num_non_empty_deques_); break; @@ -764,14 +614,14 @@ struct ApproximateTime : public PolicyBase num_virtual_moves[start_index]++; } // while(1) } - } // while(num_non_empty_deques_ == (uint32_t)RealTypeCount::value) + } // while(num_non_empty_deques_ == (uint32_t)N_MESSAGES) } Sync * parent_; uint32_t queue_size_; // Special value for the pivot indicating that no pivot has been selected - static const uint32_t NO_PIVOT = 9; + static const uint32_t NO_PIVOT = std::numeric_limits::max(); DequeTuple deques_; uint32_t num_non_empty_deques_; diff --git a/include/message_filters/sync_policies/exact_time.hpp b/include/message_filters/sync_policies/exact_time.hpp index 16f1ba3..c424869 100644 --- a/include/message_filters/sync_policies/exact_time.hpp +++ b/include/message_filters/sync_policies/exact_time.hpp @@ -48,26 +48,15 @@ namespace message_filters namespace sync_policies { -template -struct ExactTime : public PolicyBase +template +struct ExactTime : public PolicyBase { typedef Synchronizer Sync; - typedef PolicyBase Super; + typedef PolicyBase Super; typedef typename Super::Messages Messages; typedef typename Super::Signal Signal; typedef typename Super::Events Events; typedef typename Super::RealTypeCount RealTypeCount; - typedef typename Super::M0Event M0Event; - typedef typename Super::M1Event M1Event; - typedef typename Super::M2Event M2Event; - typedef typename Super::M3Event M3Event; - typedef typename Super::M4Event M4Event; - typedef typename Super::M5Event M5Event; - typedef typename Super::M6Event M6Event; - typedef typename Super::M7Event M7Event; - typedef typename Super::M8Event M8Event; typedef Events Tuple; ExactTime(uint32_t queue_size) // NOLINT(runtime/explicit) @@ -105,8 +94,9 @@ struct ExactTime : public PolicyBase std::lock_guard lock(mutex_); - Tuple & t = tuples_[mt::TimeStamp::type>::value( - *evt.getMessage())]; + Tuple & t = + tuples_[mt::TimeStamp::type>::value(*evt.getMessage())]; std::get(t) = evt; checkTuple(t); @@ -136,41 +126,27 @@ struct ExactTime : public PolicyBase return drop_signal_.addCallback(callback, t); } - rclcpp::Time getLastSignalTime() const +private: + template + static bool isFull(const Tuple & t, std::index_sequence) { - return last_signal_time_; + /* *INDENT-OFF* */ + // uncrustify messes with the brackets which are required (at least by GCC) + return (... && static_cast(std::get(t).getMessage())); + /* *INDENT-ON* */ } -private: // assumes mutex_ is already locked void checkTuple(Tuple & t) { namespace mt = message_filters::message_traits; - bool full = true; - full = full && static_cast(std::get<0>(t).getMessage()); - full = full && static_cast(std::get<1>(t).getMessage()); - full = full && (RealTypeCount::value > 2 ? - static_cast(std::get<2>(t).getMessage()) : true); - full = full && (RealTypeCount::value > 3 ? - static_cast(std::get<3>(t).getMessage()) : true); - full = full && (RealTypeCount::value > 4 ? - static_cast(std::get<4>(t).getMessage()) : true); - full = full && (RealTypeCount::value > 5 ? - static_cast(std::get<5>(t).getMessage()) : true); - full = full && (RealTypeCount::value > 6 ? - static_cast(std::get<6>(t).getMessage()) : true); - full = full && (RealTypeCount::value > 7 ? - static_cast(std::get<7>(t).getMessage()) : true); - full = full && (RealTypeCount::value > 8 ? - static_cast(std::get<8>(t).getMessage()) : true); + const bool full = isFull(t, std::make_index_sequence>{}); if (full) { - parent_->signal( - std::get<0>(t), std::get<1>(t), std::get<2>(t), - std::get<3>(t), std::get<4>(t), std::get<5>(t), - std::get<6>(t), std::get<7>(t), std::get<8>(t)); + std::apply([this](auto &&... args) {this->parent_->signal(args ...);}, t); + using M0 = std::tuple_element_t<0, std::tuple>; last_signal_time_ = mt::TimeStamp::value(*std::get<0>(t).getMessage()); tuples_.erase(last_signal_time_); @@ -181,10 +157,7 @@ struct ExactTime : public PolicyBase if (queue_size_ > 0) { while (tuples_.size() > queue_size_) { Tuple & t2 = tuples_.begin()->second; - drop_signal_.call( - std::get<0>(t2), std::get<1>(t2), std::get<2>(t2), - std::get<3>(t2), std::get<4>(t2), std::get<5>(t2), - std::get<6>(t2), std::get<7>(t2), std::get<8>(t2)); + std::apply([this](auto &&... args) {this->drop_signal_.call(args ...);}, t2); tuples_.erase(tuples_.begin()); } } @@ -201,10 +174,7 @@ struct ExactTime : public PolicyBase ++it; Tuple & t = old->second; - drop_signal_.call( - std::get<0>(t), std::get<1>(t), std::get<2>(t), - std::get<3>(t), std::get<4>(t), std::get<5>(t), - std::get<6>(t), std::get<7>(t), std::get<8>(t)); + std::apply([this](auto &&... args) {this->drop_signal_.call(args ...);}, t); tuples_.erase(old); } else { // the map is sorted by time, so we can ignore anything after this if this one's time is ok diff --git a/include/message_filters/sync_policies/latest_time.hpp b/include/message_filters/sync_policies/latest_time.hpp index 5352961..96fd584 100644 --- a/include/message_filters/sync_policies/latest_time.hpp +++ b/include/message_filters/sync_policies/latest_time.hpp @@ -81,14 +81,11 @@ namespace message_filters namespace sync_policies { -template -struct LatestTime : public PolicyBase +template +struct LatestTime : public PolicyBase { typedef Synchronizer Sync; - typedef PolicyBase Super; + typedef PolicyBase Super; typedef typename Super::Messages Messages; typedef typename Super::Signal Signal; typedef typename Super::Events Events; @@ -204,10 +201,7 @@ struct LatestTime : public PolicyBase // assumed data_mutex_ is locked void publish() { - parent_->signal( - std::get<0>(events_), std::get<1>(events_), std::get<2>(events_), - std::get<3>(events_), std::get<4>(events_), std::get<5>(events_), - std::get<6>(events_), std::get<7>(events_), std::get<8>(events_)); + std::apply([this](auto &&... args) {this->parent_->signal(args ...);}, events_); } struct Rate @@ -315,23 +309,22 @@ struct LatestTime : public PolicyBase template bool received_msg() { - return RealTypeCount::value > i ? static_cast(std::get(events_).getMessage()) : true; + return static_cast(std::get(events_).getMessage()); } // assumed data_mutex_ is locked bool is_full() { - bool full = received_msg<0>(); - full = full && received_msg<1>(); - full = full && received_msg<2>(); - full = full && received_msg<3>(); - full = full && received_msg<4>(); - full = full && received_msg<5>(); - full = full && received_msg<6>(); - full = full && received_msg<7>(); - full = full && received_msg<8>(); - - return full; + return is_full_impl(std::make_index_sequence>{}); + } + + template + bool is_full_impl(std::index_sequence) + { + /* *INDENT-OFF* */ + // uncrustify messes with the brackets which are required (at least by GCC) + return (... && received_msg()); + /* *INDENT-ON* */ } // assumed data_mutex_ is locked diff --git a/include/message_filters/synchronizer.hpp b/include/message_filters/synchronizer.hpp index 2ee0d7d..95d14ce 100644 --- a/include/message_filters/synchronizer.hpp +++ b/include/message_filters/synchronizer.hpp @@ -53,82 +53,10 @@ class Synchronizer : public noncopyable, public Policy typedef typename Policy::Events Events; typedef typename Policy::Signal Signal; - typedef typename std::tuple_element<0, Messages>::type M0; - typedef typename std::tuple_element<1, Messages>::type M1; - typedef typename std::tuple_element<2, Messages>::type M2; - typedef typename std::tuple_element<3, Messages>::type M3; - typedef typename std::tuple_element<4, Messages>::type M4; - typedef typename std::tuple_element<5, Messages>::type M5; - typedef typename std::tuple_element<6, Messages>::type M6; - typedef typename std::tuple_element<7, Messages>::type M7; - typedef typename std::tuple_element<8, Messages>::type M8; - - - typedef typename std::tuple_element<0, Events>::type M0Event; - typedef typename std::tuple_element<1, Events>::type M1Event; - typedef typename std::tuple_element<2, Events>::type M2Event; - typedef typename std::tuple_element<3, Events>::type M3Event; - typedef typename std::tuple_element<4, Events>::type M4Event; - typedef typename std::tuple_element<5, Events>::type M5Event; - typedef typename std::tuple_element<6, Events>::type M6Event; - typedef typename std::tuple_element<7, Events>::type M7Event; - typedef typename std::tuple_element<8, Events>::type M8Event; - - static const uint8_t MAX_MESSAGES = 9; - - template - Synchronizer(F0 & f0, F1 & f1) + template + Synchronizer(F0 & f0, F1 & f1, Fs &... fs) { - connectInput(f0, f1); - init(); - } - - template - Synchronizer(F0 & f0, F1 & f1, F2 & f2) - { - connectInput(f0, f1, f2); - init(); - } - - template - Synchronizer(F0 & f0, F1 & f1, F2 & f2, F3 & f3) - { - connectInput(f0, f1, f2, f3); - init(); - } - - template - Synchronizer(F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4) - { - connectInput(f0, f1, f2, f3, f4); - init(); - } - - template - Synchronizer(F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5) - { - connectInput(f0, f1, f2, f3, f4, f5); - init(); - } - - template - Synchronizer(F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5, F6 & f6) - { - connectInput(f0, f1, f2, f3, f4, f5, f6); - init(); - } - - template - Synchronizer(F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5, F6 & f6, F7 & f7) - { - connectInput(f0, f1, f2, f3, f4, f5, f6, f7); - init(); - } - - template - Synchronizer(F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5, F6 & f6, F7 & f7, F8 & f8) - { - connectInput(f0, f1, f2, f3, f4, f5, f6, f7, f8); + connectInput(f0, f1, fs ...); init(); } @@ -137,70 +65,11 @@ class Synchronizer : public noncopyable, public Policy init(); } - template - Synchronizer(const Policy & policy, F0 & f0, F1 & f1) - : Policy(policy) - { - connectInput(f0, f1); - init(); - } - - template - Synchronizer(const Policy & policy, F0 & f0, F1 & f1, F2 & f2) - : Policy(policy) - { - connectInput(f0, f1, f2); - init(); - } - - template - Synchronizer(const Policy & policy, F0 & f0, F1 & f1, F2 & f2, F3 & f3) - : Policy(policy) - { - connectInput(f0, f1, f2, f3); - init(); - } - - template - Synchronizer(const Policy & policy, F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4) + template + Synchronizer(const Policy & policy, Fs &... fs) : Policy(policy) { - connectInput(f0, f1, f2, f3, f4); - init(); - } - - template - Synchronizer(const Policy & policy, F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5) - : Policy(policy) - { - connectInput(f0, f1, f2, f3, f4, f5); - init(); - } - - template - Synchronizer(const Policy & policy, F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5, F6 & f6) - : Policy(policy) - { - connectInput(f0, f1, f2, f3, f4, f5, f6); - init(); - } - - template - Synchronizer( - const Policy & policy, F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5, F6 & f6, F7 & f7) - : Policy(policy) - { - connectInput(f0, f1, f2, f3, f4, f5, f6, f7); - init(); - } - - template - Synchronizer( - const Policy & policy, F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5, F6 & f6, F7 & f7, - F8 & f8) - : Policy(policy) - { - connectInput(f0, f1, f2, f3, f4, f5, f6, f7, f8); + connectInput(fs ...); init(); } @@ -220,87 +89,30 @@ class Synchronizer : public noncopyable, public Policy Policy::initParent(this); } - template - void connectInput(F0 & f0, F1 & f1) + template + void connect(FTuple & ftuple) { - NullFilter f2; - connectInput(f0, f1, f2); + using MEvent = typename std::tuple_element::type; + input_connections_[I] = + std::get(ftuple).registerCallback( + std::function( + std::bind( + &Synchronizer::template cb, this, std::placeholders::_1))); } - template - void connectInput(F0 & f0, F1 & f1, F2 & f2) + template + void connectInputImpl(FTuple & ftuple, std::index_sequence) { - NullFilter f3; - connectInput(f0, f1, f2, f3); + (connect(ftuple), ...); } - template - void connectInput(F0 & f0, F1 & f1, F2 & f2, F3 & f3) - { - NullFilter f4; - connectInput(f0, f1, f2, f3, f4); - } - - template - void connectInput(F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4) - { - NullFilter f5; - connectInput(f0, f1, f2, f3, f4, f5); - } - - template - void connectInput(F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5) - { - NullFilter f6; - connectInput(f0, f1, f2, f3, f4, f5, f6); - } - - template - void connectInput(F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5, F6 & f6) - { - NullFilter f7; - connectInput(f0, f1, f2, f3, f4, f5, f6, f7); - } - - template - void connectInput(F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5, F6 & f6, F7 & f7) - { - NullFilter f8; - connectInput(f0, f1, f2, f3, f4, f5, f6, f7, f8); - } - - template - void connectInput(F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5, F6 & f6, F7 & f7, F8 & f8) + template + void connectInput(Fs &... fs) { disconnectAll(); - input_connections_[0] = f0.registerCallback( - std::function( - std::bind(&Synchronizer::template cb<0>, this, std::placeholders::_1))); - input_connections_[1] = f1.registerCallback( - std::function( - std::bind(&Synchronizer::template cb<1>, this, std::placeholders::_1))); - input_connections_[2] = f2.registerCallback( - std::function( - std::bind(&Synchronizer::template cb<2>, this, std::placeholders::_1))); - input_connections_[3] = f3.registerCallback( - std::function( - std::bind(&Synchronizer::template cb<3>, this, std::placeholders::_1))); - input_connections_[4] = f4.registerCallback( - std::function( - std::bind(&Synchronizer::template cb<4>, this, std::placeholders::_1))); - input_connections_[5] = f5.registerCallback( - std::function( - std::bind(&Synchronizer::template cb<5>, this, std::placeholders::_1))); - input_connections_[6] = f6.registerCallback( - std::function( - std::bind(&Synchronizer::template cb<6>, this, std::placeholders::_1))); - input_connections_[7] = f7.registerCallback( - std::function( - std::bind(&Synchronizer::template cb<7>, this, std::placeholders::_1))); - input_connections_[8] = f8.registerCallback( - std::function( - std::bind(&Synchronizer::template cb<8>, this, std::placeholders::_1))); + std::tuple tuple{fs ...}; + connectInputImpl(tuple, std::make_index_sequence{}); } template @@ -331,12 +143,10 @@ class Synchronizer : public noncopyable, public Policy const std::string & getName() {return name_;} - 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) + template + void signal(const MEvent &... es) { - signal_.call(e0, e1, e2, e3, e4, e5, e6, e7, e8); + signal_.call(es ...); } Policy * getPolicy() {return static_cast(this);} @@ -352,8 +162,8 @@ class Synchronizer : public noncopyable, public Policy private: void disconnectAll() { - for (int i = 0; i < MAX_MESSAGES; ++i) { - input_connections_[i].disconnect(); + for (auto & input_connection : input_connections_) { + input_connection.disconnect(); } } @@ -367,55 +177,19 @@ class Synchronizer : public noncopyable, public Policy Signal signal_; - Connection input_connections_[MAX_MESSAGES]; + Connection input_connections_[Policy::N_MESSAGES]; std::string name_; }; -template -struct mp_plus; -template<> -struct mp_plus<> -{ - using type = std::integral_constant; -}; -template -struct mp_plus -{ - static constexpr auto _v = !T1::value + mp_plus::type::value; - using type = std::integral_constant< - typename std::remove_const::type, _v>; -}; - -template -struct mp_count; -template class L, class ... T, class V> -struct mp_count, V> -{ - using type = typename mp_plus...>::type; -}; - -template +template struct PolicyBase { - typedef typename mp_count, NullType>::type - RealTypeCount; - typedef std::tuple Messages; - typedef Signal9 Signal; - typedef std::tuple, MessageEvent, MessageEvent, - MessageEvent, MessageEvent, MessageEvent, - MessageEvent, MessageEvent, MessageEvent> Events; - - typedef typename std::tuple_element<0, Events>::type M0Event; - typedef typename std::tuple_element<1, Events>::type M1Event; - typedef typename std::tuple_element<2, Events>::type M2Event; - typedef typename std::tuple_element<3, Events>::type M3Event; - typedef typename std::tuple_element<4, Events>::type M4Event; - typedef typename std::tuple_element<5, Events>::type M5Event; - typedef typename std::tuple_element<6, Events>::type M6Event; - typedef typename std::tuple_element<7, Events>::type M7Event; - typedef typename std::tuple_element<8, Events>::type M8Event; + static constexpr std::size_t N_MESSAGES = sizeof...(Ms); + typedef std::integral_constant RealTypeCount; + typedef std::tuple Messages; + typedef Signal9 Signal; + typedef std::tuple...> Events; }; } // namespace message_filters diff --git a/include/message_filters/time_synchronizer.hpp b/include/message_filters/time_synchronizer.hpp index cb82e4e..aa54561 100644 --- a/include/message_filters/time_synchronizer.hpp +++ b/include/message_filters/time_synchronizer.hpp @@ -73,23 +73,12 @@ void callback(const sensor_msgs::msg::CameraInfo::SharedPtr, const sensor_msgs:: \endverbatim * */ -template -class TimeSynchronizer : public Synchronizer> +template +class TimeSynchronizer : public Synchronizer> { public: - typedef sync_policies::ExactTime Policy; + typedef sync_policies::ExactTime Policy; typedef Synchronizer Base; - typedef std::shared_ptr M0ConstPtr; - typedef std::shared_ptr M1ConstPtr; - typedef std::shared_ptr M2ConstPtr; - typedef std::shared_ptr M3ConstPtr; - typedef std::shared_ptr M4ConstPtr; - typedef std::shared_ptr M5ConstPtr; - typedef std::shared_ptr M6ConstPtr; - typedef std::shared_ptr M7ConstPtr; - typedef std::shared_ptr M8ConstPtr; using Base::add; using Base::connectInput; @@ -97,132 +86,60 @@ class TimeSynchronizer : public Synchronizer - TimeSynchronizer(F0 & f0, F1 & f1, uint32_t queue_size) + template + TimeSynchronizer(uint32_t queue_size, Fs &... fs) : Base(Policy(queue_size)) { - connectInput(f0, f1); + connectInput(fs ...); } + template + [[deprecated("Use variade constructor instead")]] + TimeSynchronizer(F0 & f0, F1 & f1, uint32_t queue_size) + : TimeSynchronizer(queue_size, f0, f1) {} + template + [[deprecated("Use variade constructor instead")]] TimeSynchronizer(F0 & f0, F1 & f1, F2 & f2, uint32_t queue_size) - : Base(Policy(queue_size)) - { - connectInput(f0, f1, f2); - } + : TimeSynchronizer(queue_size, f0, f1, f2) {} template + [[deprecated("Use variade constructor instead")]] TimeSynchronizer(F0 & f0, F1 & f1, F2 & f2, F3 & f3, uint32_t queue_size) - : Base(Policy(queue_size)) - { - connectInput(f0, f1, f2, f3); - } + : TimeSynchronizer(queue_size, f0, f1, f2, f3) {} template + [[deprecated("Use variade constructor instead")]] TimeSynchronizer(F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, uint32_t queue_size) - : Base(Policy(queue_size)) - { - connectInput(f0, f1, f2, f3, f4); - } + : TimeSynchronizer(queue_size, f0, f1, f2, f3, f4) {} template + [[deprecated("Use variade constructor instead")]] TimeSynchronizer(F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5, uint32_t queue_size) - : Base(Policy(queue_size)) - { - connectInput(f0, f1, f2, f3, f4, f5); - } + : TimeSynchronizer(queue_size, f0, f1, f2, f3, f4, f5) {} template + [[deprecated("Use variade constructor instead")]] TimeSynchronizer( F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5, F6 & f6, uint32_t queue_size) - : Base(Policy(queue_size)) - { - connectInput(f0, f1, f2, f3, f4, f5, f6); - } + : TimeSynchronizer(queue_size, f0, f1, f2, f3, f4, f5, f6) {} template + [[deprecated("Use variade constructor instead")]] TimeSynchronizer( F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5, F6 & f6, F7 & f7, uint32_t queue_size) - : Base(Policy(queue_size)) - { - connectInput(f0, f1, f2, f3, f4, f5, f6, f7); - } + : TimeSynchronizer(queue_size, f0, f1, f2, f3, f4, f5, f6, f7) {} template + [[deprecated("Use variade constructor instead")]] TimeSynchronizer( F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5, F6 & f6, F7 & f7, F8 & f8, uint32_t queue_size) - : Base(Policy(queue_size)) - { - connectInput(f0, f1, f2, f3, f4, f5, f6, f7, f8); - } - - TimeSynchronizer(uint32_t queue_size) // NOLINT(runtime/explicit) - : Base(Policy(queue_size)) - { - } - - //////////////////////////////////////////////////////////////// - // For backwards compatibility - //////////////////////////////////////////////////////////////// - void add0(const M0ConstPtr & msg) - { - this->template add<0>(M0Event(msg)); - } - - void add1(const M1ConstPtr & msg) - { - this->template add<1>(M1Event(msg)); - } - - void add2(const M2ConstPtr & msg) - { - this->template add<2>(M2Event(msg)); - } - - void add3(const M3ConstPtr & msg) - { - this->template add<3>(M3Event(msg)); - } - - void add4(const M4ConstPtr & msg) - { - this->template add<4>(M4Event(msg)); - } - - void add5(const M5ConstPtr & msg) - { - this->template add<5>(M5Event(msg)); - } - - void add6(const M6ConstPtr & msg) - { - this->template add<6>(M6Event(msg)); - } - - void add7(const M7ConstPtr & msg) - { - this->template add<7>(M7Event(msg)); - } - - void add8(const M8ConstPtr & msg) - { - this->template add<8>(M8Event(msg)); - } + : TimeSynchronizer(queue_size, f0, f1, f2, f3, f4, f5, f6, f7, f8) {} }; - } // namespace message_filters #endif // MESSAGE_FILTERS__TIME_SYNCHRONIZER_HPP_ diff --git a/test/test_fuzz.cpp b/test/test_fuzz.cpp index 030d646..9f4fad5 100644 --- a/test/test_fuzz.cpp +++ b/test/test_fuzz.cpp @@ -117,9 +117,9 @@ TEST(TimeSynchronizer, fuzz_synchronizer) msg1->header.stamp = rclcpp::Clock().now(); fuzz_msg(msg2); msg2->header.stamp = msg1->header.stamp; - sync.add0(msg1); + sync.add<0>(msg1); ASSERT_EQ(h.count_, 0); - sync.add1(msg2); + sync.add<1>(msg2); ASSERT_EQ(h.count_, 1); rclcpp::Rate(50).sleep(); } diff --git a/test/test_synchronizer.cpp b/test/test_synchronizer.cpp index 08c0d2a..67d71a5 100644 --- a/test/test_synchronizer.cpp +++ b/test/test_synchronizer.cpp @@ -49,14 +49,11 @@ typedef std::shared_ptr MsgPtr; typedef std::shared_ptr MsgConstPtr; -template -struct NullPolicy : public message_filters::PolicyBase +template +struct NullPolicy : public message_filters::PolicyBase { typedef message_filters::Synchronizer Sync; - typedef message_filters::PolicyBase Super; + typedef message_filters::PolicyBase Super; typedef typename Super::Messages Messages; typedef typename Super::Signal Signal; typedef typename Super::Events Events; diff --git a/test/time_synchronizer_unittest.cpp b/test/time_synchronizer_unittest.cpp index a41087e..5e8723c 100644 --- a/test/time_synchronizer_unittest.cpp +++ b/test/time_synchronizer_unittest.cpp @@ -88,52 +88,52 @@ class Helper TEST(TimeSynchronizer, compile2) { message_filters::NullFilter f0, f1; - message_filters::TimeSynchronizer sync(f0, f1, 1); + message_filters::TimeSynchronizer sync(1, f0, f1); } TEST(TimeSynchronizer, compile3) { message_filters::NullFilter f0, f1, f2; - message_filters::TimeSynchronizer sync(f0, f1, f2, 1); + message_filters::TimeSynchronizer sync(1, f0, f1, f2); } TEST(TimeSynchronizer, compile4) { message_filters::NullFilter f0, f1, f2, f3; - message_filters::TimeSynchronizer sync(f0, f1, f2, f3, 1); + message_filters::TimeSynchronizer sync(1, f0, f1, f2, f3); } TEST(TimeSynchronizer, compile5) { message_filters::NullFilter f0, f1, f2, f3, f4; - message_filters::TimeSynchronizer sync(f0, f1, f2, f3, f4, 1); + message_filters::TimeSynchronizer sync(1, f0, f1, f2, f3, f4); } TEST(TimeSynchronizer, compile6) { message_filters::NullFilter f0, f1, f2, f3, f4, f5; - message_filters::TimeSynchronizer sync(f0, f1, f2, f3, f4, f5, 1); + message_filters::TimeSynchronizer sync(1, f0, f1, f2, f3, f4, f5); } TEST(TimeSynchronizer, compile7) { message_filters::NullFilter f0, f1, f2, f3, f4, f5, f6; - message_filters::TimeSynchronizer sync(f0, f1, f2, f3, f4, f5, - f6, 1); + message_filters::TimeSynchronizer sync(1, f0, f1, f2, f3, f4, + f5, f6); } TEST(TimeSynchronizer, compile8) { message_filters::NullFilter f0, f1, f2, f3, f4, f5, f6, f7; - message_filters::TimeSynchronizer sync(f0, f1, f2, f3, f4, - f5, f6, f7, 1); + message_filters::TimeSynchronizer sync(1, f0, f1, f2, f3, + f4, f5, f6, f7); } TEST(TimeSynchronizer, compile9) { message_filters::NullFilter f0, f1, f2, f3, f4, f5, f6, f7, f8; - message_filters::TimeSynchronizer sync(f0, f1, - f2, f3, f4, f5, f6, f7, f8, 1); + message_filters::TimeSynchronizer sync(1, f0, f1, f2, + f3, f4, f5, f6, f7, f8); } void function2(const MsgConstPtr &, const MsgConstPtr &) {} @@ -286,9 +286,9 @@ TEST(TimeSynchronizer, immediate2) MsgPtr m(std::make_shared()); m->header.stamp = rclcpp::Clock().now(); - sync.add0(m); + sync.add<0>(m); ASSERT_EQ(h.count_, 0); - sync.add1(m); + sync.add<1>(m); ASSERT_EQ(h.count_, 1); } @@ -300,11 +300,11 @@ TEST(TimeSynchronizer, immediate3) MsgPtr m(std::make_shared()); m->header.stamp = rclcpp::Clock().now(); - sync.add0(m); + sync.add<0>(m); ASSERT_EQ(h.count_, 0); - sync.add1(m); + sync.add<1>(m); ASSERT_EQ(h.count_, 0); - sync.add2(m); + sync.add<2>(m); ASSERT_EQ(h.count_, 1); } @@ -316,13 +316,13 @@ TEST(TimeSynchronizer, immediate4) MsgPtr m(std::make_shared()); m->header.stamp = rclcpp::Clock().now(); - sync.add0(m); + sync.add<0>(m); ASSERT_EQ(h.count_, 0); - sync.add1(m); + sync.add<1>(m); ASSERT_EQ(h.count_, 0); - sync.add2(m); + sync.add<2>(m); ASSERT_EQ(h.count_, 0); - sync.add3(m); + sync.add<3>(m); ASSERT_EQ(h.count_, 1); } @@ -334,15 +334,15 @@ TEST(TimeSynchronizer, immediate5) MsgPtr m(std::make_shared()); m->header.stamp = rclcpp::Clock().now(); - sync.add0(m); + sync.add<0>(m); ASSERT_EQ(h.count_, 0); - sync.add1(m); + sync.add<1>(m); ASSERT_EQ(h.count_, 0); - sync.add2(m); + sync.add<2>(m); ASSERT_EQ(h.count_, 0); - sync.add3(m); + sync.add<3>(m); ASSERT_EQ(h.count_, 0); - sync.add4(m); + sync.add<4>(m); ASSERT_EQ(h.count_, 1); } @@ -354,17 +354,17 @@ TEST(TimeSynchronizer, immediate6) MsgPtr m(std::make_shared()); m->header.stamp = rclcpp::Clock().now(); - sync.add0(m); + sync.add<0>(m); ASSERT_EQ(h.count_, 0); - sync.add1(m); + sync.add<1>(m); ASSERT_EQ(h.count_, 0); - sync.add2(m); + sync.add<2>(m); ASSERT_EQ(h.count_, 0); - sync.add3(m); + sync.add<3>(m); ASSERT_EQ(h.count_, 0); - sync.add4(m); + sync.add<4>(m); ASSERT_EQ(h.count_, 0); - sync.add5(m); + sync.add<5>(m); ASSERT_EQ(h.count_, 1); } @@ -376,19 +376,19 @@ TEST(TimeSynchronizer, immediate7) MsgPtr m(std::make_shared()); m->header.stamp = rclcpp::Clock().now(); - sync.add0(m); + sync.add<0>(m); ASSERT_EQ(h.count_, 0); - sync.add1(m); + sync.add<1>(m); ASSERT_EQ(h.count_, 0); - sync.add2(m); + sync.add<2>(m); ASSERT_EQ(h.count_, 0); - sync.add3(m); + sync.add<3>(m); ASSERT_EQ(h.count_, 0); - sync.add4(m); + sync.add<4>(m); ASSERT_EQ(h.count_, 0); - sync.add5(m); + sync.add<5>(m); ASSERT_EQ(h.count_, 0); - sync.add6(m); + sync.add<6>(m); ASSERT_EQ(h.count_, 1); } @@ -400,21 +400,21 @@ TEST(TimeSynchronizer, immediate8) MsgPtr m(std::make_shared()); m->header.stamp = rclcpp::Clock().now(); - sync.add0(m); + sync.add<0>(m); ASSERT_EQ(h.count_, 0); - sync.add1(m); + sync.add<1>(m); ASSERT_EQ(h.count_, 0); - sync.add2(m); + sync.add<2>(m); ASSERT_EQ(h.count_, 0); - sync.add3(m); + sync.add<3>(m); ASSERT_EQ(h.count_, 0); - sync.add4(m); + sync.add<4>(m); ASSERT_EQ(h.count_, 0); - sync.add5(m); + sync.add<5>(m); ASSERT_EQ(h.count_, 0); - sync.add6(m); + sync.add<6>(m); ASSERT_EQ(h.count_, 0); - sync.add7(m); + sync.add<7>(m); ASSERT_EQ(h.count_, 1); } @@ -426,23 +426,23 @@ TEST(TimeSynchronizer, immediate9) MsgPtr m(std::make_shared()); m->header.stamp = rclcpp::Clock().now(); - sync.add0(m); + sync.add<0>(m); ASSERT_EQ(h.count_, 0); - sync.add1(m); + sync.add<1>(m); ASSERT_EQ(h.count_, 0); - sync.add2(m); + sync.add<2>(m); ASSERT_EQ(h.count_, 0); - sync.add3(m); + sync.add<3>(m); ASSERT_EQ(h.count_, 0); - sync.add4(m); + sync.add<4>(m); ASSERT_EQ(h.count_, 0); - sync.add5(m); + sync.add<5>(m); ASSERT_EQ(h.count_, 0); - sync.add6(m); + sync.add<6>(m); ASSERT_EQ(h.count_, 0); - sync.add7(m); + sync.add<7>(m); ASSERT_EQ(h.count_, 0); - sync.add8(m); + sync.add<8>(m); ASSERT_EQ(h.count_, 1); } @@ -458,16 +458,16 @@ TEST(TimeSynchronizer, multipleTimes) MsgPtr m(std::make_shared()); m->header.stamp = rclcpp::Time(); - sync.add0(m); + sync.add<0>(m); ASSERT_EQ(h.count_, 0); m = std::make_shared(); m->header.stamp = rclcpp::Time(100000000); - sync.add1(m); + sync.add<1>(m); ASSERT_EQ(h.count_, 0); - sync.add0(m); + sync.add<0>(m); ASSERT_EQ(h.count_, 0); - sync.add2(m); + sync.add<2>(m); ASSERT_EQ(h.count_, 1); } @@ -479,21 +479,21 @@ TEST(TimeSynchronizer, queueSize) MsgPtr m(std::make_shared()); m->header.stamp = rclcpp::Time(); - sync.add0(m); + sync.add<0>(m); ASSERT_EQ(h.count_, 0); - sync.add1(m); + sync.add<1>(m); ASSERT_EQ(h.count_, 0); m = std::make_shared(); m->header.stamp = rclcpp::Time(100000000); - sync.add1(m); + sync.add<1>(m); ASSERT_EQ(h.count_, 0); m = std::make_shared(); m->header.stamp = rclcpp::Time(); - sync.add1(m); + sync.add<1>(m); ASSERT_EQ(h.count_, 0); - sync.add2(m); + sync.add<2>(m); ASSERT_EQ(h.count_, 0); } @@ -506,10 +506,10 @@ TEST(TimeSynchronizer, dropCallback) MsgPtr m(std::make_shared()); m->header.stamp = rclcpp::Time(0, 0); - sync.add0(m); + sync.add<0>(m); ASSERT_EQ(h.drop_count_, 0); m->header.stamp = rclcpp::Time(100000000); - sync.add0(m); + sync.add<0>(m); ASSERT_EQ(h.drop_count_, 1); } @@ -547,7 +547,7 @@ TEST(TimeSynchronizer, eventInEventOut) TEST(TimeSynchronizer, connectConstructor) { message_filters::PassThrough pt1, pt2; - message_filters::TimeSynchronizer sync(pt1, pt2, 1); + message_filters::TimeSynchronizer sync(1, pt1, pt2); Helper h; sync.registerCallback(std::bind(&Helper::cb, &h)); MsgPtr m(std::make_shared());