From ab0f734b256ef4fab79324378b8e653405824e35 Mon Sep 17 00:00:00 2001 From: Lucas Wendland <82680922+CursedRock17@users.noreply.github.com> Date: Tue, 21 May 2024 07:07:13 -0400 Subject: [PATCH] Adding Uncrustify Changes (#124) Signed-off-by: CursedRock17 Signed-off-by: Lucas Wendland <82680922+CursedRock17@users.noreply.github.com> --- CMakeLists.txt | 2 + include/message_filters/cache.h | 172 +++--- include/message_filters/chain.h | 59 +- include/message_filters/connection.h | 10 +- include/message_filters/message_event.h | 87 +-- include/message_filters/message_traits.h | 23 +- include/message_filters/null_types.h | 6 +- include/message_filters/parameter_adapter.h | 46 +- include/message_filters/pass_through.h | 18 +- include/message_filters/signal1.h | 32 +- include/message_filters/signal9.h | 273 ++++++--- include/message_filters/simple_filter.h | 26 +- include/message_filters/subscriber.h | 88 +-- .../sync_policies/approximate_epsilon_time.h | 49 +- .../sync_policies/approximate_time.h | 563 ++++++++---------- .../sync_policies/exact_time.h | 78 ++- .../sync_policies/latest_time.h | 80 +-- include/message_filters/synchronizer.h | 144 +++-- include/message_filters/time_sequencer.h | 72 ++- include/message_filters/time_synchronizer.h | 45 +- include/message_filters/visibility_control.h | 1 - package.xml | 1 + src/connection.cpp | 9 +- test/msg_cache_unittest.cpp | 115 ++-- test/test_approximate_epsilon_time_policy.cpp | 90 +-- test/test_approximate_time_policy.cpp | 240 ++++---- test/test_chain.cpp | 44 +- test/test_exact_time_policy.cpp | 12 +- test/test_fuzz.cpp | 10 +- test/test_latest_time_policy.cpp | 19 +- test/test_simple.cpp | 24 +- test/test_subscriber.cpp | 12 +- test/test_synchronizer.cpp | 66 +- test/time_sequencer_unittest.cpp | 27 +- test/time_synchronizer_unittest.cpp | 80 ++- 35 files changed, 1425 insertions(+), 1198 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c4fe0c4..e259fe6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -58,10 +58,12 @@ ament_export_dependencies(rclcpp rcutils std_msgs) if(BUILD_TESTING) find_package(ament_cmake_copyright) + find_package(ament_cmake_uncrustify REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(sensor_msgs REQUIRED) ament_copyright() + ament_uncrustify(LANGUAGE "c++") find_package(ament_cmake_gtest) diff --git a/include/message_filters/cache.h b/include/message_filters/cache.h index 433d3ba..24f3fcc 100644 --- a/include/message_filters/cache.h +++ b/include/message_filters/cache.h @@ -29,9 +29,11 @@ #ifndef MESSAGE_FILTERS__CACHE_H_ #define MESSAGE_FILTERS__CACHE_H_ +#include #include #include #include +#include #include @@ -53,7 +55,7 @@ namespace message_filters * * Cache's input and output connections are both of the same signature as rclcpp subscription callbacks, ie. \verbatim -void callback(const std::shared_ptr&); +void callback(const std::shared_ptr &); \endverbatim */ template @@ -64,10 +66,10 @@ class Cache : public SimpleFilter typedef MessageEvent EventType; template - Cache(F& f, unsigned int cache_size = 1) + explicit Cache(F & f, unsigned int cache_size = 1) { - setCacheSize(cache_size) ; - connectInput(f) ; + setCacheSize(cache_size); + connectInput(f); } /** @@ -75,16 +77,17 @@ class Cache : public SimpleFilter * order to populate the cache, the user then has to call add themselves, or connectInput() is * called later */ - Cache(unsigned int cache_size = 1) + explicit Cache(unsigned int cache_size = 1) { setCacheSize(cache_size); } template - void connectInput(F& f) + void connectInput(F & f) { - incoming_connection_ = f.registerCallback(typename SimpleFilter::EventCallback( - std::bind(&Cache::callback, this, std::placeholders::_1))); + incoming_connection_ = f.registerCallback( + typename SimpleFilter::EventCallback( + std::bind(&Cache::callback, this, std::placeholders::_1))); } ~Cache() @@ -98,20 +101,19 @@ class Cache : public SimpleFilter */ void setCacheSize(unsigned int cache_size) { - if (cache_size == 0) - { - //ROS_ERROR("Cannot set max_size to 0") ; - return ; + if (cache_size == 0) { + // ROS_ERROR("Cannot set max_size to 0"); + return; } - cache_size_ = cache_size ; + cache_size_ = cache_size; } /** * \brief Add a message to the cache, and pop off any elements that are too old. * This method is registered with a data provider when connectTo is called. */ - void add(const MConstPtr& msg) + void add(const MConstPtr & msg) { add(EventType(msg)); } @@ -120,31 +122,37 @@ class Cache : public SimpleFilter * \brief Add a message to the cache, and pop off any elements that are too old. * This method is registered with a data provider when connectTo is called. */ - void add(const EventType& evt) + void add(const EventType & evt) { namespace mt = message_filters::message_traits; - //printf(" Cache Size: %u\n", cache_.size()) ; + // printf(" Cache Size: %u\n", cache_.size()); { std::lock_guard lock(cache_lock_); - while (cache_.size() >= cache_size_) // Keep popping off old data until we have space for a new msg - cache_.pop_front() ; // The front of the deque has the oldest elem, so we can get rid of it + // Keep popping off old data until we have space for a new msg + // The front of the deque has the oldest elem, so we can get rid of it + while (cache_.size() >= cache_size_) { + cache_.pop_front(); + } // No longer naively pushing msgs to back. Want to make sure they're sorted correctly - //cache_.push_back(msg) ; // Add the newest message to the back of the deque + // cache_.push_back(msg); + // Add the newest message to the back of the deque - typename std::deque::reverse_iterator rev_it = cache_.rbegin(); + typename std::deque::reverse_iterator rev_it = cache_.rbegin(); // Keep walking backwards along deque until we hit the beginning, - // or until we find a timestamp that's smaller than (or equal to) msg's timestamp + // or until we find a timestamp that's smaller than (or equal to) msg's timestamp rclcpp::Time evt_stamp = mt::TimeStamp::value(*evt.getMessage()); - while(rev_it != cache_.rend() && mt::TimeStamp::value(*(*rev_it).getMessage()) > evt_stamp) + while (rev_it != cache_.rend() && + mt::TimeStamp::value(*(*rev_it).getMessage()) > evt_stamp) + { rev_it++; + } // Add msg to the cache cache_.insert(rev_it.base(), evt); - } this->signalMessage(evt); @@ -158,35 +166,34 @@ class Cache : public SimpleFilter * \param start The start of the requested interval * \param end The end of the requested interval */ - std::vector getInterval(const rclcpp::Time& start, const rclcpp::Time& end) const + std::vector getInterval(const rclcpp::Time & start, const rclcpp::Time & end) const { namespace mt = message_filters::message_traits; std::lock_guard lock(cache_lock_); // Find the starting index. (Find the first index after [or at] the start of the interval) - size_t start_index = 0 ; - while(start_index < cache_.size() && - mt::TimeStamp::value(*cache_[start_index].getMessage()) < start) + size_t start_index = 0; + while (start_index < cache_.size() && + mt::TimeStamp::value(*cache_[start_index].getMessage()) < start) { - start_index++ ; + start_index++; } // Find the ending index. (Find the first index after the end of interval) - size_t end_index = start_index ; - while(end_index < cache_.size() && - mt::TimeStamp::value(*cache_[end_index].getMessage()) <= end) + size_t end_index = start_index; + while (end_index < cache_.size() && + mt::TimeStamp::value(*cache_[end_index].getMessage()) <= end) { - end_index++ ; + end_index++; } - std::vector interval_elems ; - interval_elems.reserve(end_index - start_index) ; - for (size_t i=start_index; i interval_elems; + interval_elems.reserve(end_index - start_index); + for (size_t i = start_index; i < end_index; i++) { + interval_elems.push_back(cache_[i].getMessage()); } - return interval_elems ; + return interval_elems; } @@ -196,31 +203,31 @@ class Cache : public SimpleFilter * If the messages in the cache do not surround (start,end), then this will return the interval * that gets closest to surrounding (start,end) */ - std::vector getSurroundingInterval(const rclcpp::Time& start, const rclcpp::Time& end) const + std::vector getSurroundingInterval( + const rclcpp::Time & start, const rclcpp::Time & end) const { namespace mt = message_filters::message_traits; std::lock_guard lock(cache_lock_); // Find the starting index. (Find the first index after [or at] the start of the interval) int start_index = static_cast(cache_.size()) - 1; - while(start_index > 0 && - mt::TimeStamp::value(*cache_[start_index].getMessage()) > start) + while (start_index > 0 && + mt::TimeStamp::value(*cache_[start_index].getMessage()) > start) { start_index--; } int end_index = start_index; - while(end_index < static_cast(cache_.size()) - 1 && - mt::TimeStamp::value(*cache_[end_index].getMessage()) < end) + while (end_index < static_cast(cache_.size()) - 1 && + mt::TimeStamp::value(*cache_[end_index].getMessage()) < end) { end_index++; } std::vector interval_elems; - interval_elems.reserve(end_index - start_index + 1) ; - for (int i=start_index; i<=end_index; i++) - { - interval_elems.push_back(cache_[i].getMessage()) ; + interval_elems.reserve(end_index - start_index + 1); + for (int i = start_index; i <= end_index; i++) { + interval_elems.push_back(cache_[i].getMessage()); } return interval_elems; @@ -231,27 +238,26 @@ class Cache : public SimpleFilter * \param time Time that must occur right after the returned elem * \returns shared_ptr to the newest elem that occurs before 'time'. NULL if doesn't exist */ - MConstPtr getElemBeforeTime(const rclcpp::Time& time) const + MConstPtr getElemBeforeTime(const rclcpp::Time & time) const { namespace mt = message_filters::message_traits; std::lock_guard lock(cache_lock_); - MConstPtr out ; + MConstPtr out; - unsigned int i=0 ; - int elem_index = -1 ; - while (i::value(*cache_[i].getMessage()) < time) - { - elem_index = i ; - i++ ; + unsigned int i = 0; + int elem_index = -1; + while (i < cache_.size() && mt::TimeStamp::value(*cache_[i].getMessage()) < time) { + elem_index = i; + i++; } - if (elem_index >= 0) - out = cache_[elem_index].getMessage() ; + if (elem_index >= 0) { + out = cache_[elem_index].getMessage(); + } - return out ; + return out; } /** @@ -259,29 +265,28 @@ class Cache : public SimpleFilter * \param time Time that must occur right before the returned elem * \returns shared_ptr to the oldest elem that occurs after 'time'. NULL if doesn't exist */ - MConstPtr getElemAfterTime(const rclcpp::Time& time) const + MConstPtr getElemAfterTime(const rclcpp::Time & time) const { namespace mt = message_filters::message_traits; std::lock_guard lock(cache_lock_); - MConstPtr out ; + MConstPtr out; - int i = static_cast(cache_.size()) - 1 ; - int elem_index = -1 ; - while (i>=0 && - mt::TimeStamp::value(*cache_[i].getMessage()) > time) - { - elem_index = i ; - i-- ; + int i = static_cast(cache_.size()) - 1; + int elem_index = -1; + while (i >= 0 && mt::TimeStamp::value(*cache_[i].getMessage()) > time) { + elem_index = i; + i--; } - if (elem_index >= 0) - out = cache_[elem_index].getMessage() ; - else - out.reset() ; + if (elem_index >= 0) { + out = cache_[elem_index].getMessage(); + } else { + out.reset(); + } - return out ; + return out; } /** @@ -295,10 +300,11 @@ class Cache : public SimpleFilter rclcpp::Time latest_time; - if (cache_.size() > 0) + if (cache_.size() > 0) { latest_time = mt::TimeStamp::value(*cache_.back().getMessage()); + } - return latest_time ; + return latest_time; } /** @@ -312,25 +318,25 @@ class Cache : public SimpleFilter rclcpp::Time oldest_time; - if (cache_.size() > 0) + if (cache_.size() > 0) { oldest_time = mt::TimeStamp::value(*cache_.front().getMessage()); + } - return oldest_time ; + return oldest_time; } - private: - void callback(const EventType& evt) + void callback(const EventType & evt) { add(evt); } - mutable std::mutex cache_lock_ ; //!< Lock for cache_ - std::deque cache_ ; //!< Cache for the messages - unsigned int cache_size_ ; //!< Maximum number of elements allowed in the cache. + mutable std::mutex cache_lock_; //!< Lock for cache_ + std::deque cache_; //!< Cache for the messages + unsigned int cache_size_; //!< Maximum number of elements allowed in the cache. Connection incoming_connection_; }; -} // namespace message_filters; +} // namespace message_filters #endif // MESSAGE_FILTERS__CACHE_H_ diff --git a/include/message_filters/chain.h b/include/message_filters/chain.h index 56f82b7..67e472b 100644 --- a/include/message_filters/chain.h +++ b/include/message_filters/chain.h @@ -26,10 +26,11 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. -#ifndef MESSAGE_FILTERS__CHAIN_H -#define MESSAGE_FILTERS__CHAIN_H +#ifndef MESSAGE_FILTERS__CHAIN_H_ +#define MESSAGE_FILTERS__CHAIN_H_ #include +#include #include "message_filters/simple_filter.h" #include "message_filters/pass_through.h" @@ -56,8 +57,7 @@ class ChainBase std::shared_ptr getFilter(size_t index) const { std::shared_ptr filter = getFilterForIndex(index); - if (filter) - { + if (filter) { return std::static_pointer_cast(filter); } @@ -80,7 +80,7 @@ typedef std::shared_ptr ChainBasePtr; * * Example: \verbatim -void myCallback(const MsgConstPtr& msg) +void myCallback(const MsgConstPtr & msg) { } @@ -95,7 +95,7 @@ c.registerCallback(myCallback); \verbatim Chain c; PassThrough p; -c.addFilter(&p); +c.addFilter( &p); c.registerCallback(myCallback); \endverbatim * @@ -118,14 +118,14 @@ class Chain : public ChainBase, public SimpleFilter * \brief Constructor with filter. Calls connectInput(f) */ template - Chain(F& f) + explicit Chain(F & f) { connectInput(f); } struct NullDeleter { - void operator()(void const*) const + void operator()(void const *) const { } }; @@ -134,7 +134,7 @@ class Chain : public ChainBase, public SimpleFilter * \brief Add a filter to this chain, by bare pointer. Returns the index of that filter in the chain. */ template - size_t addFilter(F* filter) + size_t addFilter(F * filter) { std::shared_ptr ptr(filter, NullDeleter()); return addFilter(ptr); @@ -144,18 +144,20 @@ class Chain : public ChainBase, public SimpleFilter * \brief Add a filter to this chain, by shared_ptr. Returns the index of that filter in the chain */ template - size_t addFilter(const std::shared_ptr& filter) + size_t addFilter(const std::shared_ptr & filter) { FilterInfo info; - info.add_func = std::bind((void(F::*)(const EventType&))&F::add, filter.get(), std::placeholders::_1); + info.add_func = std::bind( + (void (F::*)(const EventType &)) & F::add, filter.get(), std::placeholders::_1); info.filter = filter; - info.passthrough = std::make_shared >(); + info.passthrough = std::make_shared>(); last_filter_connection_.disconnect(); info.passthrough->connectInput(*filter); - last_filter_connection_ = info.passthrough->registerCallback(typename SimpleFilter::EventCallback(std::bind(&Chain::lastFilterCB, this, std::placeholders::_1))); - if (!filters_.empty()) - { + last_filter_connection_ = info.passthrough->registerCallback( + typename SimpleFilter::EventCallback( + std::bind(&Chain::lastFilterCB, this, std::placeholders::_1))); + if (!filters_.empty()) { filter->connectInput(*filters_.back().passthrough); } @@ -174,8 +176,7 @@ class Chain : public ChainBase, public SimpleFilter template std::shared_ptr getFilter(size_t index) const { - if (index >= filters_.size()) - { + if (index >= filters_.size()) { return std::shared_ptr(); } @@ -186,24 +187,25 @@ class Chain : public ChainBase, public SimpleFilter * \brief Connect this filter's input to another filter's output. */ template - void connectInput(F& f) + void connectInput(F & f) { incoming_connection_.disconnect(); - incoming_connection_ = f.registerCallback(typename SimpleFilter::EventCallback(std::bind(&Chain::incomingCB, this, std::placeholders::_1))); + incoming_connection_ = f.registerCallback( + typename SimpleFilter::EventCallback( + std::bind(&Chain::incomingCB, this, std::placeholders::_1))); } /** * \brief Add a message to the start of this chain */ - void add(const MConstPtr& msg) + void add(const MConstPtr & msg) { add(EventType(msg)); } - void add(const EventType& evt) + void add(const EventType & evt) { - if (!filters_.empty()) - { + if (!filters_.empty()) { filters_[0].add_func(evt); } } @@ -211,8 +213,7 @@ class Chain : public ChainBase, public SimpleFilter protected: virtual std::shared_ptr getFilterForIndex(size_t index) const { - if (index >= filters_.size()) - { + if (index >= filters_.size()) { return std::shared_ptr(); } @@ -220,21 +221,21 @@ class Chain : public ChainBase, public SimpleFilter } private: - void incomingCB(const EventType& evt) + void incomingCB(const EventType & evt) { add(evt); } - void lastFilterCB(const EventType& evt) + void lastFilterCB(const EventType & evt) { this->signalMessage(evt); } struct FilterInfo { - std::function add_func; + std::function add_func; std::shared_ptr filter; - std::shared_ptr > passthrough; + std::shared_ptr> passthrough; }; typedef std::vector V_FilterInfo; diff --git a/include/message_filters/connection.h b/include/message_filters/connection.h index 9c0dc9b..0c47a12 100644 --- a/include/message_filters/connection.h +++ b/include/message_filters/connection.h @@ -42,8 +42,8 @@ class noncopyable protected: noncopyable() {} ~noncopyable() {} - noncopyable( const noncopyable& ) = delete; - noncopyable& operator=( const noncopyable& ) = delete; + noncopyable(const noncopyable &) = delete; + noncopyable & operator=(const noncopyable &) = delete; }; /** @@ -52,10 +52,10 @@ class noncopyable class Connection { public: - using VoidDisconnectFunction = std::function; - using WithConnectionDisconnectFunction = std::function; + using VoidDisconnectFunction = std::function; + using WithConnectionDisconnectFunction = std::function; MESSAGE_FILTERS_PUBLIC Connection() {} - MESSAGE_FILTERS_PUBLIC Connection(const VoidDisconnectFunction& func); + MESSAGE_FILTERS_PUBLIC Connection(const VoidDisconnectFunction & func); /** * \brief disconnects this connection diff --git a/include/message_filters/message_event.h b/include/message_filters/message_event.h index a322044..4e8cf46 100644 --- a/include/message_filters/message_event.h +++ b/include/message_filters/message_event.h @@ -47,8 +47,8 @@ namespace message_filters { -typedef std::map< std::string, std::string > M_string; -typedef std::shared_ptr< M_string > M_stringPtr; +typedef std::map M_string; +typedef std::shared_ptr M_stringPtr; template struct DefaultMessageCreator @@ -67,7 +67,7 @@ ROS_DEPRECATED inline std::shared_ptr defaultMessageCreateFunction() } */ /** - * \brief Event type for subscriptions, const message_filters::MessageEvent& can be used in your callback instead of const std::shared_ptr& + * \brief Event type for subscriptions, const message_filters::MessageEvent & can be used in your callback instead of const std::shared_ptr& * * Useful if you need to retrieve meta-data about the message, such as the full connection header, or the publisher's node name */ @@ -85,52 +85,59 @@ class MessageEvent : nonconst_need_copy_(true) {} - MessageEvent(const MessageEvent& rhs) + MessageEvent(const MessageEvent & rhs) { *this = rhs; } - MessageEvent(const MessageEvent& rhs) + MessageEvent(const MessageEvent & rhs) { *this = rhs; } - MessageEvent(const MessageEvent& rhs, bool nonconst_need_copy) + MessageEvent(const MessageEvent & rhs, bool nonconst_need_copy) { *this = rhs; nonconst_need_copy_ = nonconst_need_copy; } - MessageEvent(const MessageEvent& rhs, bool nonconst_need_copy) + MessageEvent(const MessageEvent & rhs, bool nonconst_need_copy) { *this = rhs; nonconst_need_copy_ = nonconst_need_copy; } - MessageEvent(const MessageEvent& rhs, const CreateFunction& create) + MessageEvent(const MessageEvent & rhs, const CreateFunction & create) { - init(std::const_pointer_cast(std::static_pointer_cast(rhs.getMessage())), rhs.getReceiptTime(), rhs.nonConstWillCopy(), create); + init( + std::const_pointer_cast( + std::static_pointer_cast( + rhs.getMessage())), rhs.getReceiptTime(), rhs.nonConstWillCopy(), create); } /** * \todo Make this explicit in ROS 2.0. Keep as auto-converting for now to maintain backwards compatibility in some places (message_filters) */ - MessageEvent(const ConstMessagePtr& message) + MessageEvent(const ConstMessagePtr & message) { init(message, rclcpp::Clock().now(), true, message_filters::DefaultMessageCreator()); } - MessageEvent(const ConstMessagePtr& message, rclcpp::Time receipt_time) + MessageEvent(const ConstMessagePtr & message, rclcpp::Time receipt_time) { init(message, receipt_time, true, message_filters::DefaultMessageCreator()); } - MessageEvent(const ConstMessagePtr& message, rclcpp::Time receipt_time, bool nonconst_need_copy, const CreateFunction& create) + MessageEvent( + const ConstMessagePtr & message, rclcpp::Time receipt_time, bool nonconst_need_copy, + const CreateFunction & create) { init(message, receipt_time, nonconst_need_copy, create); } - void init(const ConstMessagePtr& message, rclcpp::Time receipt_time, bool nonconst_need_copy, const CreateFunction& create) + void init( + const ConstMessagePtr & message, rclcpp::Time receipt_time, bool nonconst_need_copy, + const CreateFunction & create) { message_ = message; receipt_time_ = receipt_time; @@ -138,15 +145,21 @@ class MessageEvent create_ = create; } - void operator=(const MessageEvent& rhs) + void operator=(const MessageEvent & rhs) { - init(std::static_pointer_cast(rhs.getMessage()), rhs.getReceiptTime(), rhs.nonConstWillCopy(), rhs.getMessageFactory()); + init( + std::static_pointer_cast(rhs.getMessage()), + rhs.getReceiptTime(), rhs.nonConstWillCopy(), rhs.getMessageFactory()); message_copy_.reset(); } - void operator=(const MessageEvent& rhs) + void operator=(const MessageEvent & rhs) { - init(std::const_pointer_cast(std::static_pointer_cast(rhs.getMessage())), rhs.getReceiptTime(), rhs.nonConstWillCopy(), rhs.getMessageFactory()); + init( + std::const_pointer_cast( + std::static_pointer_cast( + rhs.getMessage())), rhs.getReceiptTime(), rhs.nonConstWillCopy(), + rhs.getMessageFactory()); message_copy_.reset(); } @@ -163,54 +176,52 @@ class MessageEvent /** * \brief Retrieve a const version of the message */ - const std::shared_ptr& getConstMessage() const { return message_; } + const std::shared_ptr & getConstMessage() const {return message_;} /** * \brief Returns the time at which this message was received */ - rclcpp::Time getReceiptTime() const { return receipt_time_; } + rclcpp::Time getReceiptTime() const {return receipt_time_;} - bool nonConstWillCopy() const { return nonconst_need_copy_; } - bool getMessageWillCopy() const { return !std::is_const::value && nonconst_need_copy_; } + bool nonConstWillCopy() const {return nonconst_need_copy_;} + bool getMessageWillCopy() const {return !std::is_const::value && nonconst_need_copy_;} - bool operator<(const MessageEvent& rhs) + bool operator<(const MessageEvent & rhs) { - if (message_ != rhs.message_) - { + if (message_ != rhs.message_) { return message_ < rhs.message_; } - if (receipt_time_ != rhs.receipt_time_) - { + if (receipt_time_ != rhs.receipt_time_) { return receipt_time_ < rhs.receipt_time_; } return nonconst_need_copy_ < rhs.nonconst_need_copy_; } - bool operator==(const MessageEvent& rhs) + bool operator==(const MessageEvent & rhs) { - return message_ == rhs.message_ && receipt_time_ == rhs.receipt_time_ && nonconst_need_copy_ == rhs.nonconst_need_copy_; + return message_ == rhs.message_ && receipt_time_ == rhs.receipt_time_ && + nonconst_need_copy_ == rhs.nonconst_need_copy_; } - bool operator!=(const MessageEvent& rhs) + bool operator!=(const MessageEvent & rhs) { return !(*this == rhs); } - const CreateFunction& getMessageFactory() const { return create_; } + const CreateFunction & getMessageFactory() const {return create_;} private: template - typename std::enable_if::value, std::shared_ptr >::type copyMessageIfNecessary() const + typename std::enable_if::value, + std::shared_ptr>::type copyMessageIfNecessary() const { - if (std::is_const::value || !nonconst_need_copy_) - { + if (std::is_const::value || !nonconst_need_copy_) { return std::const_pointer_cast(message_); } - if (message_copy_) - { + if (message_copy_) { return message_copy_; } @@ -222,7 +233,8 @@ class MessageEvent } template - typename std::enable_if::value, std::shared_ptr >::type copyMessageIfNecessary() const + typename std::enable_if::value, + std::shared_ptr>::type copyMessageIfNecessary() const { return std::const_pointer_cast(message_); } @@ -237,7 +249,8 @@ class MessageEvent static const std::string s_unknown_publisher_string_; }; -template const std::string MessageEvent::s_unknown_publisher_string_("unknown_publisher"); +template const std::string MessageEvent::s_unknown_publisher_string_( + "unknown_publisher"); } // namespace message_filters diff --git a/include/message_filters/message_traits.h b/include/message_filters/message_traits.h index 4d0ae16..44d2a8c 100644 --- a/include/message_filters/message_traits.h +++ b/include/message_filters/message_traits.h @@ -53,7 +53,7 @@ struct HasHeader : public std::false_type {}; * True if the message has a field named 'header' with a type of std_msgs::msg::Header * @tparam M */ -template +template struct HasHeader::value>::type>: public std::true_type {}; @@ -65,15 +65,15 @@ struct HasHeader struct FrameId { - static std::string* pointer(M& m) { (void)m; return nullptr; } - static std::string const* pointer(const M& m) { (void)m; return nullptr; } + static std::string * pointer(M & m) {(void)m; return nullptr;} + static std::string const * pointer(const M & m) {(void)m; return nullptr;} }; template -struct FrameId::value>::type > +struct FrameId::value>::type> { - static std::string* pointer(M& m) { return &m.header.frame_id; } - static std::string const* pointer(const M& m) { return &m.header.frame_id; } - static std::string value(const M& m) { return m.header.frame_id; } + static std::string * pointer(M & m) {return &m.header.frame_id;} + static std::string const * pointer(const M & m) {return &m.header.frame_id;} + static std::string value(const M & m) {return m.header.frame_id;} }; /** @@ -84,16 +84,18 @@ struct FrameId::value>::type > template struct TimeStamp { - static rclcpp::Time value(const M& m) { + static rclcpp::Time value(const M & m) + { (void)m; return rclcpp::Time(); } }; template -struct TimeStamp::value>::type > +struct TimeStamp::value>::type> { - static rclcpp::Time value(const M& m) { + static rclcpp::Time value(const M & m) + { return rclcpp::Time(m.header.stamp, RCL_ROS_TIME); } }; @@ -102,4 +104,3 @@ struct TimeStamp::value>::type > } // namespace message_filters #endif // MESSAGE_FILTERS__MESSAGE_TRAITS_H_ - diff --git a/include/message_filters/null_types.h b/include/message_filters/null_types.h index 1f2ac42..1501bc2 100644 --- a/include/message_filters/null_types.h +++ b/include/message_filters/null_types.h @@ -48,13 +48,13 @@ template struct NullFilter { template - Connection registerCallback(const C&) + Connection registerCallback(const C &) { return Connection(); } template - Connection registerCallback(const std::function&) + Connection registerCallback(const std::function &) { return Connection(); } @@ -65,7 +65,7 @@ namespace message_traits template<> struct TimeStamp { - static rclcpp::Time value(const message_filters::NullType&) + static rclcpp::Time value(const message_filters::NullType &) { return rclcpp::Time(); } diff --git a/include/message_filters/parameter_adapter.h b/include/message_filters/parameter_adapter.h index ba4f48f..4f882e7 100644 --- a/include/message_filters/parameter_adapter.h +++ b/include/message_filters/parameter_adapter.h @@ -56,14 +56,14 @@ namespace message_filters * * ParameterAdapter is specialized to allow callbacks of any of the forms: \verbatim -void callback(const std::shared_ptr&); -void callback(const std::shared_ptr&); +void callback(const std::shared_ptr &); +void callback(const std::shared_ptr &); void callback(std::shared_ptr); void callback(std::shared_ptr); -void callback(const M&); +void callback(const M &); void callback(M); -void callback(const MessageEvent&); -void callback(const MessageEvent&); +void callback(const MessageEvent &); +void callback(const MessageEvent &); \endverbatim */ template @@ -74,105 +74,105 @@ struct ParameterAdapter typedef M Parameter; static const bool is_const = true; - static Parameter getParameter(const Event& event) + static Parameter getParameter(const Event & event) { return *event.getMessage(); } }; -//struct message_filters::ParameterAdapter&> +//struct message_filters::ParameterAdapter &> template -struct ParameterAdapter& > +struct ParameterAdapter &> { typedef typename std::remove_reference::type>::type Message; typedef MessageEvent Event; typedef const std::shared_ptr Parameter; static const bool is_const = true; - static Parameter getParameter(const Event& event) + static Parameter getParameter(const Event & event) { return event.getMessage(); } }; template -struct ParameterAdapter& > +struct ParameterAdapter &> { typedef typename std::remove_reference::type>::type Message; typedef MessageEvent Event; typedef std::shared_ptr Parameter; static const bool is_const = false; - static Parameter getParameter(const Event& event) + static Parameter getParameter(const Event & event) { return MessageEvent(event).getMessage(); } }; template -struct ParameterAdapter +struct ParameterAdapter { typedef typename std::remove_reference::type>::type Message; typedef MessageEvent Event; - typedef const M& Parameter; + typedef const M & Parameter; static const bool is_const = true; - static Parameter getParameter(const Event& event) + static Parameter getParameter(const Event & event) { return *event.getMessage(); } }; template -struct ParameterAdapter > +struct ParameterAdapter> { typedef typename std::remove_reference::type>::type Message; typedef MessageEvent Event; typedef std::shared_ptr Parameter; static const bool is_const = true; - static Parameter getParameter(const Event& event) + static Parameter getParameter(const Event & event) { return event.getMessage(); } }; template -struct ParameterAdapter > +struct ParameterAdapter> { typedef typename std::remove_reference::type>::type Message; typedef MessageEvent Event; typedef std::shared_ptr Parameter; static const bool is_const = false; - static Parameter getParameter(const Event& event) + static Parameter getParameter(const Event & event) { return MessageEvent(event).getMessage(); } }; template -struct ParameterAdapter& > +struct ParameterAdapter &> { typedef typename std::remove_reference::type>::type Message; typedef MessageEvent Event; - typedef const MessageEvent& Parameter; + typedef const MessageEvent & Parameter; static const bool is_const = true; - static Parameter getParameter(const Event& event) + static Parameter getParameter(const Event & event) { return event; } }; template -struct ParameterAdapter& > +struct ParameterAdapter &> { typedef typename std::remove_reference::type>::type Message; typedef MessageEvent Event; typedef MessageEvent Parameter; static const bool is_const = false; - static Parameter getParameter(const Event& event) + static Parameter getParameter(const Event & event) { return MessageEvent(event); } diff --git a/include/message_filters/pass_through.h b/include/message_filters/pass_through.h index a3da5ac..5c64f0b 100644 --- a/include/message_filters/pass_through.h +++ b/include/message_filters/pass_through.h @@ -29,6 +29,7 @@ #ifndef MESSAGE_FILTERS__PASS_THROUGH_H_ #define MESSAGE_FILTERS__PASS_THROUGH_H_ +#include #include #include "message_filters/simple_filter.h" @@ -50,30 +51,35 @@ class PassThrough : public SimpleFilter } template - PassThrough(F& f) + PassThrough(F & f) { connectInput(f); } template - void connectInput(F& f) + void connectInput(F & f) { incoming_connection_.disconnect(); - incoming_connection_ = f.registerCallback(typename SimpleFilter::EventCallback(std::bind(&PassThrough::cb, this, std::placeholders::_1))); + incoming_connection_ = + f.registerCallback( + typename SimpleFilter::EventCallback( + std::bind( + &PassThrough::cb, this, + std::placeholders::_1))); } - void add(const MConstPtr& msg) + void add(const MConstPtr & msg) { add(EventType(msg)); } - void add(const EventType& evt) + void add(const EventType & evt) { this->signalMessage(evt); } private: - void cb(const EventType& evt) + void cb(const EventType & evt) { add(evt); } diff --git a/include/message_filters/signal1.h b/include/message_filters/signal1.h index 8bdd390..4b5c27a 100644 --- a/include/message_filters/signal1.h +++ b/include/message_filters/signal1.h @@ -29,6 +29,7 @@ #ifndef MESSAGE_FILTERS__SIGNAL1_H_ #define MESSAGE_FILTERS__SIGNAL1_H_ +#include #include #include @@ -44,9 +45,9 @@ class CallbackHelper1 public: virtual ~CallbackHelper1() {} - virtual void call(const MessageEvent& event, bool nonconst_need_copy) = 0; + virtual void call(const MessageEvent & event, bool nonconst_need_copy) = 0; - typedef std::shared_ptr > Ptr; + typedef std::shared_ptr> Ptr; }; template @@ -54,15 +55,15 @@ class CallbackHelper1T : public CallbackHelper1 { public: typedef ParameterAdapter

Adapter; - typedef std::function Callback; + typedef std::function Callback; typedef typename Adapter::Event Event; - CallbackHelper1T(const Callback& cb) + CallbackHelper1T(const Callback & cb) : callback_(cb) { } - virtual void call(const MessageEvent& event, bool nonconst_force_copy) + virtual void call(const MessageEvent & event, bool nonconst_force_copy) { Event my_event(event, nonconst_force_copy || event.nonConstWillCopy()); callback_(Adapter::getParameter(my_event)); @@ -75,39 +76,38 @@ class CallbackHelper1T : public CallbackHelper1 template class Signal1 { - typedef std::shared_ptr > CallbackHelper1Ptr; + typedef std::shared_ptr> CallbackHelper1Ptr; typedef std::vector V_CallbackHelper1; public: template - CallbackHelper1Ptr addCallback(const std::function& callback) + CallbackHelper1Ptr addCallback(const std::function & callback) { - CallbackHelper1T* helper = new CallbackHelper1T(callback); + CallbackHelper1T * helper = new CallbackHelper1T(callback); std::lock_guard lock(mutex_); callbacks_.push_back(CallbackHelper1Ptr(helper)); return callbacks_.back(); } - void removeCallback(const CallbackHelper1Ptr& helper) + void removeCallback(const CallbackHelper1Ptr & helper) { std::lock_guard lock(mutex_); - typename V_CallbackHelper1::iterator it = std::find(callbacks_.begin(), callbacks_.end(), helper); - if (it != callbacks_.end()) - { + typename V_CallbackHelper1::iterator it = + std::find(callbacks_.begin(), callbacks_.end(), helper); + if (it != callbacks_.end()) { callbacks_.erase(it); } } - void call(const MessageEvent& event) + void call(const MessageEvent & event) { std::lock_guard lock(mutex_); bool nonconst_force_copy = callbacks_.size() > 1; typename V_CallbackHelper1::iterator it = callbacks_.begin(); typename V_CallbackHelper1::iterator end = callbacks_.end(); - for (; it != end; ++it) - { - const CallbackHelper1Ptr& helper = *it; + for (; it != end; ++it) { + const CallbackHelper1Ptr & helper = *it; helper->call(event, nonconst_force_copy); } } diff --git a/include/message_filters/signal9.h b/include/message_filters/signal9.h index ea8393f..d63539a 100644 --- a/include/message_filters/signal9.h +++ b/include/message_filters/signal9.h @@ -32,6 +32,8 @@ #include #include +#include +#include #include "message_filters/connection.h" #include "message_filters/null_types.h" @@ -41,7 +43,8 @@ namespace message_filters { -template +template class CallbackHelper9 { public: @@ -57,23 +60,26 @@ class CallbackHelper9 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 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; typedef std::shared_ptr Ptr; }; -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> +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> { private: typedef ParameterAdapter A0; @@ -96,17 +102,20 @@ class CallbackHelper9T : typedef typename A8::Event M8Event; public: - typedef std::function Callback; + typedef std::function Callback; - CallbackHelper9T(const Callback& cb) + explicit CallbackHelper9T(const Callback & cb) : 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 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) { M0Event my_e0(e0, nonconst_force_copy || e0.nonConstWillCopy()); M1Event my_e1(e1, nonconst_force_copy || e0.nonConstWillCopy()); @@ -117,25 +126,27 @@ class CallbackHelper9T : 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)); + 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)); } private: Callback callback_; }; -template +template class Signal9 { - typedef std::shared_ptr > CallbackHelper9Ptr; + typedef std::shared_ptr> CallbackHelper9Ptr; typedef std::vector V_CallbackHelper9; public: @@ -157,13 +168,15 @@ class Signal9 typedef std::shared_ptr M6ConstPtr; typedef std::shared_ptr M7ConstPtr; typedef std::shared_ptr M8ConstPtr; - typedef const std::shared_ptr& NullP; + 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)); @@ -171,129 +184,207 @@ class Signal9 } template - Connection addCallback(void(*callback)(P0, P1)) + Connection addCallback(void (* callback)(P0, P1)) { - return addCallback(std::function(std::bind(callback, std::placeholders::_1, std::placeholders::_2))); + return addCallback( + std::function( + std::bind( + callback, std::placeholders::_1, std::placeholders::_2))); } template - Connection addCallback(void(*callback)(P0, P1, P2)) + Connection addCallback(void (* callback)(P0, P1, P2)) { - return addCallback(std::function(std::bind(callback, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3))); + return addCallback( + std::function( + std::bind( + callback, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3))); } template - Connection addCallback(void(*callback)(P0, P1, P2, P3)) + Connection addCallback(void (* callback)(P0, P1, P2, P3)) { - return addCallback(std::function(std::bind(callback, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4))); + 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)) + 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))); + 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)) + 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))); + 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)) + 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))); + 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)) + 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))); + 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)) + 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))); + 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) + Connection addCallback(void (T::* callback)(P0, P1), T * t) { - return addCallback(std::function(std::bind(callback, t, std::placeholders::_1, std::placeholders::_2))); + 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) + 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))); + 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) + 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))); + 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) + 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))); + 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) + 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))); + 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) + 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))); + 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) + 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))); + 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))); } template - Connection addCallback( C& callback) + 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::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)); } - void removeCallback(const CallbackHelper9Ptr& helper) + void removeCallback(const CallbackHelper9Ptr & helper) { std::lock_guard lock(mutex_); - typename V_CallbackHelper9::iterator it = std::find(callbacks_.begin(), callbacks_.end(), helper); - if (it != callbacks_.end()) - { + 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 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) { std::lock_guard lock(mutex_); bool nonconst_force_copy = callbacks_.size() > 1; typename V_CallbackHelper9::iterator it = callbacks_.begin(); typename V_CallbackHelper9::iterator end = callbacks_.end(); - for (; it != end; ++it) - { - const CallbackHelper9Ptr& helper = *it; + for (; it != end; ++it) { + const CallbackHelper9Ptr & helper = *it; helper->call(nonconst_force_copy, e0, e1, e2, e3, e4, e5, e6, e7, e8); } } @@ -305,4 +396,4 @@ class Signal9 } // namespace message_filters -#endif // MESSAGE_FILTERS__SIGNAL9_H_ +#endif // MESSAGE_FILTERS__SIGNAL9_H_ diff --git a/include/message_filters/simple_filter.h b/include/message_filters/simple_filter.h index c08cacd..4c6251f 100644 --- a/include/message_filters/simple_filter.h +++ b/include/message_filters/simple_filter.h @@ -52,16 +52,16 @@ class SimpleFilter : public noncopyable { public: typedef std::shared_ptr MConstPtr; - typedef std::function Callback; + typedef std::function Callback; typedef MessageEvent EventType; - typedef std::function EventCallback; + typedef std::function EventCallback; /** * \brief Register a callback to be called when this filter has passed * \param callback The callback to call */ template - Connection registerCallback(const C& callback) + Connection registerCallback(const C & callback) { typename CallbackHelper1::Ptr helper = signal_.addCallback(Callback(callback)); return Connection(std::bind(&Signal::removeCallback, &signal_, helper)); @@ -72,7 +72,7 @@ class SimpleFilter : public noncopyable * \param callback The callback to call */ template - Connection registerCallback(const std::function& callback) + Connection registerCallback(const std::function & callback) { return Connection(std::bind(&Signal::removeCallback, &signal_, signal_.addCallback(callback))); } @@ -82,9 +82,10 @@ class SimpleFilter : public noncopyable * \param callback The callback to call */ template - Connection registerCallback(void(*callback)(P)) + Connection registerCallback(void (* callback)(P)) { - typename CallbackHelper1::Ptr helper = signal_.template addCallback

(std::bind(callback, std::placeholders::_1)); + typename CallbackHelper1::Ptr helper = + signal_.template addCallback

(std::bind(callback, std::placeholders::_1)); return Connection(std::bind(&Signal::removeCallback, &signal_, helper)); } @@ -93,26 +94,27 @@ class SimpleFilter : public noncopyable * \param callback The callback to call */ template - Connection registerCallback(void(T::*callback)(P), T* t) + Connection registerCallback(void (T::* callback)(P), T * t) { - typename CallbackHelper1::Ptr helper = signal_.template addCallback

(std::bind(callback, t, std::placeholders::_1)); + typename CallbackHelper1::Ptr helper = + signal_.template addCallback

(std::bind(callback, t, std::placeholders::_1)); return Connection(std::bind(&Signal::removeCallback, &signal_, helper)); } /** * \brief Set the name of this filter. For debugging use. */ - void setName(const std::string& name) { name_ = name; } + void setName(const std::string & name) {name_ = name;} /** * \brief Get the name of this filter. For debugging use. */ - const std::string& getName() { return name_; } + const std::string & getName() {return name_;} protected: /** * \brief Call all registered callbacks, passing them the specified message */ - void signalMessage(const MConstPtr& msg) + void signalMessage(const MConstPtr & msg) { MessageEvent event(msg); @@ -122,7 +124,7 @@ class SimpleFilter : public noncopyable /** * \brief Call all registered callbacks, passing them the specified message */ - void signalMessage(const MessageEvent& event) + void signalMessage(const MessageEvent & event) { signal_.call(event); } diff --git a/include/message_filters/subscriber.h b/include/message_filters/subscriber.h index c698365..6e05c2d 100644 --- a/include/message_filters/subscriber.h +++ b/include/message_filters/subscriber.h @@ -29,7 +29,9 @@ #ifndef MESSAGE_FILTERS__SUBSCRIBER_H_ #define MESSAGE_FILTERS__SUBSCRIBER_H_ +#include #include +#include #include #include @@ -56,7 +58,9 @@ class SubscriberBase * \param topic The topic to subscribe to. * \param qos (optional) The rmw qos profile to use to subscribe */ - virtual void subscribe(NodePtr node, const std::string& topic, const rmw_qos_profile_t qos = rmw_qos_profile_default) = 0; + virtual void subscribe( + NodePtr node, const std::string & topic, + const rmw_qos_profile_t qos = rmw_qos_profile_default) = 0; /** * \brief Subscribe to a topic. @@ -67,7 +71,9 @@ class SubscriberBase * \param topic The topic to subscribe to. * \param qos (optional) The rmw qos profile to use to subscribe */ - virtual void subscribe(NodeType * node, const std::string& topic, const rmw_qos_profile_t qos = rmw_qos_profile_default) = 0; + virtual void subscribe( + NodeType * node, const std::string & topic, + const rmw_qos_profile_t qos = rmw_qos_profile_default) = 0; /** * \brief Subscribe to a topic. @@ -82,12 +88,12 @@ class SubscriberBase */ virtual void subscribe( NodePtr node, - const std::string& topic, + const std::string & topic, const rmw_qos_profile_t qos, rclcpp::SubscriptionOptions options) { this->subscribe(node.get(), topic, qos, options); - }; + } /** * \brief Subscribe to a topic. @@ -101,7 +107,7 @@ class SubscriberBase */ virtual void subscribe( NodeType * node, - const std::string& topic, + const std::string & topic, const rmw_qos_profile_t qos, rclcpp::SubscriptionOptions options) = 0; @@ -114,7 +120,7 @@ class SubscriberBase */ virtual void unsubscribe() = 0; }; -template +template using SubscriberBasePtr = std::shared_ptr>; /** @@ -133,32 +139,32 @@ using SubscriberBasePtr = std::shared_ptr>; * * The output connection for the Subscriber object is the same signature as for rclcpp subscription callbacks, ie. \verbatim -void callback(const std::shared_ptr&); +void callback(const std::shared_ptr &); \endverbatim */ -template ::value> +template::value> struct message_type; -template -struct message_type +template +struct message_type { using type = typename M::custom_type; }; -template -struct message_type +template +struct message_type { using type = M; }; -template +template using message_type_t = typename message_type::type; template -class Subscriber -: public SubscriberBase -, public SimpleFilter> +class Subscriber + : public SubscriberBase, + public SimpleFilter> { public: typedef std::shared_ptr NodePtr; @@ -174,12 +180,16 @@ class Subscriber * \param topic The topic to subscribe to. * \param qos (optional) The rmw qos profile to use to subscribe */ - Subscriber(NodePtr node, const std::string& topic, const rmw_qos_profile_t qos = rmw_qos_profile_default) + Subscriber( + NodePtr node, const std::string & topic, + const rmw_qos_profile_t qos = rmw_qos_profile_default) { subscribe(node, topic, qos); } - Subscriber(NodeType * node, const std::string& topic, const rmw_qos_profile_t qos = rmw_qos_profile_default) + Subscriber( + NodeType * node, const std::string & topic, + const rmw_qos_profile_t qos = rmw_qos_profile_default) { subscribe(node, topic, qos); } @@ -196,16 +206,16 @@ class Subscriber */ Subscriber( NodePtr node, - const std::string& topic, + const std::string & topic, const rmw_qos_profile_t qos, rclcpp::SubscriptionOptions options) { - subscribe(node.get(), topic, qos, options); + subscribe(node.get(), topic, qos, options); } Subscriber( NodeType * node, - const std::string& topic, + const std::string & topic, const rmw_qos_profile_t qos, rclcpp::SubscriptionOptions options) { @@ -231,7 +241,9 @@ class Subscriber * \param topic The topic to subscribe to. * \param qos (optional) The rmw qos profile to use to subscribe */ - void subscribe(NodePtr node, const std::string& topic, const rmw_qos_profile_t qos = rmw_qos_profile_default) override + void subscribe( + NodePtr node, const std::string & topic, + const rmw_qos_profile_t qos = rmw_qos_profile_default) override { subscribe(node.get(), topic, qos, rclcpp::SubscriptionOptions()); } @@ -246,7 +258,9 @@ class Subscriber * \param qos (optional) The rmw qos profile to use to subscribe */ // TODO(wjwwood): deprecate in favor of API's that use `rclcpp::QoS` instead. - void subscribe(NodeType * node, const std::string& topic, const rmw_qos_profile_t qos = rmw_qos_profile_default) override + void subscribe( + NodeType * node, const std::string & topic, + const rmw_qos_profile_t qos = rmw_qos_profile_default) override { subscribe(node, topic, qos, rclcpp::SubscriptionOptions()); } @@ -263,7 +277,7 @@ class Subscriber */ void subscribe( NodePtr node, - const std::string& topic, + const std::string & topic, const rmw_qos_profile_t qos, rclcpp::SubscriptionOptions options) override { @@ -285,23 +299,23 @@ class Subscriber // TODO(wjwwood): deprecate in favor of API's that use `rclcpp::QoS` instead. void subscribe( NodeType * node, - const std::string& topic, + const std::string & topic, const rmw_qos_profile_t qos, rclcpp::SubscriptionOptions options) override { unsubscribe(); - if (!topic.empty()) - { + if (!topic.empty()) { topic_ = topic; rclcpp::QoS rclcpp_qos(rclcpp::QoSInitialization::from_rmw(qos)); rclcpp_qos.get_rmw_qos_profile() = qos; qos_ = qos; options_ = options; - sub_ = node->template create_subscription(topic, rclcpp_qos, - [this](const std::shared_ptr msg) { - this->cb(EventType(msg)); - }, options); + sub_ = node->template create_subscription( + topic, rclcpp_qos, + [this](const std::shared_ptr msg) { + this->cb(EventType(msg)); + }, options); node_raw_ = node; } @@ -312,8 +326,7 @@ class Subscriber */ void subscribe() override { - if (!topic_.empty()) - { + if (!topic_.empty()) { if (node_raw_ != nullptr) { subscribe(node_raw_, topic_, qos_, options_); } else if (node_shared_ != nullptr) { @@ -338,13 +351,13 @@ class Subscriber /** * \brief Returns the internal rclcpp::Subscription::SharedPtr object */ - const typename rclcpp::Subscription::SharedPtr getSubscriber() const { return sub_; } + const typename rclcpp::Subscription::SharedPtr getSubscriber() const {return sub_;} /** * \brief Does nothing. Provided so that Subscriber may be used in a message_filters::Chain */ template - void connectInput(F& f) + void connectInput(F & f) { (void)f; } @@ -352,14 +365,13 @@ class Subscriber /** * \brief Does nothing. Provided so that Subscriber may be used in a message_filters::Chain */ - void add(const EventType& e) + void add(const EventType & e) { (void)e; } private: - - void cb(const EventType& e) + void cb(const EventType & e) { this->signalMessage(e); } diff --git a/include/message_filters/sync_policies/approximate_epsilon_time.h b/include/message_filters/sync_policies/approximate_epsilon_time.h index f6017b1..b377123 100644 --- a/include/message_filters/sync_policies/approximate_epsilon_time.h +++ b/include/message_filters/sync_policies/approximate_epsilon_time.h @@ -29,9 +29,14 @@ #ifndef MESSAGE_FILTERS__SYNC_POLICIES__APPROXIMATE_EPSILON_TIME_H_ #define MESSAGE_FILTERS__SYNC_POLICIES__APPROXIMATE_EPSILON_TIME_H_ +#include +#include #include +#include #include #include +#include +#include #include @@ -46,8 +51,9 @@ namespace message_filters namespace sync_policies { -template +template class ApproximateEpsilonTime : public PolicyBase { public: @@ -70,18 +76,18 @@ class ApproximateEpsilonTime : public PolicyBase - void add(const typename std::tuple_element::type& evt) + void add(const typename std::tuple_element::type & evt) { RCUTILS_ASSERT(parent_); @@ -118,7 +124,7 @@ class ApproximateEpsilonTime : public PolicyBase; - template + template TimeIndexPair get_older_timestamp_between(const TimeIndexPair & current) { @@ -132,19 +138,21 @@ class ApproximateEpsilonTime : public PolicyBase::value(*events_of_this_type.at(0).getMessage()); + auto candidate = mt::TimeStamp::value( + *events_of_this_type.at(0).getMessage()); if (current.first > candidate) { return std::make_pair(candidate, Is); } return current; } - template + template TimeIndexPair get_older_timestamp_helper(std::index_sequence const &) { TimeIndexPair older{ - rclcpp::Time(std::numeric_limits::max(), RCL_ROS_TIME), std::numeric_limits::max()}; + rclcpp::Time(std::numeric_limits::max(), RCL_ROS_TIME), + std::numeric_limits::max()}; ((older = get_older_timestamp_between(older)), ...); return older; } @@ -155,7 +163,7 @@ class ApproximateEpsilonTime : public PolicyBase()); } - template + template bool check_timestamp_within_epsilon(const TimeIndexPair & older) { @@ -172,14 +180,15 @@ class ApproximateEpsilonTime : public PolicyBase::value(*events_of_this_type.at(0).getMessage()); + auto ts = mt::TimeStamp::value( + *events_of_this_type.at(0).getMessage()); if (older.first + epsilon_ >= ts) { return true; } return false; } - template + template bool check_all_timestamp_within_epsilon_helper( const TimeIndexPair & older, std::index_sequence const &) @@ -212,7 +221,7 @@ class ApproximateEpsilonTime : public PolicyBase + template void erase_beginning_of_vectors_helper(std::index_sequence const &) { @@ -248,9 +257,10 @@ class ApproximateEpsilonTime : public PolicyBase + template void - erase_old_events_if_on_sync_with_ts_helper(rclcpp::Time timestamp, std::index_sequence const &) + erase_old_events_if_on_sync_with_ts_helper( + rclcpp::Time timestamp, std::index_sequence const &) { ((erase_beginning_of_vector_if_on_sync_with_ts(timestamp)), ...); } @@ -321,7 +331,7 @@ class ApproximateEpsilonTime : public PolicyBase +template struct ApproximateTime : public PolicyBase { typedef Synchronizer Sync; @@ -104,30 +105,32 @@ struct ApproximateTime : public PolicyBase 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; ApproximateTime(uint32_t queue_size) : parent_(0) - , queue_size_(queue_size) - , num_non_empty_deques_(0) - , pivot_(NO_PIVOT) - , max_interval_duration_(rclcpp::Duration(std::numeric_limits::max(),999999999)) - , age_penalty_(0.1) - , has_dropped_messages_(9, false) - , inter_message_lower_bounds_(9, rclcpp::Duration(0, 0)) - , warned_about_incorrect_bound_(9, false) + , queue_size_(queue_size) + , num_non_empty_deques_(0) + , pivot_(NO_PIVOT) + , max_interval_duration_(rclcpp::Duration(std::numeric_limits::max(), 999999999)) + , age_penalty_(0.1) + , has_dropped_messages_(9, false) + , inter_message_lower_bounds_(9, rclcpp::Duration(0, 0)) + , warned_about_incorrect_bound_(9, false) { RCUTILS_ASSERT(queue_size_ > 0); // The synchronizer will tend to drop many messages with a queue size of 1. At least 2 is recommended. } - ApproximateTime(const ApproximateTime& e) - : max_interval_duration_(rclcpp::Duration(std::numeric_limits::max(),999999999)) + ApproximateTime(const ApproximateTime & e) + : max_interval_duration_(rclcpp::Duration(std::numeric_limits::max(), 999999999)) { *this = e; } - ApproximateTime& operator=(const ApproximateTime& rhs) + ApproximateTime & operator=(const ApproximateTime & rhs) { parent_ = rhs.parent_; queue_size_ = rhs.queue_size_; @@ -147,7 +150,7 @@ struct ApproximateTime : public PolicyBase return *this; } - void initParent(Sync* parent) + void initParent(Sync * parent) { parent_ = parent; } @@ -156,40 +159,38 @@ struct ApproximateTime : public PolicyBase void checkInterMessageBound() { namespace mt = message_filters::message_traits; - if (warned_about_incorrect_bound_[i]) - { + if (warned_about_incorrect_bound_[i]) { return; } - std::deque::type>& deque = std::get(deques_); - std::vector::type>& v = std::get(past_); + std::deque::type> & deque = std::get(deques_); + std::vector::type> & v = std::get(past_); RCUTILS_ASSERT(!deque.empty()); - const typename std::tuple_element::type &msg = *(deque.back()).getMessage(); - rclcpp::Time msg_time = mt::TimeStamp::type>::value(msg); + const typename std::tuple_element::type & msg = *(deque.back()).getMessage(); + rclcpp::Time msg_time = + mt::TimeStamp::type>::value(msg); rclcpp::Time previous_msg_time; - if (deque.size() == (size_t) 1) - { - if (v.empty()) - { - // We have already published (or have never received) the previous message, we cannot check the bound - return; + if (deque.size() == (size_t) 1) { + if (v.empty()) { + // We have already published (or have never received) the previous message, we cannot check the bound + return; } - const typename std::tuple_element::type &previous_msg = *(v.back()).getMessage(); - previous_msg_time = mt::TimeStamp::type>::value(previous_msg); - } - else - { + const typename std::tuple_element::type & previous_msg = *(v.back()).getMessage(); + previous_msg_time = mt::TimeStamp::type>::value( + previous_msg); + } else { // There are at least 2 elements in the deque. Check that the gap respects the bound if it was provided. - const typename std::tuple_element::type &previous_msg = *(deque[deque.size()-2]).getMessage(); - previous_msg_time = mt::TimeStamp::type>::value(previous_msg); + const typename std::tuple_element::type & previous_msg = *(deque[deque.size() - 2]).getMessage(); + previous_msg_time = mt::TimeStamp::type>::value( + previous_msg); } - if (msg_time < previous_msg_time) - { + if (msg_time < previous_msg_time) { RCUTILS_LOG_WARN_ONCE("Messages of type %d arrived out of order (will print only once)", i); warned_about_incorrect_bound_[i] = true; - } - else if ((msg_time - previous_msg_time) < inter_message_lower_bounds_[i]) - { - RCUTILS_LOG_WARN_ONCE("Messages of type %d arrived closer (" + } else if ((msg_time - previous_msg_time) < inter_message_lower_bounds_[i]) { + RCUTILS_LOG_WARN_ONCE( + "Messages of type %d arrived closer (" "%" PRId64 ") than the lower bound you provided (" "%" PRId64 ") (will print only once)", i, @@ -201,30 +202,26 @@ struct ApproximateTime : public PolicyBase template - void add(const typename std::tuple_element::type& evt) + void add(const typename std::tuple_element::type & evt) { std::lock_guard lock(data_mutex_); - std::deque::type>& deque = std::get(deques_); + std::deque::type> & deque = std::get(deques_); deque.push_back(evt); if (deque.size() == (size_t)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)RealTypeCount::value) { // All deques have messages process(); } - } - else - { + } else { checkInterMessageBound(); } // Check whether we have more messages than allowed in the queue. // Note that during the above call to process(), queue i may contain queue_size_+1 messages. - std::vector::type>& past = std::get(past_); - if (deque.size() + past.size() > queue_size_) - { + std::vector::type> & past = std::get(past_); + 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>(); @@ -240,13 +237,12 @@ struct ApproximateTime : public PolicyBase RCUTILS_ASSERT(!deque.empty()); deque.pop_front(); has_dropped_messages_[i] = true; - if (pivot_ != NO_PIVOT) - { - // The candidate is no longer valid. Destroy it. - candidate_ = Tuple(); - pivot_ = NO_PIVOT; - // There might still be enough messages to create a new candidate: - process(); + if (pivot_ != NO_PIVOT) { + // The candidate is no longer valid. Destroy it. + candidate_ = Tuple(); + pivot_ = NO_PIVOT; + // There might still be enough messages to create a new candidate: + process(); } } } @@ -258,15 +254,17 @@ struct ApproximateTime : public PolicyBase age_penalty_ = age_penalty; } - void setInterMessageLowerBound(int i, rclcpp::Duration lower_bound) { + void setInterMessageLowerBound(int i, rclcpp::Duration lower_bound) + { // For correctness we only need age_penalty > -1.0, but most likely a negative age_penalty is a mistake. - RCUTILS_ASSERT(lower_bound >= rclcpp::Duration(0,0)); + RCUTILS_ASSERT(lower_bound >= rclcpp::Duration(0, 0)); inter_message_lower_bounds_[i] = lower_bound; } - void setMaxIntervalDuration(rclcpp::Duration max_interval_duration) { + void setMaxIntervalDuration(rclcpp::Duration max_interval_duration) + { // For correctness we only need age_penalty > -1.0, but most likely a negative age_penalty is a mistake. - RCUTILS_ASSERT(max_interval_duration >= rclcpp::Duration(0,0)); + RCUTILS_ASSERT(max_interval_duration >= rclcpp::Duration(0, 0)); max_interval_duration_ = max_interval_duration; } @@ -275,11 +273,10 @@ struct ApproximateTime : public PolicyBase template void dequeDeleteFront() { - std::deque::type>& deque = std::get(deques_); + std::deque::type> & deque = std::get(deques_); RCUTILS_ASSERT(!deque.empty()); deque.pop_front(); - if (deque.empty()) - { + if (deque.empty()) { --num_non_empty_deques_; } } @@ -287,37 +284,36 @@ struct ApproximateTime : public PolicyBase // Assumes that deque number is non empty void dequeDeleteFront(uint32_t index) { - 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: - RCUTILS_BREAK(); + 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: + RCUTILS_BREAK(); } } @@ -325,50 +321,48 @@ struct ApproximateTime : public PolicyBase template void dequeMoveFrontToPast() { - std::deque::type>& deque = std::get(deques_); - std::vector::type>& vector = std::get(past_); + std::deque::type> & deque = std::get(deques_); + std::vector::type> & vector = std::get(past_); RCUTILS_ASSERT(!deque.empty()); vector.push_back(deque.front()); deque.pop_front(); - if (deque.empty()) - { + if (deque.empty()) { --num_non_empty_deques_; } } // Assumes that deque number is non empty void dequeMoveFrontToPast(uint32_t index) { - 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: - RCUTILS_BREAK(); + 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: + RCUTILS_BREAK(); } } @@ -379,32 +373,25 @@ struct ApproximateTime : public PolicyBase 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) - { + 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(); - } - } - } - } - } + 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(); + } + } + } + } + } } } // Delete all past messages, since we have found a better candidate @@ -425,23 +412,20 @@ struct ApproximateTime : public PolicyBase template void recover(size_t num_messages) { - if (i >= RealTypeCount::value) - { + if (i >= RealTypeCount::value) { return; } - std::vector::type>& v = std::get(past_); - std::deque::type>& q = std::get(deques_); + std::vector::type> & v = std::get(past_); + std::deque::type> & q = std::get(deques_); RCUTILS_ASSERT(num_messages <= v.size()); - while (num_messages > 0) - { + while (num_messages > 0) { q.push_front(v.back()); v.pop_back(); num_messages--; } - if (!q.empty()) - { + if (!q.empty()) { ++num_non_empty_deques_; } } @@ -450,21 +434,18 @@ struct ApproximateTime : public PolicyBase template void recover() { - if (i >= RealTypeCount::value) - { + if (i >= RealTypeCount::value) { return; } - std::vector::type>& v = std::get(past_); - std::deque::type>& q = std::get(deques_); - while (!v.empty()) - { + std::vector::type> & v = std::get(past_); + std::deque::type> & q = std::get(deques_); + while (!v.empty()) { q.push_front(v.back()); v.pop_back(); } - if (!q.empty()) - { + if (!q.empty()) { ++num_non_empty_deques_; } } @@ -473,15 +454,13 @@ struct ApproximateTime : public PolicyBase template void recoverAndDelete() { - if (i >= RealTypeCount::value) - { + if (i >= RealTypeCount::value) { return; } - std::vector::type>& v = std::get(past_); - std::deque::type>& q = std::get(deques_); - while (!v.empty()) - { + std::vector::type> & v = std::get(past_); + std::deque::type> & q = std::get(deques_); + while (!v.empty()) { q.push_front(v.back()); v.pop_back(); } @@ -489,8 +468,7 @@ struct ApproximateTime : public PolicyBase RCUTILS_ASSERT(!q.empty()); q.pop_front(); - if (!q.empty()) - { + if (!q.empty()) { ++num_non_empty_deques_; } } @@ -500,9 +478,12 @@ struct ApproximateTime : public PolicyBase { //printf("Publishing candidate\n"); // 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_)); + 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_)); // Delete this candidate candidate_ = Tuple(); pivot_ = NO_PIVOT; @@ -522,7 +503,7 @@ struct ApproximateTime : public PolicyBase // Assumes: all deques are non empty, i.e. num_non_empty_deques_ == RealTypeCount::value // Returns: the oldest message on the deques - void getCandidateStart(uint32_t &start_index, rclcpp::Time &start_time) + void getCandidateStart(uint32_t & start_index, rclcpp::Time & start_time) { return getCandidateBoundary(start_index, start_time, false); } @@ -530,7 +511,7 @@ struct ApproximateTime : public PolicyBase // Assumes: all deques are non empty, i.e. num_non_empty_deques_ == RealTypeCount::value // 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) + void getCandidateEnd(uint32_t & end_index, rclcpp::Time & end_time) { return getCandidateBoundary(end_index, end_time, true); } @@ -538,81 +519,65 @@ struct ApproximateTime : public PolicyBase // 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) + void getCandidateBoundary(uint32_t & index, rclcpp::Time & time, bool end) { namespace mt = message_filters::message_traits; - M0Event& m0 = std::get<0>(deques_).front(); + 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) - { + 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) - { + 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) - { + 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) - { + 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) - { + 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) - { + 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) - { + 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) - { + 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; } @@ -626,38 +591,39 @@ struct ApproximateTime : public PolicyBase { namespace mt = message_filters::message_traits; - if (i >= RealTypeCount::value) - { - return rclcpp::Time(0,0); // Dummy return value + if (i >= RealTypeCount::value) { + return rclcpp::Time(0, 0); // Dummy return value } RCUTILS_ASSERT(pivot_ != NO_PIVOT); - std::vector::type>& v = std::get(past_); - std::deque::type>& q = std::get(deques_); - if (q.empty()) - { + std::vector::type> & v = std::get(past_); + std::deque::type> & q = std::get(deques_); + if (q.empty()) { RCUTILS_ASSERT(!v.empty()); // Because we have a candidate - rclcpp::Time last_msg_time = mt::TimeStamp::type>::value(*(v.back()).getMessage()); + rclcpp::Time last_msg_time = + mt::TimeStamp::type>::value( + *(v.back()).getMessage()); rclcpp::Time msg_time_lower_bound = last_msg_time + inter_message_lower_bounds_[i]; - if (msg_time_lower_bound > pivot_time_) // Take the max - { + if (msg_time_lower_bound > pivot_time_) { // Take the max return msg_time_lower_bound; } return pivot_time_; } - rclcpp::Time current_msg_time = mt::TimeStamp::type>::value(*(q.front()).getMessage()); + rclcpp::Time current_msg_time = + mt::TimeStamp::type>::value( + *(q.front()).getMessage()); return current_msg_time; } // ASSUMES: we have a pivot and candidate - void getVirtualCandidateStart(uint32_t &start_index, rclcpp::Time &start_time) + void getVirtualCandidateStart(uint32_t & start_index, rclcpp::Time & start_time) { return getVirtualCandidateBoundary(start_index, start_time, false); } // ASSUMES: we have a pivot and candidate - void getVirtualCandidateEnd(uint32_t &end_index, rclcpp::Time &end_time) + void getVirtualCandidateEnd(uint32_t & end_index, rclcpp::Time & end_time) { return getVirtualCandidateBoundary(end_index, end_time, true); } @@ -665,7 +631,7 @@ struct ApproximateTime : public PolicyBase // 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) + void getVirtualCandidateBoundary(uint32_t & index, rclcpp::Time & time, bool end) { std::vector virtual_times(9); virtual_times[0] = getVirtualTime<0>(); @@ -680,12 +646,10 @@ struct ApproximateTime : public PolicyBase time = virtual_times[0]; index = 0; - for (int i = 0; i < RealTypeCount::value; i++) - { - if ((virtual_times[i] < time) ^ end) - { - time = virtual_times[i]; - index = i; + for (int i = 0; i < RealTypeCount::value; i++) { + if ((virtual_times[i] < time) ^ end) { + time = virtual_times[i]; + index = i; } } } @@ -695,8 +659,7 @@ 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)RealTypeCount::value) { // Find the start and end of the current interval //printf("Entering while loop in this state [\n"); //show_internal_state(); @@ -705,52 +668,42 @@ struct ApproximateTime : public PolicyBase 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++) - { - 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 - has_dropped_messages_[i] = false; - } + for (uint32_t i = 0; i < (uint32_t)RealTypeCount::value; 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 + has_dropped_messages_[i] = false; + } } - if (pivot_ == NO_PIVOT) - { + if (pivot_ == NO_PIVOT) { // We do not have a candidate // INVARIANT: the past_ vectors are empty // INVARIANT: (candidate_ has no filled members) - if (end_time - start_time > max_interval_duration_) - { + if (end_time - start_time > max_interval_duration_) { // This interval is too big to be a valid candidate, move to the next dequeDeleteFront(start_index); continue; } - if (has_dropped_messages_[end_index]) - { - // The topic that would become pivot has dropped messages, so it is not a good pivot - dequeDeleteFront(start_index); - continue; - } - // This is a valid candidate, and we don't have any, so take it - makeCandidate(); - candidate_start_ = start_time; - candidate_end_ = end_time; - pivot_ = end_index; - pivot_time_ = end_time; - dequeMoveFrontToPast(start_index); - } - else - { + if (has_dropped_messages_[end_index]) { + // The topic that would become pivot has dropped messages, so it is not a good pivot + dequeDeleteFront(start_index); + continue; + } + // This is a valid candidate, and we don't have any, so take it + makeCandidate(); + candidate_start_ = start_time; + candidate_end_ = end_time; + pivot_ = end_index; + pivot_time_ = end_time; + dequeMoveFrontToPast(start_index); + } else { // We already have a candidate // Is this one better than the current candidate? // INVARIANT: has_dropped_messages_ is all false - if ((end_time - candidate_end_) * (1 + age_penalty_) >= (start_time - candidate_start_)) - { + if ((end_time - candidate_end_) * (1 + age_penalty_) >= (start_time - candidate_start_)) { // This is not a better candidate, move to the next dequeMoveFrontToPast(start_index); - } - else - { + } else { // This is a better candidate makeCandidate(); candidate_start_ = start_time; @@ -762,12 +715,11 @@ struct ApproximateTime : public PolicyBase // INVARIANT: we have a candidate and pivot RCUTILS_ASSERT(pivot_ != NO_PIVOT); //printf("start_index == %d, pivot_ == %d\n", start_index, pivot_); - if (start_index == pivot_) // TODO: replace with start_time == pivot_time_ - { + if (start_index == pivot_) { // TODO: replace with start_time == pivot_time_ // We have exhausted all possible candidates for this pivot, we now can output the best one publishCandidate(); - } - else if ((end_time - candidate_end_) * (1 + age_penalty_) >= (pivot_time_ - candidate_start_)) + } else if ((end_time - candidate_end_) * (1 + age_penalty_) >= + (pivot_time_ - candidate_start_)) { // We have not exhausted all candidates, but this candidate is already provably optimal // Indeed, any future candidate must contain the interval [pivot_time_ end_time], which @@ -775,20 +727,18 @@ 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)RealTypeCount::value) { 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); - while (1) - { + std::vector num_virtual_moves(9, 0); + while (1) { rclcpp::Time end_time, start_time; uint32_t end_index, start_index; getVirtualCandidateEnd(end_index, end_time); getVirtualCandidateStart(start_index, start_time); - if ((end_time - candidate_end_) * (1 + age_penalty_) >= (pivot_time_ - candidate_start_)) + if ((end_time - candidate_end_) * (1 + age_penalty_) >= + (pivot_time_ - candidate_start_)) { // We have proved optimality // As above, any future candidate must contain the interval [pivot_time_ end_time], which @@ -796,22 +746,23 @@ struct ApproximateTime : public PolicyBase publishCandidate(); // This cleans up the virtual moves as a byproduct break; // From the while(1) loop only } - if ((end_time - candidate_end_) * (1 + age_penalty_) < (start_time - candidate_start_)) + if ((end_time - candidate_end_) * (1 + age_penalty_) < + (start_time - candidate_start_)) { // We cannot prove optimality // Indeed, we have a virtual (i.e. optimistic) candidate that is better than the current // 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]); + 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]); (void)num_non_empty_deques_before_virtual_search; // unused variable warning stopper RCUTILS_ASSERT(num_non_empty_deques_before_virtual_search == num_non_empty_deques_); break; @@ -819,8 +770,8 @@ struct ApproximateTime : public PolicyBase // Note: we cannot reach this point with start_index == pivot_ since in that case we would // have start_time == pivot_time, in which case the two tests above are the negation // of each other, so that one must be true. Therefore the while loop always terminates. - RCUTILS_ASSERT(start_index != pivot_); - RCUTILS_ASSERT(start_time < pivot_time_); + RCUTILS_ASSERT(start_index != pivot_); + RCUTILS_ASSERT(start_time < pivot_time_); dequeMoveFrontToPast(start_index); num_virtual_moves[start_index]++; } // while(1) @@ -828,7 +779,7 @@ struct ApproximateTime : public PolicyBase } // while(num_non_empty_deques_ == (uint32_t)RealTypeCount::value) } - Sync* parent_; + Sync * parent_; uint32_t queue_size_; static const uint32_t NO_PIVOT = 9; // Special value for the pivot indicating that no pivot has been selected diff --git a/include/message_filters/sync_policies/exact_time.h b/include/message_filters/sync_policies/exact_time.h index 0d87a37..39ca7d4 100644 --- a/include/message_filters/sync_policies/exact_time.h +++ b/include/message_filters/sync_policies/exact_time.h @@ -29,7 +29,9 @@ #ifndef MESSAGE_FILTERS__SYNC_POLICIES__EXACT_TIME_H_ #define MESSAGE_FILTERS__SYNC_POLICIES__EXACT_TIME_H_ +#include #include +#include #include #include @@ -46,8 +48,9 @@ namespace message_filters namespace sync_policies { -template +template struct ExactTime : public PolicyBase { typedef Synchronizer Sync; @@ -69,16 +72,16 @@ struct ExactTime : public PolicyBase ExactTime(uint32_t queue_size) : parent_(0) - , queue_size_(queue_size) + , queue_size_(queue_size) { } - ExactTime(const ExactTime& e) + ExactTime(const ExactTime & e) { *this = e; } - ExactTime& operator=(const ExactTime& rhs) + ExactTime & operator=(const ExactTime & rhs) { parent_ = rhs.parent_; queue_size_ = rhs.queue_size_; @@ -88,13 +91,13 @@ struct ExactTime : public PolicyBase return *this; } - void initParent(Sync* parent) + void initParent(Sync * parent) { parent_ = parent; } template - void add(const typename std::tuple_element::type& evt) + void add(const typename std::tuple_element::type & evt) { RCUTILS_ASSERT(parent_); @@ -102,32 +105,33 @@ 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); } template - Connection registerDropCallback(const C& callback) + Connection registerDropCallback(const C & callback) { return drop_signal_.addCallback(callback); } template - Connection registerDropCallback(C& callback) + Connection registerDropCallback(C & callback) { return drop_signal_.addCallback(callback); } template - Connection registerDropCallback(const C& callback, T* t) + Connection registerDropCallback(const C & callback, T * t) { return drop_signal_.addCallback(callback, t); } template - Connection registerDropCallback(C& callback, T* t) + Connection registerDropCallback(C & callback, T * t) { return drop_signal_.addCallback(callback, t); } @@ -138,9 +142,8 @@ struct ExactTime : public PolicyBase } private: - // assumes mutex_ is already locked - void checkTuple(Tuple& t) + void checkTuple(Tuple & t) { namespace mt = message_filters::message_traits; @@ -155,11 +158,11 @@ struct ExactTime : public PolicyBase full = full && (RealTypeCount::value > 7 ? (bool)std::get<7>(t).getMessage() : true); full = full && (RealTypeCount::value > 8 ? (bool)std::get<8>(t).getMessage() : true); - 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)); + 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)); last_signal_time_ = mt::TimeStamp::value(*std::get<0>(t).getMessage()); @@ -168,14 +171,13 @@ struct ExactTime : public PolicyBase clearOldTuples(); } - 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)); + 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)); tuples_.erase(tuples_.begin()); } } @@ -186,21 +188,18 @@ struct ExactTime : public PolicyBase { typename M_TimeToTuple::iterator it = tuples_.begin(); typename M_TimeToTuple::iterator end = tuples_.end(); - for (; it != end;) - { - if (it->first <= last_signal_time_) - { + for (; it != end; ) { + if (it->first <= last_signal_time_) { typename M_TimeToTuple::iterator old = it; ++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)); + 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)); tuples_.erase(old); - } - else - { + } else { // the map is sorted by time, so we can ignore anything after this if this one's time is ok break; } @@ -208,7 +207,7 @@ struct ExactTime : public PolicyBase } private: - Sync* parent_; + Sync * parent_; uint32_t queue_size_; typedef std::map M_TimeToTuple; @@ -224,4 +223,3 @@ struct ExactTime : public PolicyBase } // namespace message_filters #endif // MESSAGE_FILTERS__SYNC_POLICIES__EXACT_TIME_H_ - diff --git a/include/message_filters/sync_policies/latest_time.h b/include/message_filters/sync_policies/latest_time.h index 2514f96..ffcd016 100644 --- a/include/message_filters/sync_policies/latest_time.h +++ b/include/message_filters/sync_policies/latest_time.h @@ -43,7 +43,7 @@ Synchronizer sync_policies(latest_policy(), caminfo_sub, limage_s sync_policies.registerCallback(callback); \endverbatim - * May also take an instance of a `rclcpp::Clock::SharedPtr` from `rclpp::Node::get_clock()` + * May also take an instance of a `rclcpp::Clock::SharedPtr` from `rclpp::Node::get_clock()` * to use the node's time source (e.g. sim time) as in: \verbatim typedef LatestTime latest_policy; @@ -53,7 +53,7 @@ sync_policies.registerCallback(callback); * The callback is then of the form: \verbatim -void callback(const sensor_msgs::CameraInfo::ConstPtr&, const sensor_msgs::Image::ConstPtr&, const sensor_msgs::Image::ConstPtr&); +void callback(const sensor_msgs::CameraInfo::ConstPtr &, const sensor_msgs::Image::ConstPtr&, const sensor_msgs::Image::ConstPtr&); \endverbatim * */ @@ -62,6 +62,7 @@ void callback(const sensor_msgs::CameraInfo::ConstPtr&, const sensor_msgs::Image #define MESSAGE_FILTERS__SYNC_POLICIES__LATEST_TIME_H_ #include +#include #include #include #include @@ -81,9 +82,9 @@ namespace sync_policies { template + typename M2 = NullType, typename M3 = NullType, typename M4 = NullType, + typename M5 = NullType, typename M6 = NullType, typename M7 = NullType, + typename M8 = NullType> struct LatestTime : public PolicyBase { typedef Synchronizer Sync; @@ -98,17 +99,17 @@ struct LatestTime : public PolicyBase typedef std::tuple RateConfig; LatestTime() - : LatestTime(rclcpp::Clock::SharedPtr(new rclcpp::Clock(RCL_ROS_TIME))) + : LatestTime(rclcpp::Clock::SharedPtr(new rclcpp::Clock(RCL_ROS_TIME))) { } - LatestTime(rclcpp::Clock::SharedPtr clock) + explicit LatestTime(rclcpp::Clock::SharedPtr clock) : parent_(0), ros_clock_{clock} { } - LatestTime(const LatestTime& e) + LatestTime(const LatestTime & e) { *this = e; } @@ -150,8 +151,7 @@ struct LatestTime : public PolicyBase std::lock_guard lock(data_mutex_); - if(!received_msg()) - { + if (!received_msg()) { initialize_rate(); // wait until we get each message once to publish // then wait until we got each message twice to compute rates @@ -165,8 +165,7 @@ struct LatestTime : public PolicyBase std::get(events_) = evt; rclcpp::Time now = ros_clock_->now(); bool valid_rate = rates_[i].compute_hz(now); - if(valid_rate && (i == find_pivot(now)) && is_full()) - { + if (valid_rate && (i == find_pivot(now)) && is_full()) { publish(); } } @@ -181,12 +180,12 @@ struct LatestTime : public PolicyBase double error_ema_alpha{Rate::DEFAULT_ERROR_EMA_ALPHA}; double rate_step_change_margin_factor{Rate::DEFAULT_MARGIN_FACTOR}; if (rate_configs_.size() == RealTypeCount::value) { - std::tie ( + std::tie( rate_ema_alpha, error_ema_alpha, rate_step_change_margin_factor) = rate_configs_[i]; } else if (rate_configs_.size() == 1U) { - std::tie ( + std::tie( rate_ema_alpha, error_ema_alpha, rate_step_change_margin_factor) = rate_configs_[0U]; @@ -205,9 +204,10 @@ 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_)); + 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_)); } struct Rate @@ -225,18 +225,19 @@ struct LatestTime : public PolicyBase bool do_hz_init{true}; bool do_error_init{true}; - Rate(const rclcpp::Time & start) - : Rate(start, DEFAULT_RATE_EMA_ALPHA, DEFAULT_ERROR_EMA_ALPHA, DEFAULT_MARGIN_FACTOR) + explicit Rate(const rclcpp::Time & start) + : Rate(start, DEFAULT_RATE_EMA_ALPHA, DEFAULT_ERROR_EMA_ALPHA, DEFAULT_MARGIN_FACTOR) { } - Rate(const rclcpp::Time & start, + Rate( + const rclcpp::Time & start, const double & rate_ema_alpha, const double & error_ema_alpha, const double & rate_step_change_margin_factor) - : prev{start}, - rate_ema_alpha{rate_ema_alpha}, - error_ema_alpha{error_ema_alpha}, - rate_step_change_margin_factor{rate_step_change_margin_factor} + : prev{start}, + rate_ema_alpha{rate_ema_alpha}, + error_ema_alpha{error_ema_alpha}, + rate_step_change_margin_factor{rate_step_change_margin_factor} { } @@ -249,23 +250,23 @@ struct LatestTime : public PolicyBase { bool step_change_detected = false; do { - double period = (now-prev).seconds(); + double period = (now - prev).seconds(); if (period <= 0.0) { // multiple messages and time isn't updating return false; } if (do_hz_init) { - hz = 1.0/period; + hz = 1.0 / period; do_hz_init = false; step_change_detected = false; } else { if (do_error_init) { - error = fabs(hz - 1.0/period); + error = fabs(hz - 1.0 / period); do_error_init = false; } else { // check if rate is some multiple of mean error from mean - if (fabs(hz - 1.0/period) > rate_step_change_margin_factor*error) { + if (fabs(hz - 1.0 / period) > rate_step_change_margin_factor * error) { // detected step change in rate so reset do_hz_init = true; do_error_init = true; @@ -273,9 +274,9 @@ struct LatestTime : public PolicyBase continue; } // on-line mean error from mean - error = error_ema_alpha*fabs(hz - 1.0/period) + (1.0 - error_ema_alpha)*error; + error = error_ema_alpha * fabs(hz - 1.0 / period) + (1.0 - error_ema_alpha) * error; } - hz = rate_ema_alpha/period + (1.0 - rate_ema_alpha)*hz; + hz = rate_ema_alpha / period + (1.0 - rate_ema_alpha) * hz; } } while (step_change_detected); prev = now; @@ -284,7 +285,7 @@ struct LatestTime : public PolicyBase }; // assumed data_mutex_ is locked - template + template std::vector sort_indices(const std::vector & v) { // initialize original index locations @@ -295,8 +296,9 @@ struct LatestTime : public PolicyBase // using std::stable_sort instead of std::sort // to avoid unnecessary index re-orderings // when v contains elements of equal values - std::stable_sort(idx.begin(), idx.end(), - [&v](std::size_t i1, std::size_t i2) {return v[i1] > v[i2];}); + std::stable_sort( + idx.begin(), idx.end(), + [&v](std::size_t i1, std::size_t i2) {return v[i1] > v[i2];}); return idx; } @@ -305,7 +307,7 @@ struct LatestTime : public PolicyBase template bool received_msg() { - return (RealTypeCount::value > i ? (bool)std::get(events_).getMessage() : true); + return RealTypeCount::value > i ? (bool)std::get(events_).getMessage() : true; } // assumed data_mutex_ is locked @@ -332,7 +334,7 @@ struct LatestTime : public PolicyBase // use fastest message that isn't late as pivot for (size_t pivot : sorted_idx) { - double period = (now-rates_[pivot].prev).seconds(); + double period = (now - rates_[pivot].prev).seconds(); if (period == 0.0) { if (rates_[pivot].hz > 0.0) { // we just updated updated this one, @@ -346,7 +348,7 @@ struct LatestTime : public PolicyBase if (!rates_[pivot].do_error_init) { // can now check if new messages are late - double rate_delta = rates_[pivot].hz - 1.0/period; + double rate_delta = rates_[pivot].hz - 1.0 / period; double margin = rates_[pivot].rate_step_change_margin_factor * rates_[pivot].error; if (rate_delta > margin) { // this pivot is late @@ -365,7 +367,7 @@ struct LatestTime : public PolicyBase return NO_PIVOT; } - Sync* parent_; + Sync * parent_; Events events_; std::vector rates_; std::mutex data_mutex_; // Protects all of the above @@ -377,7 +379,7 @@ struct LatestTime : public PolicyBase rclcpp::Clock::SharedPtr ros_clock_{nullptr}; }; -} // namespace sync +} // namespace sync_policies } // namespace message_filters -#endif // MESSAGE_FILTERS__SYNC_POLICIES__LATEST_TIME_H_ +#endif // MESSAGE_FILTERS__SYNC_POLICIES__LATEST_TIME_H_ diff --git a/include/message_filters/synchronizer.h b/include/message_filters/synchronizer.h index 3735e3a..feaa269 100644 --- a/include/message_filters/synchronizer.h +++ b/include/message_filters/synchronizer.h @@ -30,6 +30,8 @@ #define MESSAGE_FILTERS__SYNCHRONIZER_H_ #include +#include +#include #include #include #include @@ -75,56 +77,56 @@ class Synchronizer : public noncopyable, public Policy static const uint8_t MAX_MESSAGES = 9; template - Synchronizer(F0& f0, F1& f1) + Synchronizer(F0 & f0, F1 & f1) { connectInput(f0, f1); init(); } template - Synchronizer(F0& f0, F1& f1, F2& f2) + Synchronizer(F0 & f0, F1 & f1, F2 & f2) { connectInput(f0, f1, f2); init(); } template - Synchronizer(F0& f0, F1& f1, F2& f2, F3& f3) + 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) + 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) + 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) + 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) + 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) + 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); init(); @@ -136,7 +138,7 @@ class Synchronizer : public noncopyable, public Policy } template - Synchronizer(const Policy& policy, F0& f0, F1& f1) + Synchronizer(const Policy & policy, F0 & f0, F1 & f1) : Policy(policy) { connectInput(f0, f1); @@ -144,7 +146,7 @@ class Synchronizer : public noncopyable, public Policy } template - Synchronizer(const Policy& policy, F0& f0, F1& f1, F2& f2) + Synchronizer(const Policy & policy, F0 & f0, F1 & f1, F2 & f2) : Policy(policy) { connectInput(f0, f1, f2); @@ -152,7 +154,7 @@ class Synchronizer : public noncopyable, public Policy } template - Synchronizer(const Policy& policy, F0& f0, F1& f1, F2& f2, F3& f3) + Synchronizer(const Policy & policy, F0 & f0, F1 & f1, F2 & f2, F3 & f3) : Policy(policy) { connectInput(f0, f1, f2, f3); @@ -160,7 +162,7 @@ class Synchronizer : public noncopyable, public Policy } template - Synchronizer(const Policy& policy, F0& f0, F1& f1, F2& f2, F3& f3, F4& f4) + Synchronizer(const Policy & policy, F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4) : Policy(policy) { connectInput(f0, f1, f2, f3, f4); @@ -168,7 +170,7 @@ class Synchronizer : public noncopyable, public Policy } template - Synchronizer(const Policy& policy, F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5) + 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); @@ -176,7 +178,7 @@ class Synchronizer : public noncopyable, public Policy } template - Synchronizer(const Policy& policy, F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, F6& f6) + 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); @@ -184,7 +186,8 @@ class Synchronizer : public noncopyable, public Policy } template - Synchronizer(const Policy& policy, F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, F6& f6, F7& f7) + 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); @@ -192,14 +195,16 @@ class Synchronizer : public noncopyable, public Policy } template - Synchronizer(const Policy& policy, F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, F6& f6, F7& f7, F8& f8) + 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); init(); } - Synchronizer(const Policy& policy) + Synchronizer(const Policy & policy) : Policy(policy) { init(); @@ -216,126 +221,144 @@ class Synchronizer : public noncopyable, public Policy } template - void connectInput(F0& f0, F1& f1) + void connectInput(F0 & f0, F1 & f1) { NullFilter f2; connectInput(f0, f1, f2); } template - void connectInput(F0& f0, F1& f1, F2& f2) + void connectInput(F0 & f0, F1 & f1, F2 & f2) { NullFilter f3; connectInput(f0, f1, f2, f3); } template - void connectInput(F0& f0, F1& f1, F2& f2, F3& f3) + 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) + 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) + 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) + 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) + 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) + void connectInput(F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5, F6 & f6, F7 & f7, F8 & f8) { 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))); + 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))); } template - Connection registerCallback(C& callback) + Connection registerCallback(C & callback) { return signal_.addCallback(callback); } template - Connection registerCallback(const C& callback) + Connection registerCallback(const C & callback) { return signal_.addCallback(callback); } template - Connection registerCallback(const C& callback, T* t) + Connection registerCallback(const C & callback, T * t) { return signal_.addCallback(callback, t); } template - Connection registerCallback(C& callback, T* t) + Connection registerCallback(C & callback, T * t) { return signal_.addCallback(callback, t); } - void setName(const std::string& name) { name_ = name; } - const std::string& getName() { return name_; } + void setName(const std::string & name) {name_ = name;} + 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) + 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) { signal_.call(e0, e1, e2, e3, e4, e5, e6, e7, e8); } - Policy* getPolicy() { return static_cast(this); } + Policy * getPolicy() {return static_cast(this);} using Policy::add; template - void add(const std::shared_ptr::type const>& msg) + void add(const std::shared_ptr::type const> & msg) { this->template add(typename std::tuple_element::type(msg)); } private: - void disconnectAll() { - for (int i = 0; i < MAX_MESSAGES; ++i) - { + for (int i = 0; i < MAX_MESSAGES; ++i) { input_connections_[i].disconnect(); } } template - void cb(const typename std::tuple_element::type& evt) + void cb(const typename std::tuple_element::type & evt) { this->template add(evt); } @@ -349,35 +372,40 @@ class Synchronizer : public noncopyable, public Policy std::string name_; }; -template struct mp_plus; -template<> struct mp_plus<> +template +struct mp_plus; +template<> +struct mp_plus<> { using type = std::integral_constant; }; -template struct mp_plus +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> +template +struct mp_count; +template class L, class ... T, class V> +struct mp_count, V> { using type = typename mp_plus...>::type; }; template + typename M5, typename M6, typename M7, typename M8> struct PolicyBase { - typedef typename mp_count, NullType>::type RealTypeCount; + 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; + 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; diff --git a/include/message_filters/time_sequencer.h b/include/message_filters/time_sequencer.h index 020f0c7..f78327d 100644 --- a/include/message_filters/time_sequencer.h +++ b/include/message_filters/time_sequencer.h @@ -29,6 +29,10 @@ #ifndef MESSAGE_FILTERS__TIME_SEQUENCER_H_ #define MESSAGE_FILTERS__TIME_SEQUENCER_H_ +#include +#include +#include + #include #include "message_filters/connection.h" @@ -82,11 +86,13 @@ class TimeSequencer : public SimpleFilter * \param node The Node to use to create the rclcpp::SteadyTimer that runs at update_rate */ template - TimeSequencer(F& f, rclcpp::Duration delay, rclcpp::Duration update_rate, uint32_t queue_size, rclcpp::Node::SharedPtr node) + TimeSequencer( + F & f, rclcpp::Duration delay, rclcpp::Duration update_rate, uint32_t queue_size, + rclcpp::Node::SharedPtr node) : delay_(delay) - , update_rate_(update_rate) - , queue_size_(queue_size) - , node_(node) + , update_rate_(update_rate) + , queue_size_(queue_size) + , node_(node) { init(); connectInput(f); @@ -102,11 +108,13 @@ class TimeSequencer : public SimpleFilter * \param queue_size The number of messages to store * \param node The Node to use to create the rclcpp::SteadyTimer that runs at update_rate */ - TimeSequencer(rclcpp::Duration delay, rclcpp::Duration update_rate, uint32_t queue_size, rclcpp::Node::SharedPtr node) + TimeSequencer( + rclcpp::Duration delay, rclcpp::Duration update_rate, uint32_t queue_size, + rclcpp::Node::SharedPtr node) : delay_(delay) - , update_rate_(update_rate) - , queue_size_(queue_size) - , node_(node) + , update_rate_(update_rate) + , queue_size_(queue_size) + , node_(node) { init(); } @@ -115,10 +123,15 @@ class TimeSequencer : public SimpleFilter * \brief Connect this filter's input to another filter's output. */ template - void connectInput(F& f) + void connectInput(F & f) { incoming_connection_.disconnect(); - incoming_connection_ = f.registerCallback(typename SimpleFilter::EventCallback(std::bind(&TimeSequencer::cb, this, std::placeholders::_1))); + incoming_connection_ = + f.registerCallback( + typename SimpleFilter::EventCallback( + std::bind( + &TimeSequencer::cb, this, + std::placeholders::_1))); } ~TimeSequencer() @@ -127,20 +140,18 @@ class TimeSequencer : public SimpleFilter incoming_connection_.disconnect(); } - void add(const EventType& evt) + void add(const EventType & evt) { namespace mt = message_filters::message_traits; std::lock_guard lock(messages_mutex_); - if (mt::TimeStamp::value(*evt.getMessage()) < last_time_) - { + if (mt::TimeStamp::value(*evt.getMessage()) < last_time_) { return; } messages_.insert(evt); - if (queue_size_ != 0 && messages_.size() > queue_size_) - { + if (queue_size_ != 0 && messages_.size() > queue_size_) { messages_.erase(*messages_.begin()); } } @@ -148,7 +159,7 @@ class TimeSequencer : public SimpleFilter /** * \brief Manually add a message to the cache. */ - void add(const MConstPtr& msg) + void add(const MConstPtr & msg) { EventType evt(msg); add(evt); @@ -157,17 +168,18 @@ class TimeSequencer : public SimpleFilter private: class MessageSort { - public: - bool operator()(const EventType& lhs, const EventType& rhs) const +public: + bool operator()(const EventType & lhs, const EventType & rhs) const { namespace mt = message_filters::message_traits; - return mt::TimeStamp::value(*lhs.getMessage()) < mt::TimeStamp::value(*rhs.getMessage()); + return mt::TimeStamp::value(*lhs.getMessage()) < + mt::TimeStamp::value(*rhs.getMessage()); } }; typedef std::multiset S_Message; typedef std::vector V_Message; - void cb(const EventType& evt) + void cb(const EventType & evt) { add(evt); } @@ -181,18 +193,14 @@ class TimeSequencer : public SimpleFilter { std::lock_guard lock(messages_mutex_); - while (!messages_.empty()) - { - const EventType& e = *messages_.begin(); + while (!messages_.empty()) { + const EventType & e = *messages_.begin(); rclcpp::Time stamp = mt::TimeStamp::value(*e.getMessage()); - if ((stamp + delay_) <= rclcpp::Clock().now()) - { + if ((stamp + delay_) <= rclcpp::Clock().now()) { last_time_ = stamp; to_call.push_back(e); messages_.erase(messages_.begin()); - } - else - { + } else { break; } } @@ -201,8 +209,7 @@ class TimeSequencer : public SimpleFilter { typename V_Message::iterator it = to_call.begin(); typename V_Message::iterator end = to_call.end(); - for (; it != end; ++it) - { + for (; it != end; ++it) { this->signalMessage(*it); } } @@ -210,8 +217,9 @@ class TimeSequencer : public SimpleFilter void init() { - update_timer_ = node_->create_wall_timer(std::chrono::nanoseconds(update_rate_.nanoseconds()), [this]() { - dispatch(); + update_timer_ = node_->create_wall_timer( + std::chrono::nanoseconds(update_rate_.nanoseconds()), [this]() { + dispatch(); }); } diff --git a/include/message_filters/time_synchronizer.h b/include/message_filters/time_synchronizer.h index 1ea6f1b..4b11373 100644 --- a/include/message_filters/time_synchronizer.h +++ b/include/message_filters/time_synchronizer.h @@ -74,8 +74,9 @@ void callback(const sensor_msgs::msg::CameraInfo::SharedPtr, const sensor_msgs:: * */ template -class TimeSynchronizer : public Synchronizer > + class M5 = NullType, class M6 = NullType, class M7 = NullType, class M8 = NullType> +class TimeSynchronizer : public Synchronizer> { public: typedef sync_policies::ExactTime Policy; @@ -107,56 +108,62 @@ class TimeSynchronizer : public Synchronizer - TimeSynchronizer(F0& f0, F1& f1, uint32_t queue_size) + TimeSynchronizer(F0 & f0, F1 & f1, uint32_t queue_size) : Base(Policy(queue_size)) { connectInput(f0, f1); } template - TimeSynchronizer(F0& f0, F1& f1, F2& f2, uint32_t queue_size) + TimeSynchronizer(F0 & f0, F1 & f1, F2 & f2, uint32_t queue_size) : Base(Policy(queue_size)) { connectInput(f0, f1, f2); } template - TimeSynchronizer(F0& f0, F1& f1, F2& f2, F3& f3, uint32_t queue_size) + TimeSynchronizer(F0 & f0, F1 & f1, F2 & f2, F3 & f3, uint32_t queue_size) : Base(Policy(queue_size)) { connectInput(f0, f1, f2, f3); } template - TimeSynchronizer(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, uint32_t queue_size) + 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); } template - TimeSynchronizer(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, uint32_t queue_size) + 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); } template - TimeSynchronizer(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, F6& f6, uint32_t queue_size) + 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); } template - TimeSynchronizer(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, F6& f6, F7& f7, uint32_t queue_size) + 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); } template - TimeSynchronizer(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, F6& f6, F7& f7, F8& f8, uint32_t queue_size) + 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); @@ -170,47 +177,47 @@ class TimeSynchronizer : public Synchronizertemplate add<0>(M0Event(msg)); } - void add1(const M1ConstPtr& msg) + void add1(const M1ConstPtr & msg) { this->template add<1>(M1Event(msg)); } - void add2(const M2ConstPtr& msg) + void add2(const M2ConstPtr & msg) { this->template add<2>(M2Event(msg)); } - void add3(const M3ConstPtr& msg) + void add3(const M3ConstPtr & msg) { this->template add<3>(M3Event(msg)); } - void add4(const M4ConstPtr& msg) + void add4(const M4ConstPtr & msg) { this->template add<4>(M4Event(msg)); } - void add5(const M5ConstPtr& msg) + void add5(const M5ConstPtr & msg) { this->template add<5>(M5Event(msg)); } - void add6(const M6ConstPtr& msg) + void add6(const M6ConstPtr & msg) { this->template add<6>(M6Event(msg)); } - void add7(const M7ConstPtr& msg) + void add7(const M7ConstPtr & msg) { this->template add<7>(M7Event(msg)); } - void add8(const M8ConstPtr& msg) + void add8(const M8ConstPtr & msg) { this->template add<8>(M8Event(msg)); } diff --git a/include/message_filters/visibility_control.h b/include/message_filters/visibility_control.h index 72875ba..43d52e3 100644 --- a/include/message_filters/visibility_control.h +++ b/include/message_filters/visibility_control.h @@ -54,4 +54,3 @@ extern "C" #endif #endif // MESSAGE_FILTERS__VISIBILITY_CONTROL_H_ - diff --git a/package.xml b/package.xml index 5914e3e..74a0e3b 100644 --- a/package.xml +++ b/package.xml @@ -30,6 +30,7 @@ rclpy ament_cmake_copyright + ament_cmake_uncrustify ament_cmake_gtest ament_cmake_pytest sensor_msgs diff --git a/src/connection.cpp b/src/connection.cpp index b294e40..852102d 100644 --- a/src/connection.cpp +++ b/src/connection.cpp @@ -31,19 +31,16 @@ namespace message_filters { -Connection::Connection(const VoidDisconnectFunction& func) +Connection::Connection(const VoidDisconnectFunction & func) : void_disconnect_(func) { } void Connection::disconnect() { - if (void_disconnect_) - { + if (void_disconnect_) { void_disconnect_(); - } - else if (connection_disconnect_) - { + } else if (connection_disconnect_) { connection_disconnect_(*this); } } diff --git a/test/msg_cache_unittest.cpp b/test/msg_cache_unittest.cpp index 75553a6..be36fe2 100644 --- a/test/msg_cache_unittest.cpp +++ b/test/msg_cache_unittest.cpp @@ -38,15 +38,15 @@ struct Header { - rclcpp::Time stamp ; -} ; + rclcpp::Time stamp; +}; struct Msg { - Header header ; - int data ; -} ; + Header header; + int data; +}; typedef std::shared_ptr MsgConstPtr; namespace message_filters { @@ -55,7 +55,7 @@ namespace message_traits template<> struct TimeStamp { - static rclcpp::Time value(const Msg& m) + static rclcpp::Time value(const Msg & m) { return m.header.stamp; } @@ -63,7 +63,7 @@ struct TimeStamp } } -void fillCacheEasy(message_filters::Cache& cache, unsigned int start, unsigned int end) +void fillCacheEasy(message_filters::Cache & cache, unsigned int start, unsigned int end) { for (unsigned int i = start; i < end; i++) { Msg * msg = new Msg; @@ -80,21 +80,25 @@ TEST(Cache, easyInterval) message_filters::Cache cache(10); fillCacheEasy(cache, 0, 5); - std::vector > interval_data = cache.getInterval(rclcpp::Time(5, 0), rclcpp::Time(35, 0)); + std::vector> interval_data = cache.getInterval( + rclcpp::Time( + 5, + 0), rclcpp::Time( + 35, 0)); - ASSERT_EQ(interval_data.size(), (unsigned int) 3) ; - EXPECT_EQ(interval_data[0]->data, 1) ; - EXPECT_EQ(interval_data[1]->data, 2) ; - EXPECT_EQ(interval_data[2]->data, 3) ; + ASSERT_EQ(interval_data.size(), (unsigned int) 3); + EXPECT_EQ(interval_data[0]->data, 1); + EXPECT_EQ(interval_data[1]->data, 2); + EXPECT_EQ(interval_data[2]->data, 3); // Look for an interval past the end of the cache - interval_data = cache.getInterval(rclcpp::Time(55, 0), rclcpp::Time(65, 0)) ; - EXPECT_EQ(interval_data.size(), (unsigned int) 0) ; + interval_data = cache.getInterval(rclcpp::Time(55, 0), rclcpp::Time(65, 0)); + EXPECT_EQ(interval_data.size(), (unsigned int) 0); // Look for an interval that fell off the back of the cache - fillCacheEasy(cache, 5, 20) ; - interval_data = cache.getInterval(rclcpp::Time(5, 0), rclcpp::Time(35, 0)) ; - EXPECT_EQ(interval_data.size(), (unsigned int) 0) ; + fillCacheEasy(cache, 5, 20); + interval_data = cache.getInterval(rclcpp::Time(5, 0), rclcpp::Time(35, 0)); + EXPECT_EQ(interval_data.size(), (unsigned int) 0); } TEST(Cache, easySurroundingInterval) @@ -102,24 +106,24 @@ TEST(Cache, easySurroundingInterval) message_filters::Cache cache(10); fillCacheEasy(cache, 1, 6); - std::vector > interval_data; - interval_data = cache.getSurroundingInterval(rclcpp::Time(15,0), rclcpp::Time(35,0)) ; + std::vector> interval_data; + interval_data = cache.getSurroundingInterval(rclcpp::Time(15, 0), rclcpp::Time(35, 0)); ASSERT_EQ(interval_data.size(), (unsigned int) 4); EXPECT_EQ(interval_data[0]->data, 1); EXPECT_EQ(interval_data[1]->data, 2); EXPECT_EQ(interval_data[2]->data, 3); EXPECT_EQ(interval_data[3]->data, 4); - interval_data = cache.getSurroundingInterval(rclcpp::Time(0,0), rclcpp::Time(35,0)) ; + interval_data = cache.getSurroundingInterval(rclcpp::Time(0, 0), rclcpp::Time(35, 0)); ASSERT_EQ(interval_data.size(), (unsigned int) 4); EXPECT_EQ(interval_data[0]->data, 1); - interval_data = cache.getSurroundingInterval(rclcpp::Time(35,0), rclcpp::Time(35,0)) ; + interval_data = cache.getSurroundingInterval(rclcpp::Time(35, 0), rclcpp::Time(35, 0)); ASSERT_EQ(interval_data.size(), (unsigned int) 2); EXPECT_EQ(interval_data[0]->data, 3); EXPECT_EQ(interval_data[1]->data, 4); - interval_data = cache.getSurroundingInterval(rclcpp::Time(55,0), rclcpp::Time(55,0)) ; + interval_data = cache.getSurroundingInterval(rclcpp::Time(55, 0), rclcpp::Time(55, 0)); ASSERT_EQ(interval_data.size(), (unsigned int) 1); EXPECT_EQ(interval_data[0]->data, 5); } @@ -127,38 +131,42 @@ TEST(Cache, easySurroundingInterval) std::shared_ptr buildMsg(int32_t seconds, int data) { - Msg* msg = new Msg ; - msg->data = data ; - msg->header.stamp = rclcpp::Time(seconds, 0) ; + Msg * msg = new Msg; + msg->data = data; + msg->header.stamp = rclcpp::Time(seconds, 0); - std::shared_ptr msg_ptr(msg) ; - return msg_ptr ; + std::shared_ptr msg_ptr(msg); + return msg_ptr; } TEST(Cache, easyUnsorted) { message_filters::Cache cache(10); - cache.add(buildMsg(10, 1)) ; - cache.add(buildMsg(30, 3)) ; - cache.add(buildMsg(70, 7)) ; - cache.add(buildMsg( 5, 0)) ; - cache.add(buildMsg(20, 2)) ; + cache.add(buildMsg(10, 1)); + cache.add(buildMsg(30, 3)); + cache.add(buildMsg(70, 7)); + cache.add(buildMsg(5, 0)); + cache.add(buildMsg(20, 2)); - std::vector > interval_data = cache.getInterval(rclcpp::Time(3, 0), rclcpp::Time(15, 0)); + std::vector> interval_data = cache.getInterval( + rclcpp::Time( + 3, + 0), rclcpp::Time( + 15, 0)); - ASSERT_EQ(interval_data.size(), (unsigned int) 2) ; - EXPECT_EQ(interval_data[0]->data, 0) ; - EXPECT_EQ(interval_data[1]->data, 1) ; + ASSERT_EQ(interval_data.size(), (unsigned int) 2); + EXPECT_EQ(interval_data[0]->data, 0); + EXPECT_EQ(interval_data[1]->data, 1); // Grab all the data - interval_data = cache.getInterval(rclcpp::Time(0, 0), rclcpp::Time(80, 0)) ; - ASSERT_EQ(interval_data.size(), (unsigned int) 5) ; - EXPECT_EQ(interval_data[0]->data, 0) ; - EXPECT_EQ(interval_data[1]->data, 1) ; - EXPECT_EQ(interval_data[2]->data, 2) ; - EXPECT_EQ(interval_data[3]->data, 3) ; - EXPECT_EQ(interval_data[4]->data, 7) ; + interval_data = cache.getInterval(rclcpp::Time(0, 0), rclcpp::Time(80, 0)); + ASSERT_EQ(interval_data.size(), (unsigned int) 5); + EXPECT_EQ(interval_data[0]->data, 0); + EXPECT_EQ(interval_data[1]->data, 1); + EXPECT_EQ(interval_data[2]->data, 2); + EXPECT_EQ(interval_data[3]->data, 3); + EXPECT_EQ(interval_data[4]->data, 7); } @@ -167,19 +175,19 @@ TEST(Cache, easyElemBeforeAfter) message_filters::Cache cache(10); std::shared_ptr elem_ptr; - fillCacheEasy(cache, 5, 10) ; + fillCacheEasy(cache, 5, 10); - elem_ptr = cache.getElemAfterTime( rclcpp::Time(85, 0)) ; + elem_ptr = cache.getElemAfterTime(rclcpp::Time(85, 0)); - ASSERT_FALSE(!elem_ptr) ; - EXPECT_EQ(elem_ptr->data, 9) ; + ASSERT_FALSE(!elem_ptr); + EXPECT_EQ(elem_ptr->data, 9); - elem_ptr = cache.getElemBeforeTime( rclcpp::Time(85, 0)) ; - ASSERT_FALSE(!elem_ptr) ; - EXPECT_EQ(elem_ptr->data, 8) ; + elem_ptr = cache.getElemBeforeTime(rclcpp::Time(85, 0)); + ASSERT_FALSE(!elem_ptr); + EXPECT_EQ(elem_ptr->data, 8); - elem_ptr = cache.getElemBeforeTime( rclcpp::Time(45, 0)) ; - EXPECT_TRUE(!elem_ptr) ; + elem_ptr = cache.getElemBeforeTime(rclcpp::Time(45, 0)); + EXPECT_TRUE(!elem_ptr); } struct EventHelper @@ -207,10 +215,9 @@ TEST(Cache, eventInEventOut) EXPECT_EQ(h.event_.getMessage(), evt.getMessage()); } -int main(int argc, char **argv) +int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); rclcpp::init(argc, argv); return RUN_ALL_TESTS(); } - diff --git a/test/test_approximate_epsilon_time_policy.cpp b/test/test_approximate_epsilon_time_policy.cpp index c6b1269..a1f3d4b 100644 --- a/test/test_approximate_epsilon_time_policy.cpp +++ b/test/test_approximate_epsilon_time_policy.cpp @@ -26,6 +26,7 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. +#include #include #include #include @@ -58,7 +59,7 @@ namespace message_traits template<> struct TimeStamp { - static rclcpp::Time value(const Msg& m) + static rclcpp::Time value(const Msg & m) { return m.header.stamp; } @@ -84,17 +85,20 @@ struct TimeQuad class ApproximateEpsilonTimeSynchronizerTest { public: - - ApproximateEpsilonTimeSynchronizerTest(const std::vector &input, - const std::vector &output, - uint32_t queue_size, rclcpp::Duration epsilon) : - input_(input), output_(output), output_position_(0), sync_( + ApproximateEpsilonTimeSynchronizerTest( + const std::vector & input, + const std::vector & output, + uint32_t queue_size, rclcpp::Duration epsilon) + : input_(input), output_(output), output_position_(0), sync_( message_filters::sync_policies::ApproximateEpsilonTime{queue_size, epsilon}) { - sync_.registerCallback(std::bind(&ApproximateEpsilonTimeSynchronizerTest::callback, this, std::placeholders::_1, std::placeholders::_2)); + sync_.registerCallback( + std::bind( + &ApproximateEpsilonTimeSynchronizerTest::callback, this, + std::placeholders::_1, std::placeholders::_2)); } - void callback(const MsgConstPtr& p, const MsgConstPtr& q) + void callback(const MsgConstPtr & p, const MsgConstPtr & q) { ASSERT_TRUE(p); ASSERT_TRUE(q); @@ -106,18 +110,14 @@ class ApproximateEpsilonTimeSynchronizerTest void run() { - for (const auto & time_topic: input_) - { + for (const auto & time_topic: input_) { const rclcpp::Time & time = time_topic.first; const unsigned int & topic = time_topic.second; - if (topic == 0) - { + if (topic == 0) { MsgPtr p(std::make_shared()); p->header.stamp = time; sync_.add<0>(p); - } - else - { + } else { MsgPtr q(std::make_shared()); q->header.stamp = time; sync_.add<1>(q); @@ -130,7 +130,9 @@ class ApproximateEpsilonTimeSynchronizerTest const std::vector & input_; const std::vector & output_; unsigned int output_position_; - typedef message_filters::Synchronizer> Sync2; + typedef message_filters::Synchronizer> Sync2; + public: Sync2 sync_; }; @@ -146,15 +148,15 @@ TEST(ApproxTimeSync, ExactMatch) { rclcpp::Time t(0, 0, RCL_ROS_TIME); rclcpp::Duration s(1, 0); - input.push_back(TimeAndTopic(t,0)); // a - input.push_back(TimeAndTopic(t,1)); // A - input.push_back(TimeAndTopic(t+s*3,0)); // b - input.push_back(TimeAndTopic(t+s*3,1)); // B - input.push_back(TimeAndTopic(t+s*6,0)); // c - input.push_back(TimeAndTopic(t+s*6,1)); // C + input.push_back(TimeAndTopic(t, 0)); // a + input.push_back(TimeAndTopic(t, 1)); // A + input.push_back(TimeAndTopic(t + s * 3, 0)); // b + input.push_back(TimeAndTopic(t + s * 3, 1)); // B + input.push_back(TimeAndTopic(t + s * 6, 0)); // c + input.push_back(TimeAndTopic(t + s * 6, 1)); // C output.push_back(TimePair(t, t)); - output.push_back(TimePair(t+s*3, t+s*3)); - output.push_back(TimePair(t+s*6, t+s*6)); + output.push_back(TimePair(t + s * 3, t + s * 3)); + output.push_back(TimePair(t + s * 6, t + s * 6)); ApproximateEpsilonTimeSynchronizerTest sync_test( input, output, 10, rclcpp::Duration::from_seconds(0.5)); @@ -173,15 +175,15 @@ TEST(ApproxTimeSync, PerfectMatch) { rclcpp::Time t(0, 0, RCL_ROS_TIME); rclcpp::Duration s(1, 0); - input.push_back(TimeAndTopic(t,0)); // a - input.push_back(TimeAndTopic(t+s,1)); // A - input.push_back(TimeAndTopic(t+s*3,0)); // b - input.push_back(TimeAndTopic(t+s*4,1)); // B - input.push_back(TimeAndTopic(t+s*6,0)); // c - input.push_back(TimeAndTopic(t+s*7,1)); // C - output.push_back(TimePair(t, t+s)); - output.push_back(TimePair(t+s*3, t+s*4)); - output.push_back(TimePair(t+s*6, t+s*7)); + input.push_back(TimeAndTopic(t, 0)); // a + input.push_back(TimeAndTopic(t + s, 1)); // A + input.push_back(TimeAndTopic(t + s * 3, 0)); // b + input.push_back(TimeAndTopic(t + s * 4, 1)); // B + input.push_back(TimeAndTopic(t + s * 6, 0)); // c + input.push_back(TimeAndTopic(t + s * 7, 1)); // C + output.push_back(TimePair(t, t + s)); + output.push_back(TimePair(t + s * 3, t + s * 4)); + output.push_back(TimePair(t + s * 6, t + s * 7)); ApproximateEpsilonTimeSynchronizerTest sync_test( input, output, 10, rclcpp::Duration::from_seconds(1.5)); @@ -200,23 +202,23 @@ TEST(ApproxTimeSync, ImperfectMatch) { rclcpp::Time t(0, 0, RCL_ROS_TIME); rclcpp::Duration s(1, 0); - input.push_back(TimeAndTopic(t,0)); // a - input.push_back(TimeAndTopic(t+s,1)); // A - input.push_back(TimeAndTopic(t+s*2,0)); // x - input.push_back(TimeAndTopic(t+s*3,0)); // b - input.push_back(TimeAndTopic(t+s*4,1)); // B - input.push_back(TimeAndTopic(t+s*6,0)); // c - input.push_back(TimeAndTopic(t+s*7,1)); // C - output.push_back(TimePair(t, t+s)); - output.push_back(TimePair(t+s*3, t+s*4)); - output.push_back(TimePair(t+s*6, t+s*7)); + input.push_back(TimeAndTopic(t, 0)); // a + input.push_back(TimeAndTopic(t + s, 1)); // A + input.push_back(TimeAndTopic(t + s * 2, 0)); // x + input.push_back(TimeAndTopic(t + s * 3, 0)); // b + input.push_back(TimeAndTopic(t + s * 4, 1)); // B + input.push_back(TimeAndTopic(t + s * 6, 0)); // c + input.push_back(TimeAndTopic(t + s * 7, 1)); // C + output.push_back(TimePair(t, t + s)); + output.push_back(TimePair(t + s * 3, t + s * 4)); + output.push_back(TimePair(t + s * 6, t + s * 7)); ApproximateEpsilonTimeSynchronizerTest sync_test( input, output, 10, rclcpp::Duration::from_seconds(1.5)); sync_test.run(); } -int main(int argc, char **argv) +int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); rclcpp::init(argc, argv); diff --git a/test/test_approximate_time_policy.cpp b/test/test_approximate_time_policy.cpp index f5b5967..c6b1407 100644 --- a/test/test_approximate_time_policy.cpp +++ b/test/test_approximate_time_policy.cpp @@ -26,6 +26,7 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. +#include #include #include #include @@ -85,13 +86,16 @@ struct TimeQuad class ApproximateTimeSynchronizerTest { public: - - ApproximateTimeSynchronizerTest(const std::vector & input, - const std::vector & output, - uint32_t queue_size) : - input_(input), output_(output), output_position_(0), sync_(queue_size) + ApproximateTimeSynchronizerTest( + const std::vector & input, + const std::vector & output, + uint32_t queue_size) + : input_(input), output_(output), output_position_(0), sync_(queue_size) { - sync_.registerCallback(std::bind(&ApproximateTimeSynchronizerTest::callback, this, std::placeholders::_1, std::placeholders::_2)); + sync_.registerCallback( + std::bind( + &ApproximateTimeSynchronizerTest::callback, this, + std::placeholders::_1, std::placeholders::_2)); } void callback(const MsgConstPtr & p, const MsgConstPtr & q) @@ -124,7 +128,9 @@ class ApproximateTimeSynchronizerTest const std::vector & input_; const std::vector & output_; unsigned int output_position_; - typedef message_filters::Synchronizer> Sync2; + typedef message_filters::Synchronizer> Sync2; + public: Sync2 sync_; }; @@ -136,16 +142,22 @@ class ApproximateTimeSynchronizerTest class ApproximateTimeSynchronizerTestQuad { public: - - ApproximateTimeSynchronizerTestQuad(const std::vector & input, - const std::vector & output, - uint32_t queue_size) : - input_(input), output_(output), output_position_(0), sync_(queue_size) + ApproximateTimeSynchronizerTestQuad( + const std::vector & input, + const std::vector & output, + uint32_t queue_size) + : input_(input), output_(output), output_position_(0), sync_(queue_size) { - sync_.registerCallback(std::bind(&ApproximateTimeSynchronizerTestQuad::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4)); + sync_.registerCallback( + std::bind( + &ApproximateTimeSynchronizerTestQuad::callback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4)); } - void callback(const MsgConstPtr & p, const MsgConstPtr & q, const MsgConstPtr & r, const MsgConstPtr & s) + void callback( + const MsgConstPtr & p, const MsgConstPtr & q, const MsgConstPtr & r, + const MsgConstPtr & s) { ASSERT_TRUE(p); ASSERT_TRUE(q); @@ -164,8 +176,7 @@ class ApproximateTimeSynchronizerTestQuad for (size_t i = 0; i < input_.size(); i++) { MsgPtr p(std::make_shared()); p->header.stamp = input_[i].first; - switch (input_[i].second) - { + switch (input_[i].second) { case 0: sync_.add<0>(p); break; @@ -184,15 +195,16 @@ class ApproximateTimeSynchronizerTestQuad } private: - const std::vector &input_; - const std::vector &output_; + const std::vector & input_; + const std::vector & output_; unsigned int output_position_; - typedef message_filters::Synchronizer > Sync4; + typedef message_filters::Synchronizer> Sync4; + public: Sync4 sync_; }; - //---------------------------------------------------------- // Test Suite //---------------------------------------------------------- @@ -207,15 +219,15 @@ TEST(ApproxTimeSync, ExactMatch) { rclcpp::Time t(0, 0); rclcpp::Duration s(1, 0); - input.push_back(TimeAndTopic(t,0)); // a - input.push_back(TimeAndTopic(t,1)); // A - input.push_back(TimeAndTopic(t+s*3,0)); // b - input.push_back(TimeAndTopic(t+s*3,1)); // B - input.push_back(TimeAndTopic(t+s*6,0)); // c - input.push_back(TimeAndTopic(t+s*6,1)); // C + input.push_back(TimeAndTopic(t, 0)); // a + input.push_back(TimeAndTopic(t, 1)); // A + input.push_back(TimeAndTopic(t + s * 3, 0)); // b + input.push_back(TimeAndTopic(t + s * 3, 1)); // B + input.push_back(TimeAndTopic(t + s * 6, 0)); // c + input.push_back(TimeAndTopic(t + s * 6, 1)); // C output.push_back(TimePair(t, t)); - output.push_back(TimePair(t+s*3, t+s*3)); - output.push_back(TimePair(t+s*6, t+s*6)); + output.push_back(TimePair(t + s * 3, t + s * 3)); + output.push_back(TimePair(t + s * 6, t + s * 6)); ApproximateTimeSynchronizerTest sync_test(input, output, 10); sync_test.run(); @@ -233,14 +245,14 @@ TEST(ApproxTimeSync, PerfectMatch) { rclcpp::Time t(0, 0); rclcpp::Duration s(1, 0); - input.push_back(TimeAndTopic(t,0)); // a - input.push_back(TimeAndTopic(t+s,1)); // A - input.push_back(TimeAndTopic(t+s*3,0)); // b - input.push_back(TimeAndTopic(t+s*4,1)); // B - input.push_back(TimeAndTopic(t+s*6,0)); // c - input.push_back(TimeAndTopic(t+s*7,1)); // C - output.push_back(TimePair(t, t+s)); - output.push_back(TimePair(t+s*3, t+s*4)); + input.push_back(TimeAndTopic(t, 0)); // a + input.push_back(TimeAndTopic(t + s, 1)); // A + input.push_back(TimeAndTopic(t + s * 3, 0)); // b + input.push_back(TimeAndTopic(t + s * 4, 1)); // B + input.push_back(TimeAndTopic(t + s * 6, 0)); // c + input.push_back(TimeAndTopic(t + s * 7, 1)); // C + output.push_back(TimePair(t, t + s)); + output.push_back(TimePair(t + s * 3, t + s * 4)); ApproximateTimeSynchronizerTest sync_test(input, output, 10); sync_test.run(); @@ -258,15 +270,15 @@ TEST(ApproxTimeSync, ImperfectMatch) { rclcpp::Time t(0, 0); rclcpp::Duration s(1, 0); - input.push_back(TimeAndTopic(t,0)); // a - input.push_back(TimeAndTopic(t+s,1)); // A - input.push_back(TimeAndTopic(t+s*2,0)); // x - input.push_back(TimeAndTopic(t+s*3,0)); // b - input.push_back(TimeAndTopic(t+s*5,1)); // B - input.push_back(TimeAndTopic(t+s*6,0)); // c - input.push_back(TimeAndTopic(t+s*7,1)); // C - output.push_back(TimePair(t, t+s)); - output.push_back(TimePair(t+s*6, t+s*5)); + input.push_back(TimeAndTopic(t, 0)); // a + input.push_back(TimeAndTopic(t + s, 1)); // A + input.push_back(TimeAndTopic(t + s * 2, 0)); // x + input.push_back(TimeAndTopic(t + s * 3, 0)); // b + input.push_back(TimeAndTopic(t + s * 5, 1)); // B + input.push_back(TimeAndTopic(t + s * 6, 0)); // c + input.push_back(TimeAndTopic(t + s * 7, 1)); // C + output.push_back(TimePair(t, t + s)); + output.push_back(TimePair(t + s * 6, t + s * 5)); ApproximateTimeSynchronizerTest sync_test(input, output, 10); sync_test.run(); @@ -285,14 +297,14 @@ TEST(ApproxTimeSync, Acceleration) { rclcpp::Time t(0, 0); rclcpp::Duration s(1, 0); - input.push_back(TimeAndTopic(t,0)); // a - input.push_back(TimeAndTopic(t+s*7,1)); // A - input.push_back(TimeAndTopic(t+s*12,0)); // b - input.push_back(TimeAndTopic(t+s*15,1)); // B - input.push_back(TimeAndTopic(t+s*17,0)); // c - input.push_back(TimeAndTopic(t+s*18,1)); // C - output.push_back(TimePair(t+s*12, t+s*7)); - output.push_back(TimePair(t+s*17, t+s*18)); + input.push_back(TimeAndTopic(t, 0)); // a + input.push_back(TimeAndTopic(t + s * 7, 1)); // A + input.push_back(TimeAndTopic(t + s * 12, 0)); // b + input.push_back(TimeAndTopic(t + s * 15, 1)); // B + input.push_back(TimeAndTopic(t + s * 17, 0)); // c + input.push_back(TimeAndTopic(t + s * 18, 1)); // C + output.push_back(TimePair(t + s * 12, t + s * 7)); + output.push_back(TimePair(t + s * 17, t + s * 18)); ApproximateTimeSynchronizerTest sync_test(input, output, 10); sync_test.run(); @@ -312,18 +324,18 @@ TEST(ApproxTimeSync, DroppedMessages) { rclcpp::Time t(0, 0); rclcpp::Duration s(1, 0); - input.push_back(TimeAndTopic(t,0)); // a - input.push_back(TimeAndTopic(t+s,1)); // A - input.push_back(TimeAndTopic(t+s*3,1)); // B - input.push_back(TimeAndTopic(t+s*4,0)); // b - input.push_back(TimeAndTopic(t+s*7,1)); // C - input.push_back(TimeAndTopic(t+s*8,0)); // c - input.push_back(TimeAndTopic(t+s*10,0)); // d - input.push_back(TimeAndTopic(t+s*11,1)); // D - input.push_back(TimeAndTopic(t+s*13,0)); // e - input.push_back(TimeAndTopic(t+s*14,1)); // E - output.push_back(TimePair(t+s*4, t+s*3)); - output.push_back(TimePair(t+s*10, t+s*11)); + input.push_back(TimeAndTopic(t, 0)); // a + input.push_back(TimeAndTopic(t + s, 1)); // A + input.push_back(TimeAndTopic(t + s * 3, 1)); // B + input.push_back(TimeAndTopic(t + s * 4, 0)); // b + input.push_back(TimeAndTopic(t + s * 7, 1)); // C + input.push_back(TimeAndTopic(t + s * 8, 0)); // c + input.push_back(TimeAndTopic(t + s * 10, 0)); // d + input.push_back(TimeAndTopic(t + s * 11, 1)); // D + input.push_back(TimeAndTopic(t + s * 13, 0)); // e + input.push_back(TimeAndTopic(t + s * 14, 1)); // E + output.push_back(TimePair(t + s * 4, t + s * 3)); + output.push_back(TimePair(t + s * 10, t + s * 11)); ApproximateTimeSynchronizerTest sync_test(input, output, 1); sync_test.run(); @@ -335,10 +347,10 @@ TEST(ApproxTimeSync, DroppedMessages) { // Output: ....a..b...c.d. // ....A..B...C.D. std::vector output2; - output2.push_back(TimePair(t, t+s)); - output2.push_back(TimePair(t+s*4, t+s*3)); - output2.push_back(TimePair(t+s*8, t+s*7)); - output2.push_back(TimePair(t+s*10, t+s*11)); + output2.push_back(TimePair(t, t + s)); + output2.push_back(TimePair(t + s * 4, t + s * 3)); + output2.push_back(TimePair(t + s * 8, t + s * 7)); + output2.push_back(TimePair(t + s * 10, t + s * 11)); ApproximateTimeSynchronizerTest sync_test2(input, output2, 2); sync_test2.run(); @@ -395,12 +407,12 @@ TEST(ApproxTimeSync, DoublePublish) { rclcpp::Time t(0, 0); rclcpp::Duration s(1, 0); - input.push_back(TimeAndTopic(t,0)); // a - input.push_back(TimeAndTopic(t+s,1)); // A - input.push_back(TimeAndTopic(t+rclcpp::Duration(3, 0), 1)); // B - input.push_back(TimeAndTopic(t+rclcpp::Duration(3, 0), 0)); // b - output.push_back(TimePair(t, t+s)); - output.push_back(TimePair(t+rclcpp::Duration(3, 0), t+rclcpp::Duration(3, 0))); + input.push_back(TimeAndTopic(t, 0)); // a + input.push_back(TimeAndTopic(t + s, 1)); // A + input.push_back(TimeAndTopic(t + rclcpp::Duration(3, 0), 1)); // B + input.push_back(TimeAndTopic(t + rclcpp::Duration(3, 0), 0)); // b + output.push_back(TimePair(t, t + s)); + output.push_back(TimePair(t + rclcpp::Duration(3, 0), t + rclcpp::Duration(3, 0))); ApproximateTimeSynchronizerTest sync_test(input, output, 10); sync_test.run(); @@ -423,24 +435,30 @@ TEST(ApproxTimeSync, FourTopics) { rclcpp::Time t(0, 0); rclcpp::Duration s(1, 0); - input.push_back(TimeAndTopic(t,0)); // a - input.push_back(TimeAndTopic(t+s,1)); // b - input.push_back(TimeAndTopic(t+rclcpp::Duration(2, 0),2)); // c - input.push_back(TimeAndTopic(t+rclcpp::Duration(3, 0),3)); // d - input.push_back(TimeAndTopic(t+rclcpp::Duration(5, 0),0)); // e - input.push_back(TimeAndTopic(t+rclcpp::Duration(5, 0),3)); // f - input.push_back(TimeAndTopic(t+rclcpp::Duration(6, 0),1)); // g - input.push_back(TimeAndTopic(t+rclcpp::Duration(6, 0),2)); // h - input.push_back(TimeAndTopic(t+rclcpp::Duration(8, 0),0)); // i - input.push_back(TimeAndTopic(t+rclcpp::Duration(9, 0),1)); // j - input.push_back(TimeAndTopic(t+rclcpp::Duration(10, 0),2)); // k - input.push_back(TimeAndTopic(t+rclcpp::Duration(11, 0),3)); // l - input.push_back(TimeAndTopic(t+rclcpp::Duration(10, 0),0)); // m - input.push_back(TimeAndTopic(t+rclcpp::Duration(13, 0),0)); // n - input.push_back(TimeAndTopic(t+rclcpp::Duration(14, 0),1)); // o - output.push_back(TimeQuad(t, t+s, t+rclcpp::Duration(2, 0), t+rclcpp::Duration(3, 0))); - output.push_back(TimeQuad(t+rclcpp::Duration(5, 0), t+rclcpp::Duration(6, 0), t+rclcpp::Duration(6, 0), t+rclcpp::Duration(5, 0))); - output.push_back(TimeQuad(t+rclcpp::Duration(10, 0), t+rclcpp::Duration(9, 0), t+rclcpp::Duration(10, 0), t+rclcpp::Duration(11, 0))); + input.push_back(TimeAndTopic(t, 0)); // a + input.push_back(TimeAndTopic(t + s, 1)); // b + input.push_back(TimeAndTopic(t + rclcpp::Duration(2, 0), 2)); // c + input.push_back(TimeAndTopic(t + rclcpp::Duration(3, 0), 3)); // d + input.push_back(TimeAndTopic(t + rclcpp::Duration(5, 0), 0)); // e + input.push_back(TimeAndTopic(t + rclcpp::Duration(5, 0), 3)); // f + input.push_back(TimeAndTopic(t + rclcpp::Duration(6, 0), 1)); // g + input.push_back(TimeAndTopic(t + rclcpp::Duration(6, 0), 2)); // h + input.push_back(TimeAndTopic(t + rclcpp::Duration(8, 0), 0)); // i + input.push_back(TimeAndTopic(t + rclcpp::Duration(9, 0), 1)); // j + input.push_back(TimeAndTopic(t + rclcpp::Duration(10, 0), 2)); // k + input.push_back(TimeAndTopic(t + rclcpp::Duration(11, 0), 3)); // l + input.push_back(TimeAndTopic(t + rclcpp::Duration(10, 0), 0)); // m + input.push_back(TimeAndTopic(t + rclcpp::Duration(13, 0), 0)); // n + input.push_back(TimeAndTopic(t + rclcpp::Duration(14, 0), 1)); // o + output.push_back(TimeQuad(t, t + s, t + rclcpp::Duration(2, 0), t + rclcpp::Duration(3, 0))); + output.push_back( + TimeQuad( + t + rclcpp::Duration(5, 0), t + rclcpp::Duration(6, 0), + t + rclcpp::Duration(6, 0), t + rclcpp::Duration(5, 0))); + output.push_back( + TimeQuad( + t + rclcpp::Duration(10, 0), t + rclcpp::Duration(9, 0), + t + rclcpp::Duration(10, 0), t + rclcpp::Duration(11, 0))); ApproximateTimeSynchronizerTestQuad sync_test(input, output, 10); sync_test.run(); @@ -463,12 +481,12 @@ TEST(ApproxTimeSync, EarlyPublish) { rclcpp::Time t(0, 0); rclcpp::Duration s(1, 0); - input.push_back(TimeAndTopic(t,0)); // a - input.push_back(TimeAndTopic(t+s,1)); // b - input.push_back(TimeAndTopic(t+s*2,2)); // c - input.push_back(TimeAndTopic(t+s*3,3)); // d - input.push_back(TimeAndTopic(t+s*7,0)); // e - output.push_back(TimeQuad(t, t+s, t+s*2, t+s*3)); + input.push_back(TimeAndTopic(t, 0)); // a + input.push_back(TimeAndTopic(t + s, 1)); // b + input.push_back(TimeAndTopic(t + s * 2, 2)); // c + input.push_back(TimeAndTopic(t + s * 3, 3)); // d + input.push_back(TimeAndTopic(t + s * 7, 0)); // e + output.push_back(TimeQuad(t, t + s, t + s * 2, t + s * 3)); ApproximateTimeSynchronizerTestQuad sync_test(input, output, 10); sync_test.run(); @@ -487,17 +505,17 @@ TEST(ApproxTimeSync, RateBound) { rclcpp::Time t(0, 0); rclcpp::Duration s(1, 0); - input.push_back(TimeAndTopic(t,0)); // a - input.push_back(TimeAndTopic(t+s,1)); // A - input.push_back(TimeAndTopic(t+s*3,0)); // b - input.push_back(TimeAndTopic(t+s*4,1)); // B - input.push_back(TimeAndTopic(t+s*6,0)); // c - input.push_back(TimeAndTopic(t+s*7,1)); // C - output.push_back(TimePair(t, t+s)); - output.push_back(TimePair(t+s*3, t+s*4)); + input.push_back(TimeAndTopic(t, 0)); // a + input.push_back(TimeAndTopic(t + s, 1)); // A + input.push_back(TimeAndTopic(t + s * 3, 0)); // b + input.push_back(TimeAndTopic(t + s * 4, 1)); // B + input.push_back(TimeAndTopic(t + s * 6, 0)); // c + input.push_back(TimeAndTopic(t + s * 7, 1)); // C + output.push_back(TimePair(t, t + s)); + output.push_back(TimePair(t + s * 3, t + s * 4)); ApproximateTimeSynchronizerTest sync_test(input, output, 10); - sync_test.sync_.setInterMessageLowerBound(0, s*1.5); + sync_test.sync_.setInterMessageLowerBound(0, s * 1.5); sync_test.run(); // Rate bound A: 2 @@ -506,16 +524,16 @@ TEST(ApproxTimeSync, RateBound) { // Output: .a..b..c // .A..B..C - output.push_back(TimePair(t+s*6, t+s*7)); + output.push_back(TimePair(t + s * 6, t + s * 7)); ApproximateTimeSynchronizerTest sync_test2(input, output, 10); - sync_test2.sync_.setInterMessageLowerBound(0, s*2); + sync_test2.sync_.setInterMessageLowerBound(0, s * 2); sync_test2.run(); } -int main(int argc, char **argv) +int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); rclcpp::init(argc, argv); diff --git a/test/test_chain.cpp b/test/test_chain.cpp index c45e2a0..06c089c 100644 --- a/test/test_chain.cpp +++ b/test/test_chain.cpp @@ -54,13 +54,13 @@ class Helper int32_t count_; }; -typedef std::shared_ptr > PassThroughPtr; +typedef std::shared_ptr> PassThroughPtr; TEST(Chain, simple) { Helper h; message_filters::Chain c; - c.addFilter(std::make_shared >()); + c.addFilter(std::make_shared>()); c.registerCallback(std::bind(&Helper::cb, &h)); c.add(std::make_shared()); @@ -73,10 +73,10 @@ TEST(Chain, multipleFilters) { Helper h; message_filters::Chain c; - c.addFilter(std::make_shared >()); - c.addFilter(std::make_shared >()); - c.addFilter(std::make_shared >()); - c.addFilter(std::make_shared >()); + c.addFilter(std::make_shared>()); + c.addFilter(std::make_shared>()); + c.addFilter(std::make_shared>()); + c.addFilter(std::make_shared>()); c.registerCallback(std::bind(&Helper::cb, &h)); c.add(std::make_shared()); @@ -89,15 +89,15 @@ TEST(Chain, addingFilters) { Helper h; message_filters::Chain c; - c.addFilter(std::make_shared >()); - c.addFilter(std::make_shared >()); + c.addFilter(std::make_shared>()); + c.addFilter(std::make_shared>()); c.registerCallback(std::bind(&Helper::cb, &h)); c.add(std::make_shared()); EXPECT_EQ(h.count_, 1); - c.addFilter(std::make_shared >()); - c.addFilter(std::make_shared >()); + c.addFilter(std::make_shared>()); + c.addFilter(std::make_shared>()); c.add(std::make_shared()); EXPECT_EQ(h.count_, 2); @@ -107,7 +107,7 @@ TEST(Chain, inputFilter) { Helper h; message_filters::Chain c; - c.addFilter(std::make_shared >()); + c.addFilter(std::make_shared>()); c.registerCallback(std::bind(&Helper::cb, &h)); message_filters::PassThrough p; @@ -137,12 +137,12 @@ TEST(Chain, retrieveFilter) { message_filters::Chain c; - ASSERT_FALSE(c.getFilter >(0)); + ASSERT_FALSE(c.getFilter>(0)); - c.addFilter(std::make_shared >()); + c.addFilter(std::make_shared>()); - ASSERT_TRUE(c.getFilter >(0)); - ASSERT_FALSE(c.getFilter >(1)); + ASSERT_TRUE(c.getFilter>(0)); + ASSERT_FALSE(c.getFilter>(1)); } TEST(Chain, retrieveFilterThroughBaseClass) @@ -150,12 +150,12 @@ TEST(Chain, retrieveFilterThroughBaseClass) message_filters::Chain c; message_filters::ChainBase * cb = &c; - ASSERT_FALSE(cb->getFilter >(0)); + ASSERT_FALSE(cb->getFilter>(0)); - c.addFilter(std::make_shared >()); + c.addFilter(std::make_shared>()); - ASSERT_TRUE(cb->getFilter >(0)); - ASSERT_FALSE(cb->getFilter >(1)); + ASSERT_TRUE(cb->getFilter>(0)); + ASSERT_FALSE(cb->getFilter>(1)); } struct PTDerived : public message_filters::PassThrough @@ -167,15 +167,13 @@ TEST(Chain, retrieveBaseClass) { message_filters::Chain c; c.addFilter(std::make_shared()); - ASSERT_TRUE(c.getFilter >(0)); + ASSERT_TRUE(c.getFilter>(0)); ASSERT_TRUE(c.getFilter(0)); } -int main(int argc, char **argv) +int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); rclcpp::init(argc, argv); return RUN_ALL_TESTS(); } - - diff --git a/test/test_exact_time_policy.cpp b/test/test_exact_time_policy.cpp index 1905fff..8127e70 100644 --- a/test/test_exact_time_policy.cpp +++ b/test/test_exact_time_policy.cpp @@ -55,7 +55,7 @@ namespace message_traits template<> struct TimeStamp { - static rclcpp::Time value(const Msg& m) + static rclcpp::Time value(const Msg & m) { return m.header.stamp; } @@ -68,7 +68,7 @@ class Helper public: Helper() : count_(0) - , drop_count_(0) + , drop_count_(0) {} void cb() @@ -160,7 +160,9 @@ TEST(ExactTime, dropCallback) struct EventHelper { - void callback(const message_filters::MessageEvent & e1, const message_filters::MessageEvent & e2) + void callback( + const message_filters::MessageEvent & e1, + const message_filters::MessageEvent & e2) { e1_ = e1; e2_ = e2; @@ -186,11 +188,9 @@ TEST(ExactTime, eventInEventOut) ASSERT_EQ(h.e2_.getReceiptTime(), evt.getReceiptTime()); } -int main(int argc, char **argv) +int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); rclcpp::init(argc, argv); return RUN_ALL_TESTS(); } - - diff --git a/test/test_fuzz.cpp b/test/test_fuzz.cpp index 73fd569..49868b2 100644 --- a/test/test_fuzz.cpp +++ b/test/test_fuzz.cpp @@ -53,12 +53,12 @@ class Helper : count_(0) {} - void cb(const MsgConstPtr&) + void cb(const MsgConstPtr &) { ++count_; } - void cb2(const MsgConstPtr&, const MsgConstPtr&) + void cb2(const MsgConstPtr &, const MsgConstPtr &) { ++count_; } @@ -78,7 +78,9 @@ static void fuzz_msg(MsgPtr msg) TEST(TimeSequencer, fuzz_sequencer) { rclcpp::Node::SharedPtr node = std::make_shared("test_node"); - message_filters::TimeSequencer seq(rclcpp::Duration(0, 10000000), rclcpp::Duration(0, 1000000), 10, node); + message_filters::TimeSequencer seq(rclcpp::Duration(0, 10000000), rclcpp::Duration( + 0, 1000000), + 10, node); Helper h; seq.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); rclcpp::Clock ros_clock; @@ -145,7 +147,7 @@ TEST(Subscriber, fuzz_subscriber) rclcpp::spin_some(node); } -int main(int argc, char **argv) +int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/test/test_latest_time_policy.cpp b/test/test_latest_time_policy.cpp index 90ac312..f1f693b 100644 --- a/test/test_latest_time_policy.cpp +++ b/test/test_latest_time_policy.cpp @@ -29,6 +29,7 @@ #include #include +#include #include #include #include @@ -100,13 +101,15 @@ class LatestTimePolicy : public ::testing::Test param_client = std::make_shared(node); ASSERT_TRUE(param_client->wait_for_service(5s)); - sync.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1, - std::placeholders::_2, - std::placeholders::_3)); + sync.registerCallback( + std::bind( + &Helper::cb, &h, std::placeholders::_1, + std::placeholders::_2, + std::placeholders::_3)); p.reserve(12U); q.reserve(6U); r.reserve(3U); - for(std::size_t idx = 0U; idx < 12U; ++idx) { + for (std::size_t idx = 0U; idx < 12U; ++idx) { MsgPtr p_idx(std::make_shared()); p_idx->data = idx; p.push_back(p_idx); if (idx % 2U == 0U) { MsgPtr q_idx(std::make_shared()); q_idx->data = idx; q.push_back(q_idx); @@ -194,11 +197,11 @@ TEST_F(LatestTimePolicy, ChangeRateLeading) for (std::size_t idx = 0U; idx < 12U; ++idx) { if (idx % 2U == 0U) { - sync.add<1>(q[idx/2U]); + sync.add<1>(q[idx / 2U]); } if (idx % 4U == 0U) { - sync.add<2>(r[idx/4U]); + sync.add<2>(r[idx / 4U]); } if (idx < 4U) { @@ -267,7 +270,7 @@ TEST_F(LatestTimePolicy, ChangeRateTrailing) executor.add_node(node); for (std::size_t idx = 0U; idx < 12U; ++idx) { - if(idx % 2U == 1U) { + if (idx % 2U == 1U) { sync.add<1>(q[(idx - 1U) / 2U]); } @@ -331,7 +334,7 @@ TEST_F(LatestTimePolicy, ChangeRateTrailing) } } -int main(int argc, char **argv) +int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); rclcpp::init(argc, argv); diff --git a/test/test_simple.cpp b/test/test_simple.cpp index 841e965..287d271 100644 --- a/test/test_simple.cpp +++ b/test/test_simple.cpp @@ -65,7 +65,7 @@ class Helper ++counts_[0]; } - void cb1(const Msg&) + void cb1(const Msg &) { ++counts_[1]; } @@ -108,13 +108,19 @@ TEST(SimpleFilter, callbackTypes) Helper h; Filter f; f.registerCallback(std::bind(&Helper::cb0, &h, std::placeholders::_1)); - f.registerCallback(std::bind(&Helper::cb1, &h, std::placeholders::_1)); + f.registerCallback(std::bind(&Helper::cb1, &h, std::placeholders::_1)); f.registerCallback(std::bind(&Helper::cb2, &h, std::placeholders::_1)); - f.registerCallback&>(std::bind(&Helper::cb3, &h, std::placeholders::_1)); + f.registerCallback &>( + std::bind( + &Helper::cb3, &h, + std::placeholders::_1)); f.registerCallback(std::bind(&Helper::cb4, &h, std::placeholders::_1)); - f.registerCallback(std::bind(&Helper::cb5, &h, std::placeholders::_1)); + f.registerCallback(std::bind(&Helper::cb5, &h, std::placeholders::_1)); f.registerCallback(std::bind(&Helper::cb6, &h, std::placeholders::_1)); - f.registerCallback&>(std::bind(&Helper::cb7, &h, std::placeholders::_1)); + f.registerCallback &>( + std::bind( + &Helper::cb7, &h, + std::placeholders::_1)); f.add(Filter::EventType(std::make_shared())); EXPECT_EQ(h.counts_[0], 1); @@ -129,7 +135,7 @@ TEST(SimpleFilter, callbackTypes) struct OldFilter { - message_filters::Connection registerCallback(const std::function&) + message_filters::Connection registerCallback(const std::function &) { return message_filters::Connection(); } @@ -142,11 +148,9 @@ TEST(SimpleFilter, oldRegisterWithNewFilter) f.registerCallback(std::bind(&Helper::cb3, &h, std::placeholders::_1)); } -int main(int argc, char **argv){ +int main(int argc, char ** argv) +{ testing::InitGoogleTest(&argc, argv); rclcpp::init(argc, argv); return RUN_ALL_TESTS(); } - - - diff --git a/test/test_subscriber.cpp b/test/test_subscriber.cpp index 866e81f..8781701 100644 --- a/test/test_subscriber.cpp +++ b/test/test_subscriber.cpp @@ -98,7 +98,7 @@ TEST(Subscriber, subUnsubSub) auto node = std::make_shared("test_node"); Helper h; message_filters::Subscriber sub(node, "test_topic"); - sub.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); + sub.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); auto pub = node->create_publisher("test_topic", 10); sub.unsubscribe(); @@ -120,7 +120,7 @@ TEST(Subscriber, subUnsubSub_raw) auto node = std::make_shared("test_node"); Helper h; message_filters::Subscriber sub(node.get(), "test_topic"); - sub.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); + sub.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); auto pub = node->create_publisher("test_topic", 10); sub.unsubscribe(); @@ -142,7 +142,7 @@ TEST(Subscriber, switchRawAndShared) auto node = std::make_shared("test_node"); Helper h; message_filters::Subscriber sub(node, "test_topic"); - sub.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); + sub.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); auto pub = node->create_publisher("test_topic2", 10); sub.unsubscribe(); @@ -164,8 +164,8 @@ TEST(Subscriber, subInChain) auto node = std::make_shared("test_node"); Helper h; message_filters::Chain c; - c.addFilter(std::make_shared >(node, "test_topic")); - c.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); + c.addFilter(std::make_shared>(node, "test_topic")); + c.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); auto pub = node->create_publisher("test_topic", 10); rclcpp::Clock ros_clock; @@ -282,7 +282,7 @@ TEST(Subscriber, lifecycle) } -int main(int argc, char **argv) +int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/test/test_synchronizer.cpp b/test/test_synchronizer.cpp index 36d0546..578d943 100644 --- a/test/test_synchronizer.cpp +++ b/test/test_synchronizer.cpp @@ -49,8 +49,10 @@ typedef std::shared_ptr MsgPtr; typedef std::shared_ptr MsgConstPtr; -template +template struct NullPolicy : public message_filters::PolicyBase { typedef message_filters::Synchronizer Sync; @@ -67,7 +69,7 @@ struct NullPolicy : public message_filters::PolicyBase sync(f0, f1, f2, f3, f4, f5, f6, f7, f8); } -void function2(const MsgConstPtr&, const MsgConstPtr&) {} -void function3(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} -void function4(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} -void function5(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} -void function6(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} -void function7(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} -void function8(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} -void function9(const MsgConstPtr&, MsgConstPtr, const MsgPtr&, MsgPtr, const Msg&, Msg, const message_filters::MessageEvent&, const message_filters::MessageEvent&, const MsgConstPtr&) {} +void function2(const MsgConstPtr &, const MsgConstPtr &) {} +void function3(const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &) {} +void function4(const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &) +{ +} +void function5( + const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, + const MsgConstPtr &) {} +void function6( + const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, + const MsgConstPtr &, const MsgConstPtr &) {} +void function7( + const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, + const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &) {} +void function8( + const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, + const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &) {} +void function9( + const MsgConstPtr &, MsgConstPtr, const MsgPtr &, MsgPtr, const Msg &, Msg, + const message_filters::MessageEvent &, + const message_filters::MessageEvent &, const MsgConstPtr &) {} TEST(Synchronizer, compileFunction2) { @@ -195,13 +210,24 @@ TEST(Synchronizer, compileFunction9) struct MethodHelper { - void method2(const MsgConstPtr&, const MsgConstPtr&) {} - void method3(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} - void method4(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} - void method5(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} - void method6(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} - void method7(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} - void method8(const MsgConstPtr&, MsgConstPtr, const MsgPtr&, MsgPtr, const Msg&, Msg, const message_filters::MessageEvent&, const message_filters::MessageEvent&) {} + void method2(const MsgConstPtr &, const MsgConstPtr &) {} + void method3(const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &) {} + void method4(const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &) + { + } + void method5( + const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, + const MsgConstPtr &) {} + void method6( + const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, + const MsgConstPtr &, const MsgConstPtr &) {} + void method7( + const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, + const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &) {} + void method8( + const MsgConstPtr &, MsgConstPtr, const MsgPtr &, MsgPtr, const Msg &, Msg, + const message_filters::MessageEvent &, + const message_filters::MessageEvent &) {} // Can only do 8 here because the object instance counts as a parameter and bind only supports 9 }; @@ -442,10 +468,8 @@ TEST(Synchronizer, add9) ASSERT_EQ(sync.added_[8], 1); } -int main(int argc, char **argv) +int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } - - diff --git a/test/time_sequencer_unittest.cpp b/test/time_sequencer_unittest.cpp index 775e5a9..438c936 100644 --- a/test/time_sequencer_unittest.cpp +++ b/test/time_sequencer_unittest.cpp @@ -53,7 +53,7 @@ namespace message_traits template<> struct TimeStamp { - static rclcpp::Time value(const Msg& m) + static rclcpp::Time value(const Msg & m) { return m.header.stamp; } @@ -68,7 +68,7 @@ class Helper : count_(0) {} - void cb(const MsgConstPtr&) + void cb(const MsgConstPtr &) { ++count_; } @@ -79,7 +79,8 @@ class Helper TEST(TimeSequencer, simple) { rclcpp::Node::SharedPtr node = std::make_shared("test_node"); - message_filters::TimeSequencer seq(rclcpp::Duration(0, 250000000), rclcpp::Duration(0, 10000000), 10, node); + message_filters::TimeSequencer seq(rclcpp::Duration(0, 250000000), + rclcpp::Duration(0, 10000000), 10, node); Helper h; seq.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); MsgPtr msg(std::make_shared()); @@ -100,8 +101,11 @@ TEST(TimeSequencer, simple) TEST(TimeSequencer, compilation) { rclcpp::Node::SharedPtr node = std::make_shared("test_node"); - message_filters::TimeSequencer seq(rclcpp::Duration(1, 0), rclcpp::Duration(0, 10000000), 10, node); - message_filters::TimeSequencer seq2(rclcpp::Duration(1, 0), rclcpp::Duration(0, 10000000), 10, node); + message_filters::TimeSequencer seq(rclcpp::Duration(1, 0), rclcpp::Duration(0, 10000000), 10, + node); + message_filters::TimeSequencer seq2(rclcpp::Duration(1, 0), rclcpp::Duration(0, 10000000), + 10, + node); seq2.connectInput(seq); } @@ -119,12 +123,17 @@ struct EventHelper TEST(TimeSequencer, eventInEventOut) { rclcpp::Node::SharedPtr nh = std::make_shared("test_node"); - message_filters::TimeSequencer seq(rclcpp::Duration(1, 0), rclcpp::Duration(0, 10000000), 10, nh); - message_filters::TimeSequencer seq2(seq, rclcpp::Duration(1, 0), rclcpp::Duration(0, 10000000), 10, nh); + message_filters::TimeSequencer seq(rclcpp::Duration(1, 0), rclcpp::Duration(0, 10000000), 10, + nh); + message_filters::TimeSequencer seq2(seq, rclcpp::Duration(1, 0), rclcpp::Duration( + 0, + 10000000), + 10, nh); EventHelper h; seq2.registerCallback(&EventHelper::cb, &h); - message_filters::MessageEvent evt(std::make_shared(), rclcpp::Clock().now()); + message_filters::MessageEvent evt(std::make_shared(), + rclcpp::Clock().now()); seq.add(evt); while (!h.event_.getMessage()) { @@ -136,7 +145,7 @@ TEST(TimeSequencer, eventInEventOut) EXPECT_EQ(h.event_.getMessage(), evt.getMessage()); } -int main(int argc, char **argv) +int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/test/time_synchronizer_unittest.cpp b/test/time_synchronizer_unittest.cpp index f813487..66d715d 100644 --- a/test/time_synchronizer_unittest.cpp +++ b/test/time_synchronizer_unittest.cpp @@ -55,7 +55,7 @@ namespace message_traits template<> struct TimeStamp { - static rclcpp::Time value(const Msg& m) + static rclcpp::Time value(const Msg & m) { return m.header.stamp; } @@ -68,7 +68,7 @@ class Helper public: Helper() : count_(0) - , drop_count_(0) + , drop_count_(0) {} void cb() @@ -118,29 +118,45 @@ TEST(TimeSynchronizer, compile6) 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(f0, f1, f2, f3, f4, f5, + f6, 1); } 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(f0, f1, f2, f3, f4, + f5, f6, f7, 1); } 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); -} - -void function2(const MsgConstPtr&, const MsgConstPtr&) {} -void function3(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} -void function4(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} -void function5(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} -void function6(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} -void function7(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} -void function8(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} -void function9(const MsgConstPtr&, MsgConstPtr, const MsgPtr&, MsgPtr, const Msg&, Msg, const message_filters::MessageEvent&, const message_filters::MessageEvent&, const MsgConstPtr&) {} + message_filters::TimeSynchronizer sync(f0, f1, + f2, f3, f4, f5, f6, f7, f8, 1); +} + +void function2(const MsgConstPtr &, const MsgConstPtr &) {} +void function3(const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &) {} +void function4(const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &) +{ +} +void function5( + const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, + const MsgConstPtr &) {} +void function6( + const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, + const MsgConstPtr &, const MsgConstPtr &) {} +void function7( + const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, + const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &) {} +void function8( + const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, + const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &) {} +void function9( + const MsgConstPtr &, MsgConstPtr, const MsgPtr &, MsgPtr, const Msg &, Msg, + const message_filters::MessageEvent &, + const message_filters::MessageEvent &, const MsgConstPtr &) {} TEST(TimeSynchronizer, compileFunction2) { @@ -192,13 +208,24 @@ TEST(TimeSynchronizer, compileFunction9) struct MethodHelper { - void method2(const MsgConstPtr&, const MsgConstPtr&) {} - void method3(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} - void method4(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} - void method5(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} - void method6(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} - void method7(const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&, const MsgConstPtr&) {} - void method8(const MsgConstPtr&, MsgConstPtr, const MsgPtr&, MsgPtr, const Msg&, Msg, const message_filters::MessageEvent&, const message_filters::MessageEvent&) {} + void method2(const MsgConstPtr &, const MsgConstPtr &) {} + void method3(const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &) {} + void method4(const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &) + { + } + void method5( + const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, + const MsgConstPtr &) {} + void method6( + const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, + const MsgConstPtr &, const MsgConstPtr &) {} + void method7( + const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, + const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &) {} + void method8( + const MsgConstPtr &, MsgConstPtr, const MsgPtr &, MsgPtr, const Msg &, Msg, + const message_filters::MessageEvent &, + const message_filters::MessageEvent &) {} // Can only do 8 here because the object instance counts as a parameter and bind only supports 9 }; @@ -489,7 +516,9 @@ TEST(TimeSynchronizer, dropCallback) struct EventHelper { - void callback(const message_filters::MessageEvent & e1, const message_filters::MessageEvent & e2) + void callback( + const message_filters::MessageEvent & e1, + const message_filters::MessageEvent & e2) { e1_ = e1; e2_ = e2; @@ -530,12 +559,9 @@ TEST(TimeSynchronizer, connectConstructor) ASSERT_EQ(h.count_, 1); } -int main(int argc, char **argv) +int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); rclcpp::init(argc, argv); return RUN_ALL_TESTS(); } - - -