diff --git a/doc/Tutorials/Approximate-Synchronizer-Cpp.rst b/doc/Tutorials/Approximate-Synchronizer-Cpp.rst index b6abb65..4b4e9ec 100644 --- a/doc/Tutorials/Approximate-Synchronizer-Cpp.rst +++ b/doc/Tutorials/Approximate-Synchronizer-Cpp.rst @@ -18,9 +18,9 @@ If you have not done so already `create a workspace #include - #include "message_filters/subscriber.h" - #include "message_filters/synchronizer.h" - #include "message_filters/sync_policies/approximate_time.h" + #include "message_filters/subscriber.hpp" + #include "message_filters/synchronizer.hpp" + #include "message_filters/sync_policies/approximate_time.hpp" #include "sensor_msgs/msg/temperature.hpp" #include "sensor_msgs/msg/fluid_pressure.hpp" diff --git a/doc/Tutorials/Writing-A-Time-Synchronizer-Cpp.rst b/doc/Tutorials/Writing-A-Time-Synchronizer-Cpp.rst index 8a98e35..c2a527a 100644 --- a/doc/Tutorials/Writing-A-Time-Synchronizer-Cpp.rst +++ b/doc/Tutorials/Writing-A-Time-Synchronizer-Cpp.rst @@ -19,8 +19,8 @@ If you have not done so already `create a workspace #include - #include "message_filters/subscriber.h" - #include "message_filters/time_synchronizer.h" + #include "message_filters/subscriber.hpp" + #include "message_filters/time_synchronizer.hpp" #include "sensor_msgs/msg/temperature.hpp" #include "sensor_msgs/msg/fluid_pressure.hpp" diff --git a/include/message_filters/cache.h b/include/message_filters/cache.h index 24f3fcc..4cbac1d 100644 --- a/include/message_filters/cache.h +++ b/include/message_filters/cache.h @@ -1,342 +1,22 @@ -// Copyright 2008, Willow Garage, Inc. All rights reserved. +// Copyright 2024 Open Source Robotics Foundation, Inc. All rights reserved. // -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. +// http://www.apache.org/licenses/LICENSE-2.0 // -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the Willow Garage nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef MESSAGE_FILTERS__CACHE_H_ #define MESSAGE_FILTERS__CACHE_H_ -#include -#include -#include -#include -#include - -#include - -#include "message_filters/connection.h" -#include "message_filters/simple_filter.h" -#include "message_filters/message_traits.h" - -namespace message_filters -{ -/** - * \brief Stores a time history of messages - * - * Given a stream of messages, the most recent N messages are cached in a ring buffer, - * from which time intervals of the cache can then be retrieved by the client. - * - * Cache immediately passes messages through to its output connections. - * - * \section connections CONNECTIONS - * - * Cache's input and output connections are both of the same signature as rclcpp subscription callbacks, ie. -\verbatim -void callback(const std::shared_ptr &); -\endverbatim - */ -template -class Cache : public SimpleFilter -{ -public: - typedef std::shared_ptr MConstPtr; - typedef MessageEvent EventType; - - template - explicit Cache(F & f, unsigned int cache_size = 1) - { - setCacheSize(cache_size); - connectInput(f); - } - - /** - * Initializes a Message Cache without specifying a parent filter. This implies that in - * order to populate the cache, the user then has to call add themselves, or connectInput() is - * called later - */ - explicit Cache(unsigned int cache_size = 1) - { - setCacheSize(cache_size); - } - - template - void connectInput(F & f) - { - incoming_connection_ = f.registerCallback( - typename SimpleFilter::EventCallback( - std::bind(&Cache::callback, this, std::placeholders::_1))); - } - - ~Cache() - { - incoming_connection_.disconnect(); - } - - /** - * Set the size of the cache. - * \param cache_size The new size the cache should be. Must be > 0 - */ - void setCacheSize(unsigned int cache_size) - { - if (cache_size == 0) { - // ROS_ERROR("Cannot set max_size to 0"); - return; - } - - 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) - { - add(EventType(msg)); - } - - /** - * \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) - { - namespace mt = message_filters::message_traits; - - // printf(" Cache Size: %u\n", cache_.size()); - { - std::lock_guard lock(cache_lock_); - - // 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 - - 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 - rclcpp::Time evt_stamp = mt::TimeStamp::value(*evt.getMessage()); - 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); - } - - /** - * \brief Receive a vector of messages that occur between a start and end time (inclusive). - * - * This call is non-blocking, and only aggregates messages it has already received. - * It will not wait for messages have not yet been received, but occur in the interval. - * \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 - { - 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) - { - 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) - { - end_index++; - } - - std::vector 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; - } - - - /** - * \brief Retrieve the smallest interval of messages that surrounds an interval from start to end. - * - * 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 - { - 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) - { - start_index--; - } - - int end_index = start_index; - 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()); - } - - return interval_elems; - } - - /** - * \brief Grab the newest element that occurs right before the specified time. - * \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 - { - namespace mt = message_filters::message_traits; - - std::lock_guard lock(cache_lock_); - - MConstPtr out; - - 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(); - } - - return out; - } - - /** - * \brief Grab the oldest element that occurs right after the specified time. - * \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 - { - namespace mt = message_filters::message_traits; - - std::lock_guard lock(cache_lock_); - - 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--; - } - - if (elem_index >= 0) { - out = cache_[elem_index].getMessage(); - } else { - out.reset(); - } - - return out; - } - - /** - * \brief Returns the timestamp associated with the newest packet cache - */ - rclcpp::Time getLatestTime() const - { - namespace mt = message_filters::message_traits; - - std::lock_guard lock(cache_lock_); - - rclcpp::Time latest_time; - - if (cache_.size() > 0) { - latest_time = mt::TimeStamp::value(*cache_.back().getMessage()); - } - - return latest_time; - } - - /** - * \brief Returns the timestamp associated with the oldest packet cache - */ - rclcpp::Time getOldestTime() const - { - namespace mt = message_filters::message_traits; - - std::lock_guard lock(cache_lock_); - - rclcpp::Time oldest_time; - - if (cache_.size() > 0) { - oldest_time = mt::TimeStamp::value(*cache_.front().getMessage()); - } - - return oldest_time; - } - -private: - 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. +#warning This header is obsolete, please include message_filters/cache.hpp instead - Connection incoming_connection_; -}; -} // namespace message_filters +#include #endif // MESSAGE_FILTERS__CACHE_H_ diff --git a/include/message_filters/cache.hpp b/include/message_filters/cache.hpp new file mode 100644 index 0000000..e7ec325 --- /dev/null +++ b/include/message_filters/cache.hpp @@ -0,0 +1,342 @@ +// Copyright 2008, Willow Garage, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef MESSAGE_FILTERS__CACHE_HPP_ +#define MESSAGE_FILTERS__CACHE_HPP_ + +#include +#include +#include +#include +#include + +#include + +#include "message_filters/connection.hpp" +#include "message_filters/simple_filter.hpp" +#include "message_filters/message_traits.hpp" + +namespace message_filters +{ +/** + * \brief Stores a time history of messages + * + * Given a stream of messages, the most recent N messages are cached in a ring buffer, + * from which time intervals of the cache can then be retrieved by the client. + * + * Cache immediately passes messages through to its output connections. + * + * \section connections CONNECTIONS + * + * Cache's input and output connections are both of the same signature as rclcpp subscription callbacks, ie. +\verbatim +void callback(const std::shared_ptr &); +\endverbatim + */ +template +class Cache : public SimpleFilter +{ +public: + typedef std::shared_ptr MConstPtr; + typedef MessageEvent EventType; + + template + explicit Cache(F & f, unsigned int cache_size = 1) + { + setCacheSize(cache_size); + connectInput(f); + } + + /** + * Initializes a Message Cache without specifying a parent filter. This implies that in + * order to populate the cache, the user then has to call add themselves, or connectInput() is + * called later + */ + explicit Cache(unsigned int cache_size = 1) + { + setCacheSize(cache_size); + } + + template + void connectInput(F & f) + { + incoming_connection_ = f.registerCallback( + typename SimpleFilter::EventCallback( + std::bind(&Cache::callback, this, std::placeholders::_1))); + } + + ~Cache() + { + incoming_connection_.disconnect(); + } + + /** + * Set the size of the cache. + * \param cache_size The new size the cache should be. Must be > 0 + */ + void setCacheSize(unsigned int cache_size) + { + if (cache_size == 0) { + // ROS_ERROR("Cannot set max_size to 0"); + return; + } + + 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) + { + add(EventType(msg)); + } + + /** + * \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) + { + namespace mt = message_filters::message_traits; + + // printf(" Cache Size: %u\n", cache_.size()); + { + std::lock_guard lock(cache_lock_); + + // 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 + + 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 + rclcpp::Time evt_stamp = mt::TimeStamp::value(*evt.getMessage()); + 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); + } + + /** + * \brief Receive a vector of messages that occur between a start and end time (inclusive). + * + * This call is non-blocking, and only aggregates messages it has already received. + * It will not wait for messages have not yet been received, but occur in the interval. + * \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 + { + 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) + { + 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) + { + end_index++; + } + + std::vector 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; + } + + + /** + * \brief Retrieve the smallest interval of messages that surrounds an interval from start to end. + * + * 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 + { + 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) + { + start_index--; + } + + int end_index = start_index; + 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()); + } + + return interval_elems; + } + + /** + * \brief Grab the newest element that occurs right before the specified time. + * \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 + { + namespace mt = message_filters::message_traits; + + std::lock_guard lock(cache_lock_); + + MConstPtr out; + + 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(); + } + + return out; + } + + /** + * \brief Grab the oldest element that occurs right after the specified time. + * \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 + { + namespace mt = message_filters::message_traits; + + std::lock_guard lock(cache_lock_); + + 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--; + } + + if (elem_index >= 0) { + out = cache_[elem_index].getMessage(); + } else { + out.reset(); + } + + return out; + } + + /** + * \brief Returns the timestamp associated with the newest packet cache + */ + rclcpp::Time getLatestTime() const + { + namespace mt = message_filters::message_traits; + + std::lock_guard lock(cache_lock_); + + rclcpp::Time latest_time; + + if (cache_.size() > 0) { + latest_time = mt::TimeStamp::value(*cache_.back().getMessage()); + } + + return latest_time; + } + + /** + * \brief Returns the timestamp associated with the oldest packet cache + */ + rclcpp::Time getOldestTime() const + { + namespace mt = message_filters::message_traits; + + std::lock_guard lock(cache_lock_); + + rclcpp::Time oldest_time; + + if (cache_.size() > 0) { + oldest_time = mt::TimeStamp::value(*cache_.front().getMessage()); + } + + return oldest_time; + } + +private: + 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. + + Connection incoming_connection_; +}; +} // namespace message_filters + +#endif // MESSAGE_FILTERS__CACHE_HPP_ diff --git a/include/message_filters/chain.h b/include/message_filters/chain.h index 67e472b..0bf3a0e 100644 --- a/include/message_filters/chain.h +++ b/include/message_filters/chain.h @@ -1,249 +1,22 @@ -// Copyright 2010, Willow Garage, Inc. All rights reserved. +// Copyright 2024 Open Source Robotics Foundation, Inc. All rights reserved. // -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. +// http://www.apache.org/licenses/LICENSE-2.0 // -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the Willow Garage nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef MESSAGE_FILTERS__CHAIN_H_ #define MESSAGE_FILTERS__CHAIN_H_ -#include -#include - -#include "message_filters/simple_filter.h" -#include "message_filters/pass_through.h" - -namespace message_filters -{ -/** - * \brief Base class for Chain, allows you to store multiple chains in the same container. Provides filter retrieval - * by index. - */ -class ChainBase -{ -public: - virtual ~ChainBase() {} - - /** - * \brief Retrieve a filter from this chain by index. Returns an empty shared_ptr if the index is greater than - * the size of the chain. \b NOT type-safe - * - * \param F [template] The type of the filter - * \param index The index of the filter (returned by addFilter()) - */ - template - std::shared_ptr getFilter(size_t index) const - { - std::shared_ptr filter = getFilterForIndex(index); - if (filter) { - return std::static_pointer_cast(filter); - } - - return std::shared_ptr(); - } - -protected: - virtual std::shared_ptr getFilterForIndex(size_t index) const = 0; -}; -typedef std::shared_ptr ChainBasePtr; - -/** - * \brief Chains a dynamic number of simple filters together. Allows retrieval of filters by index after they are added. - * - * The Chain filter provides a container for simple filters. It allows you to store an N-long set of filters inside a single - * structure, making it much easier to manage them. - * - * Adding filters to the chain is done by adding shared_ptrs of them to the filter. They are automatically connected to each other - * and the output of the last filter in the chain is forwarded to the callback you've registered with Chain::registerCallback - * - * Example: -\verbatim -void myCallback(const MsgConstPtr & msg) -{ -} - -Chain c; -c.addFilter(std::make_shared >()); -c.addFilter(std::make_shared >()); -c.registerCallback(myCallback); -\endverbatim - - * - * It is also possible to pass bare pointers in, which will not be automatically deleted when Chain is destructed: -\verbatim -Chain c; -PassThrough p; -c.addFilter( &p); -c.registerCallback(myCallback); -\endverbatim - * - */ -template -class Chain : public ChainBase, public SimpleFilter -{ -public: - typedef std::shared_ptr MConstPtr; - typedef MessageEvent EventType; - - /** - * \brief Default constructor - */ - Chain() - { - } - - /** - * \brief Constructor with filter. Calls connectInput(f) - */ - template - explicit Chain(F & f) - { - connectInput(f); - } - - struct NullDeleter - { - void operator()(void const *) const - { - } - }; - - /** - * \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) - { - std::shared_ptr ptr(filter, NullDeleter()); - return addFilter(ptr); - } - - /** - * \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) - { - FilterInfo info; - info.add_func = std::bind( - (void (F::*)(const EventType &)) & F::add, filter.get(), std::placeholders::_1); - info.filter = filter; - 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()) { - filter->connectInput(*filters_.back().passthrough); - } - - size_t count = filters_.size(); - filters_.push_back(info); - return count; - } - - /** - * \brief Retrieve a filter from this chain by index. Returns an empty shared_ptr if the index is greater than - * the size of the chain. \b NOT type-safe - * - * \param F [template] The type of the filter - * \param index The index of the filter (returned by addFilter()) - */ - template - std::shared_ptr getFilter(size_t index) const - { - if (index >= filters_.size()) { - return std::shared_ptr(); - } - - return std::static_pointer_cast(filters_[index].filter); - } - - /** - * \brief Connect this filter's input to another filter's output. - */ - template - void connectInput(F & f) - { - incoming_connection_.disconnect(); - 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) - { - add(EventType(msg)); - } - - void add(const EventType & evt) - { - if (!filters_.empty()) { - filters_[0].add_func(evt); - } - } - -protected: - virtual std::shared_ptr getFilterForIndex(size_t index) const - { - if (index >= filters_.size()) { - return std::shared_ptr(); - } - - return filters_[index].filter; - } - -private: - void incomingCB(const EventType & evt) - { - add(evt); - } - - void lastFilterCB(const EventType & evt) - { - this->signalMessage(evt); - } - - struct FilterInfo - { - std::function add_func; - std::shared_ptr filter; - std::shared_ptr> passthrough; - }; - typedef std::vector V_FilterInfo; - - V_FilterInfo filters_; +#warning This header is obsolete, please include message_filters/chain.hpp instead - Connection incoming_connection_; - Connection last_filter_connection_; -}; -} // namespace message_filters +#include #endif // MESSAGE_FILTERS__CHAIN_H_ diff --git a/include/message_filters/chain.hpp b/include/message_filters/chain.hpp new file mode 100644 index 0000000..c3e227f --- /dev/null +++ b/include/message_filters/chain.hpp @@ -0,0 +1,249 @@ +// Copyright 2010, Willow Garage, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef MESSAGE_FILTERS__CHAIN_HPP_ +#define MESSAGE_FILTERS__CHAIN_HPP_ + +#include +#include + +#include "message_filters/simple_filter.hpp" +#include "message_filters/pass_through.hpp" + +namespace message_filters +{ +/** + * \brief Base class for Chain, allows you to store multiple chains in the same container. Provides filter retrieval + * by index. + */ +class ChainBase +{ +public: + virtual ~ChainBase() {} + + /** + * \brief Retrieve a filter from this chain by index. Returns an empty shared_ptr if the index is greater than + * the size of the chain. \b NOT type-safe + * + * \param F [template] The type of the filter + * \param index The index of the filter (returned by addFilter()) + */ + template + std::shared_ptr getFilter(size_t index) const + { + std::shared_ptr filter = getFilterForIndex(index); + if (filter) { + return std::static_pointer_cast(filter); + } + + return std::shared_ptr(); + } + +protected: + virtual std::shared_ptr getFilterForIndex(size_t index) const = 0; +}; +typedef std::shared_ptr ChainBasePtr; + +/** + * \brief Chains a dynamic number of simple filters together. Allows retrieval of filters by index after they are added. + * + * The Chain filter provides a container for simple filters. It allows you to store an N-long set of filters inside a single + * structure, making it much easier to manage them. + * + * Adding filters to the chain is done by adding shared_ptrs of them to the filter. They are automatically connected to each other + * and the output of the last filter in the chain is forwarded to the callback you've registered with Chain::registerCallback + * + * Example: +\verbatim +void myCallback(const MsgConstPtr & msg) +{ +} + +Chain c; +c.addFilter(std::make_shared >()); +c.addFilter(std::make_shared >()); +c.registerCallback(myCallback); +\endverbatim + + * + * It is also possible to pass bare pointers in, which will not be automatically deleted when Chain is destructed: +\verbatim +Chain c; +PassThrough p; +c.addFilter( &p); +c.registerCallback(myCallback); +\endverbatim + * + */ +template +class Chain : public ChainBase, public SimpleFilter +{ +public: + typedef std::shared_ptr MConstPtr; + typedef MessageEvent EventType; + + /** + * \brief Default constructor + */ + Chain() + { + } + + /** + * \brief Constructor with filter. Calls connectInput(f) + */ + template + explicit Chain(F & f) + { + connectInput(f); + } + + struct NullDeleter + { + void operator()(void const *) const + { + } + }; + + /** + * \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) + { + std::shared_ptr ptr(filter, NullDeleter()); + return addFilter(ptr); + } + + /** + * \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) + { + FilterInfo info; + info.add_func = std::bind( + (void (F::*)(const EventType &)) & F::add, filter.get(), std::placeholders::_1); + info.filter = filter; + 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()) { + filter->connectInput(*filters_.back().passthrough); + } + + size_t count = filters_.size(); + filters_.push_back(info); + return count; + } + + /** + * \brief Retrieve a filter from this chain by index. Returns an empty shared_ptr if the index is greater than + * the size of the chain. \b NOT type-safe + * + * \param F [template] The type of the filter + * \param index The index of the filter (returned by addFilter()) + */ + template + std::shared_ptr getFilter(size_t index) const + { + if (index >= filters_.size()) { + return std::shared_ptr(); + } + + return std::static_pointer_cast(filters_[index].filter); + } + + /** + * \brief Connect this filter's input to another filter's output. + */ + template + void connectInput(F & f) + { + incoming_connection_.disconnect(); + 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) + { + add(EventType(msg)); + } + + void add(const EventType & evt) + { + if (!filters_.empty()) { + filters_[0].add_func(evt); + } + } + +protected: + virtual std::shared_ptr getFilterForIndex(size_t index) const + { + if (index >= filters_.size()) { + return std::shared_ptr(); + } + + return filters_[index].filter; + } + +private: + void incomingCB(const EventType & evt) + { + add(evt); + } + + void lastFilterCB(const EventType & evt) + { + this->signalMessage(evt); + } + + struct FilterInfo + { + std::function add_func; + std::shared_ptr filter; + std::shared_ptr> passthrough; + }; + typedef std::vector V_FilterInfo; + + V_FilterInfo filters_; + + Connection incoming_connection_; + Connection last_filter_connection_; +}; +} // namespace message_filters + +#endif // MESSAGE_FILTERS__CHAIN_HPP_ diff --git a/include/message_filters/connection.h b/include/message_filters/connection.h index 0c47a12..d3bc808 100644 --- a/include/message_filters/connection.h +++ b/include/message_filters/connection.h @@ -1,72 +1,22 @@ -// Copyright 2009, Willow Garage, Inc. All rights reserved. +// Copyright 2024 Open Source Robotics Foundation, Inc. All rights reserved. // -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. +// http://www.apache.org/licenses/LICENSE-2.0 // -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the Willow Garage nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef MESSAGE_FILTERS__CONNECTION_H_ #define MESSAGE_FILTERS__CONNECTION_H_ -#include -#include - -#include "message_filters/visibility_control.h" - -namespace message_filters -{ - -class noncopyable -{ -protected: - noncopyable() {} - ~noncopyable() {} - noncopyable(const noncopyable &) = delete; - noncopyable & operator=(const noncopyable &) = delete; -}; - -/** - * \brief Encapsulates a connection from one filter to another (or to a user-specified callback) - */ -class Connection -{ -public: - using VoidDisconnectFunction = std::function; - using WithConnectionDisconnectFunction = std::function; - MESSAGE_FILTERS_PUBLIC Connection() {} - MESSAGE_FILTERS_PUBLIC Connection(const VoidDisconnectFunction & func); - - /** - * \brief disconnects this connection - */ - MESSAGE_FILTERS_PUBLIC void disconnect(); - -private: - VoidDisconnectFunction void_disconnect_; - WithConnectionDisconnectFunction connection_disconnect_; -}; +#warning This header is obsolete, please include message_filters/connection.hpp instead -} // namespace message_filters +#include #endif // MESSAGE_FILTERS__CONNECTION_H_ diff --git a/include/message_filters/connection.hpp b/include/message_filters/connection.hpp new file mode 100644 index 0000000..d9a1496 --- /dev/null +++ b/include/message_filters/connection.hpp @@ -0,0 +1,72 @@ +// Copyright 2009, Willow Garage, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef MESSAGE_FILTERS__CONNECTION_HPP_ +#define MESSAGE_FILTERS__CONNECTION_HPP_ + +#include +#include + +#include "message_filters/visibility_control.hpp" + +namespace message_filters +{ + +class noncopyable +{ +protected: + noncopyable() {} + ~noncopyable() {} + noncopyable(const noncopyable &) = delete; + noncopyable & operator=(const noncopyable &) = delete; +}; + +/** + * \brief Encapsulates a connection from one filter to another (or to a user-specified callback) + */ +class Connection +{ +public: + using VoidDisconnectFunction = std::function; + using WithConnectionDisconnectFunction = std::function; + MESSAGE_FILTERS_PUBLIC Connection() {} + MESSAGE_FILTERS_PUBLIC Connection(const VoidDisconnectFunction & func); + + /** + * \brief disconnects this connection + */ + MESSAGE_FILTERS_PUBLIC void disconnect(); + +private: + VoidDisconnectFunction void_disconnect_; + WithConnectionDisconnectFunction connection_disconnect_; +}; + +} // namespace message_filters + +#endif // MESSAGE_FILTERS__CONNECTION_HPP_ diff --git a/include/message_filters/message_event.h b/include/message_filters/message_event.h index ecb5005..72077ac 100644 --- a/include/message_filters/message_event.h +++ b/include/message_filters/message_event.h @@ -1,247 +1,22 @@ -// Copyright 2010, Willow Garage, Inc. All rights reserved. +// Copyright 2024 Open Source Robotics Foundation, Inc. All rights reserved. // -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. +// http://www.apache.org/licenses/LICENSE-2.0 // -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the Willow Garage nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. - -// File imported from -// https://github.com/ros/roscpp_core/blob/38b9663/roscpp_traits/include/ros/message_event.h +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef MESSAGE_FILTERS__MESSAGE_EVENT_H_ #define MESSAGE_FILTERS__MESSAGE_EVENT_H_ -#include -#include -#include -#include -#include - -#include - -namespace message_filters -{ -typedef std::map M_string; -typedef std::shared_ptr M_stringPtr; - -template -struct DefaultMessageCreator -{ - std::shared_ptr operator()() - { - return std::make_shared(); - } -}; - - -/** - * \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 - */ -template -class MessageEvent -{ -public: - typedef typename std::add_const::type ConstMessage; - typedef typename std::remove_const::type Message; - typedef std::shared_ptr MessagePtr; - typedef std::shared_ptr ConstMessagePtr; - typedef std::function CreateFunction; - - MessageEvent() - : nonconst_need_copy_(true) - {} - - MessageEvent(const MessageEvent & rhs) - { - *this = rhs; - } - - MessageEvent(const MessageEvent & rhs) - { - *this = rhs; - } - - MessageEvent(const MessageEvent & rhs, bool nonconst_need_copy) - { - *this = rhs; - nonconst_need_copy_ = 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) - { - 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) // NOLINT(runtime/explicit) - { - init(message, rclcpp::Clock().now(), true, message_filters::DefaultMessageCreator()); - } - - 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) - { - init(message, receipt_time, nonconst_need_copy, create); - } - - void init( - const ConstMessagePtr & message, rclcpp::Time receipt_time, bool nonconst_need_copy, - const CreateFunction & create) - { - message_ = message; - receipt_time_ = receipt_time; - nonconst_need_copy_ = nonconst_need_copy; - create_ = create; - } - - void operator=(const MessageEvent & rhs) - { - init( - std::static_pointer_cast(rhs.getMessage()), - rhs.getReceiptTime(), rhs.nonConstWillCopy(), rhs.getMessageFactory()); - message_copy_.reset(); - } - - void operator=(const MessageEvent & rhs) - { - init( - std::const_pointer_cast( - std::static_pointer_cast( - rhs.getMessage())), rhs.getReceiptTime(), rhs.nonConstWillCopy(), - rhs.getMessageFactory()); - message_copy_.reset(); - } - - /** - * \brief Retrieve the message. If M is const, this returns a reference to it. If M is non const - * and this event requires it, returns a copy. Note that it caches this copy for later use, so it will - * only every make the copy once - */ - std::shared_ptr getMessage() const - { - return copyMessageIfNecessary(); - } - - /** - * \brief Retrieve a const version of the 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_;} - - bool nonConstWillCopy() const {return nonconst_need_copy_;} - bool getMessageWillCopy() const {return !std::is_const::value && nonconst_need_copy_;} - - bool operator<(const MessageEvent & rhs) - { - if (message_ != rhs.message_) { - return message_ < rhs.message_; - } - - 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) - { - return message_ == rhs.message_ && receipt_time_ == rhs.receipt_time_ && - nonconst_need_copy_ == rhs.nonconst_need_copy_; - } - - bool operator!=(const MessageEvent & rhs) - { - return !(*this == rhs); - } - - const CreateFunction & getMessageFactory() const {return create_;} - -private: - template - typename std::enable_if::value, - std::shared_ptr>::type copyMessageIfNecessary() const - { - if (std::is_const::value || !nonconst_need_copy_) { - return std::const_pointer_cast(message_); - } - - if (message_copy_) { - return message_copy_; - } - - assert(create_); - message_copy_ = create_(); - *message_copy_ = *message_; - - return message_copy_; - } - - template - typename std::enable_if::value, - std::shared_ptr>::type copyMessageIfNecessary() const - { - return std::const_pointer_cast(message_); - } - - ConstMessagePtr message_; - // Kind of ugly to make this mutable, but it means we can pass a const MessageEvent - // to a callback and not worry about other things being modified - mutable MessagePtr message_copy_; - rclcpp::Time receipt_time_; - bool nonconst_need_copy_; - CreateFunction create_; - - static const std::string s_unknown_publisher_string_; -}; - -template const std::string MessageEvent::s_unknown_publisher_string_( - "unknown_publisher"); +#warning This header is obsolete, please include message_filters/message_event.hpp instead -} // namespace message_filters +#include #endif // MESSAGE_FILTERS__MESSAGE_EVENT_H_ diff --git a/include/message_filters/message_event.hpp b/include/message_filters/message_event.hpp new file mode 100644 index 0000000..7ce3e66 --- /dev/null +++ b/include/message_filters/message_event.hpp @@ -0,0 +1,247 @@ +// Copyright 2010, Willow Garage, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +// File imported from +// https://github.com/ros/roscpp_core/blob/38b9663/roscpp_traits/include/ros/message_event.h + +#ifndef MESSAGE_FILTERS__MESSAGE_EVENT_HPP_ +#define MESSAGE_FILTERS__MESSAGE_EVENT_HPP_ + +#include +#include +#include +#include +#include + +#include + +namespace message_filters +{ +typedef std::map M_string; +typedef std::shared_ptr M_stringPtr; + +template +struct DefaultMessageCreator +{ + std::shared_ptr operator()() + { + return std::make_shared(); + } +}; + + +/** + * \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 + */ +template +class MessageEvent +{ +public: + typedef typename std::add_const::type ConstMessage; + typedef typename std::remove_const::type Message; + typedef std::shared_ptr MessagePtr; + typedef std::shared_ptr ConstMessagePtr; + typedef std::function CreateFunction; + + MessageEvent() + : nonconst_need_copy_(true) + {} + + MessageEvent(const MessageEvent & rhs) + { + *this = rhs; + } + + MessageEvent(const MessageEvent & rhs) + { + *this = rhs; + } + + MessageEvent(const MessageEvent & rhs, bool nonconst_need_copy) + { + *this = rhs; + nonconst_need_copy_ = 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) + { + 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) // NOLINT(runtime/explicit) + { + init(message, rclcpp::Clock().now(), true, message_filters::DefaultMessageCreator()); + } + + 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) + { + init(message, receipt_time, nonconst_need_copy, create); + } + + void init( + const ConstMessagePtr & message, rclcpp::Time receipt_time, bool nonconst_need_copy, + const CreateFunction & create) + { + message_ = message; + receipt_time_ = receipt_time; + nonconst_need_copy_ = nonconst_need_copy; + create_ = create; + } + + void operator=(const MessageEvent & rhs) + { + init( + std::static_pointer_cast(rhs.getMessage()), + rhs.getReceiptTime(), rhs.nonConstWillCopy(), rhs.getMessageFactory()); + message_copy_.reset(); + } + + void operator=(const MessageEvent & rhs) + { + init( + std::const_pointer_cast( + std::static_pointer_cast( + rhs.getMessage())), rhs.getReceiptTime(), rhs.nonConstWillCopy(), + rhs.getMessageFactory()); + message_copy_.reset(); + } + + /** + * \brief Retrieve the message. If M is const, this returns a reference to it. If M is non const + * and this event requires it, returns a copy. Note that it caches this copy for later use, so it will + * only every make the copy once + */ + std::shared_ptr getMessage() const + { + return copyMessageIfNecessary(); + } + + /** + * \brief Retrieve a const version of the 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_;} + + bool nonConstWillCopy() const {return nonconst_need_copy_;} + bool getMessageWillCopy() const {return !std::is_const::value && nonconst_need_copy_;} + + bool operator<(const MessageEvent & rhs) + { + if (message_ != rhs.message_) { + return message_ < rhs.message_; + } + + 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) + { + return message_ == rhs.message_ && receipt_time_ == rhs.receipt_time_ && + nonconst_need_copy_ == rhs.nonconst_need_copy_; + } + + bool operator!=(const MessageEvent & rhs) + { + return !(*this == rhs); + } + + const CreateFunction & getMessageFactory() const {return create_;} + +private: + template + typename std::enable_if::value, + std::shared_ptr>::type copyMessageIfNecessary() const + { + if (std::is_const::value || !nonconst_need_copy_) { + return std::const_pointer_cast(message_); + } + + if (message_copy_) { + return message_copy_; + } + + assert(create_); + message_copy_ = create_(); + *message_copy_ = *message_; + + return message_copy_; + } + + template + typename std::enable_if::value, + std::shared_ptr>::type copyMessageIfNecessary() const + { + return std::const_pointer_cast(message_); + } + + ConstMessagePtr message_; + // Kind of ugly to make this mutable, but it means we can pass a const MessageEvent + // to a callback and not worry about other things being modified + mutable MessagePtr message_copy_; + rclcpp::Time receipt_time_; + bool nonconst_need_copy_; + CreateFunction create_; + + static const std::string s_unknown_publisher_string_; +}; + +template const std::string MessageEvent::s_unknown_publisher_string_( + "unknown_publisher"); + +} // namespace message_filters + +#endif // MESSAGE_FILTERS__MESSAGE_EVENT_HPP_ diff --git a/include/message_filters/message_traits.h b/include/message_filters/message_traits.h index c1cb146..fdf3933 100644 --- a/include/message_filters/message_traits.h +++ b/include/message_filters/message_traits.h @@ -1,107 +1,22 @@ -// Copyright 2009, Willow Garage, Inc. All rights reserved. +// Copyright 2024 Open Source Robotics Foundation, Inc. All rights reserved. // -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. +// http://www.apache.org/licenses/LICENSE-2.0 // -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the Willow Garage nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. - -// File imported from -// https://github.com/ros/roscpp_core/blob/38b9663/roscpp_traits/include/ros/message_traits.h +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef MESSAGE_FILTERS__MESSAGE_TRAITS_H_ #define MESSAGE_FILTERS__MESSAGE_TRAITS_H_ -#include -#include - -#include -#include - -namespace message_filters -{ -namespace message_traits -{ - -/** - * False if the message does not have a header - * @tparam M - */ -template -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 -struct HasHeader::value>::type>: public std::true_type {}; - -/** - * \brief FrameId trait. In the default implementation pointer() - * returns &m.header.frame_id if HasHeader::value is true, otherwise returns NULL. value() - * does not exist, and causes a compile error - */ -template -struct FrameId -{ - 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> -{ - 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;} -}; - -/** - * \brief TimeStamp trait. In the default implementation pointer() - * returns &m.header.stamp if HasHeader::value is true, otherwise returns NULL. value() - * does not exist, and causes a compile error - */ -template -struct TimeStamp -{ - static rclcpp::Time value(const M & m) - { - (void)m; - return rclcpp::Time(0, 0, RCL_ROS_TIME); - } -}; - -template -struct TimeStamp::value>::type> -{ - static rclcpp::Time value(const M & m) - { - return rclcpp::Time(m.header.stamp, RCL_ROS_TIME); - } -}; +#warning This header is obsolete, please include message_filters/message_traits.hpp instead -} // namespace message_traits -} // namespace message_filters +#include #endif // MESSAGE_FILTERS__MESSAGE_TRAITS_H_ diff --git a/include/message_filters/message_traits.hpp b/include/message_filters/message_traits.hpp new file mode 100644 index 0000000..c9d83f6 --- /dev/null +++ b/include/message_filters/message_traits.hpp @@ -0,0 +1,107 @@ +// Copyright 2009, Willow Garage, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +// File imported from +// https://github.com/ros/roscpp_core/blob/38b9663/roscpp_traits/include/ros/message_traits.h + +#ifndef MESSAGE_FILTERS__MESSAGE_TRAITS_HPP_ +#define MESSAGE_FILTERS__MESSAGE_TRAITS_HPP_ + +#include +#include + +#include +#include + +namespace message_filters +{ +namespace message_traits +{ + +/** + * False if the message does not have a header + * @tparam M + */ +template +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 +struct HasHeader::value>::type>: public std::true_type {}; + +/** + * \brief FrameId trait. In the default implementation pointer() + * returns &m.header.frame_id if HasHeader::value is true, otherwise returns NULL. value() + * does not exist, and causes a compile error + */ +template +struct FrameId +{ + 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> +{ + 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;} +}; + +/** + * \brief TimeStamp trait. In the default implementation pointer() + * returns &m.header.stamp if HasHeader::value is true, otherwise returns NULL. value() + * does not exist, and causes a compile error + */ +template +struct TimeStamp +{ + static rclcpp::Time value(const M & m) + { + (void)m; + return rclcpp::Time(0, 0, RCL_ROS_TIME); + } +}; + +template +struct TimeStamp::value>::type> +{ + static rclcpp::Time value(const M & m) + { + return rclcpp::Time(m.header.stamp, RCL_ROS_TIME); + } +}; + +} // namespace message_traits +} // namespace message_filters + +#endif // MESSAGE_FILTERS__MESSAGE_TRAITS_HPP_ diff --git a/include/message_filters/null_types.h b/include/message_filters/null_types.h index 1501bc2..9d9c901 100644 --- a/include/message_filters/null_types.h +++ b/include/message_filters/null_types.h @@ -1,76 +1,22 @@ -// Copyright 2010, Willow Garage, Inc. All rights reserved. +// Copyright 2024 Open Source Robotics Foundation, Inc. All rights reserved. // -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. +// http://www.apache.org/licenses/LICENSE-2.0 // -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the Willow Garage nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef MESSAGE_FILTERS__NULL_TYPES_H_ #define MESSAGE_FILTERS__NULL_TYPES_H_ -#include - -#include - -#include "message_filters/connection.h" -#include "message_filters/message_traits.h" - -namespace message_filters -{ - -struct NullType -{ -}; -typedef std::shared_ptr NullTypeConstPtr; - -template -struct NullFilter -{ - template - Connection registerCallback(const C &) - { - return Connection(); - } - - template - Connection registerCallback(const std::function &) - { - return Connection(); - } -}; +#warning This header is obsolete, please include message_filters/null_types.hpp instead -namespace message_traits -{ -template<> -struct TimeStamp -{ - static rclcpp::Time value(const message_filters::NullType &) - { - return rclcpp::Time(); - } -}; -} // namespace message_traits -} // namespace message_filters +#include #endif // MESSAGE_FILTERS__NULL_TYPES_H_ diff --git a/include/message_filters/null_types.hpp b/include/message_filters/null_types.hpp new file mode 100644 index 0000000..f720a1f --- /dev/null +++ b/include/message_filters/null_types.hpp @@ -0,0 +1,76 @@ +// Copyright 2010, Willow Garage, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef MESSAGE_FILTERS__NULL_TYPES_HPP_ +#define MESSAGE_FILTERS__NULL_TYPES_HPP_ + +#include + +#include + +#include "message_filters/connection.hpp" +#include "message_filters/message_traits.hpp" + +namespace message_filters +{ + +struct NullType +{ +}; +typedef std::shared_ptr NullTypeConstPtr; + +template +struct NullFilter +{ + template + Connection registerCallback(const C &) + { + return Connection(); + } + + template + Connection registerCallback(const std::function &) + { + return Connection(); + } +}; + +namespace message_traits +{ +template<> +struct TimeStamp +{ + static rclcpp::Time value(const message_filters::NullType &) + { + return rclcpp::Time(); + } +}; +} // namespace message_traits +} // namespace message_filters + +#endif // MESSAGE_FILTERS__NULL_TYPES_HPP_ diff --git a/include/message_filters/parameter_adapter.h b/include/message_filters/parameter_adapter.h index cc6860e..6777bc1 100644 --- a/include/message_filters/parameter_adapter.h +++ b/include/message_filters/parameter_adapter.h @@ -1,183 +1,22 @@ -// Copyright 2010, Willow Garage, Inc. All rights reserved. +// Copyright 2024 Open Source Robotics Foundation, Inc. All rights reserved. // -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. +// http://www.apache.org/licenses/LICENSE-2.0 // -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the Willow Garage nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. - -// Imported from -// https://github.com/ros/ros_comm/blob/a2ca01ecf31d677dd5c1b08cc6852bd0dac02b97/clients/roscpp/include/ros/parameter_adapter.h +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef MESSAGE_FILTERS__PARAMETER_ADAPTER_H_ #define MESSAGE_FILTERS__PARAMETER_ADAPTER_H_ -#include -#include - -#include "message_filters/message_event.h" - -namespace message_filters -{ - -/** - * \brief Generally not for outside use. Adapts a function parameter type into the message type, event type and parameter. Allows you to - * retrieve a parameter type from an event type. - * - * ParameterAdapter is generally only useful for outside use when implementing things that require message callbacks - * (such as the message_filters package)and you would like to support all the rclcpp message parameter types - * - * The ParameterAdapter is templated on the callback parameter type (\b not the bare message type), and provides 3 things: - * - Message typedef, which provides the bare message type, no const or reference qualifiers - * - Event typedef, which provides the message_filters::MessageEvent type - * - Parameter typedef, which provides the actual parameter type (may be slightly different from M) - * - static getParameter(event) function, which returns a parameter type given the event - * - static bool is_const informs you whether or not the parameter type is a const message - * - * 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(std::shared_ptr); -void callback(std::shared_ptr); -void callback(const M &); -void callback(M); -void callback(const MessageEvent &); -void callback(const MessageEvent &); -\endverbatim - */ -template -struct ParameterAdapter -{ - typedef typename std::remove_reference::type>::type Message; - typedef MessageEvent Event; - typedef M Parameter; - static const bool is_const = true; - - static Parameter getParameter(const Event & event) - { - return *event.getMessage(); - } -}; - -template -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) - { - return event.getMessage(); - } -}; - -template -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) - { - return MessageEvent(event).getMessage(); - } -}; - -template -struct ParameterAdapter -{ - typedef typename std::remove_reference::type>::type Message; - typedef MessageEvent Event; - typedef const M & Parameter; - static const bool is_const = true; - - static Parameter getParameter(const Event & event) - { - return *event.getMessage(); - } -}; - -template -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) - { - return event.getMessage(); - } -}; - -template -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) - { - return MessageEvent(event).getMessage(); - } -}; - -template -struct ParameterAdapter &> -{ - typedef typename std::remove_reference::type>::type Message; - typedef MessageEvent Event; - typedef const MessageEvent & Parameter; - static const bool is_const = true; - - static Parameter getParameter(const Event & event) - { - return event; - } -}; - -template -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) - { - return MessageEvent(event); - } -}; +#warning This header is obsolete, please include message_filters/parameter_adapter.hpp instead -} // namespace message_filters +#include #endif // MESSAGE_FILTERS__PARAMETER_ADAPTER_H_ diff --git a/include/message_filters/parameter_adapter.hpp b/include/message_filters/parameter_adapter.hpp new file mode 100644 index 0000000..3e5ab29 --- /dev/null +++ b/include/message_filters/parameter_adapter.hpp @@ -0,0 +1,183 @@ +// Copyright 2010, Willow Garage, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +// Imported from +// https://github.com/ros/ros_comm/blob/a2ca01ecf31d677dd5c1b08cc6852bd0dac02b97/clients/roscpp/include/ros/parameter_adapter.h + +#ifndef MESSAGE_FILTERS__PARAMETER_ADAPTER_HPP_ +#define MESSAGE_FILTERS__PARAMETER_ADAPTER_HPP_ + +#include +#include + +#include "message_filters/message_event.hpp" + +namespace message_filters +{ + +/** + * \brief Generally not for outside use. Adapts a function parameter type into the message type, event type and parameter. Allows you to + * retrieve a parameter type from an event type. + * + * ParameterAdapter is generally only useful for outside use when implementing things that require message callbacks + * (such as the message_filters package)and you would like to support all the rclcpp message parameter types + * + * The ParameterAdapter is templated on the callback parameter type (\b not the bare message type), and provides 3 things: + * - Message typedef, which provides the bare message type, no const or reference qualifiers + * - Event typedef, which provides the message_filters::MessageEvent type + * - Parameter typedef, which provides the actual parameter type (may be slightly different from M) + * - static getParameter(event) function, which returns a parameter type given the event + * - static bool is_const informs you whether or not the parameter type is a const message + * + * 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(std::shared_ptr); +void callback(std::shared_ptr); +void callback(const M &); +void callback(M); +void callback(const MessageEvent &); +void callback(const MessageEvent &); +\endverbatim + */ +template +struct ParameterAdapter +{ + typedef typename std::remove_reference::type>::type Message; + typedef MessageEvent Event; + typedef M Parameter; + static const bool is_const = true; + + static Parameter getParameter(const Event & event) + { + return *event.getMessage(); + } +}; + +template +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) + { + return event.getMessage(); + } +}; + +template +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) + { + return MessageEvent(event).getMessage(); + } +}; + +template +struct ParameterAdapter +{ + typedef typename std::remove_reference::type>::type Message; + typedef MessageEvent Event; + typedef const M & Parameter; + static const bool is_const = true; + + static Parameter getParameter(const Event & event) + { + return *event.getMessage(); + } +}; + +template +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) + { + return event.getMessage(); + } +}; + +template +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) + { + return MessageEvent(event).getMessage(); + } +}; + +template +struct ParameterAdapter &> +{ + typedef typename std::remove_reference::type>::type Message; + typedef MessageEvent Event; + typedef const MessageEvent & Parameter; + static const bool is_const = true; + + static Parameter getParameter(const Event & event) + { + return event; + } +}; + +template +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) + { + return MessageEvent(event); + } +}; + +} // namespace message_filters + +#endif // MESSAGE_FILTERS__PARAMETER_ADAPTER_HPP_ diff --git a/include/message_filters/pass_through.h b/include/message_filters/pass_through.h index 14d8fc3..f7d44cf 100644 --- a/include/message_filters/pass_through.h +++ b/include/message_filters/pass_through.h @@ -1,94 +1,22 @@ -// Copyright 2010, Willow Garage, Inc. All rights reserved. +// Copyright 2024 Open Source Robotics Foundation, Inc. All rights reserved. // -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. +// http://www.apache.org/licenses/LICENSE-2.0 // -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the Willow Garage nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef MESSAGE_FILTERS__PASS_THROUGH_H_ #define MESSAGE_FILTERS__PASS_THROUGH_H_ -#include -#include -#include - -#include "message_filters/simple_filter.h" - -namespace message_filters -{ -/** - * \brief Simple passthrough filter. What comes in goes out immediately. - */ -template -class PassThrough : public SimpleFilter -{ -public: - typedef std::shared_ptr MConstPtr; - typedef MessageEvent EventType; - - PassThrough() - { - } - - - template - PassThrough(F & f) // NOLINT(runtime/explicit) - { - connectInput(f); - } - - template - void connectInput(F & f) - { - incoming_connection_.disconnect(); - incoming_connection_ = - f.registerCallback( - typename SimpleFilter::EventCallback( - std::bind( - &PassThrough::cb, this, - std::placeholders::_1))); - } - - void add(const MConstPtr & msg) - { - add(EventType(msg)); - } - - void add(const EventType & evt) - { - this->signalMessage(evt); - } - -private: - void cb(const EventType & evt) - { - add(evt); - } - - Connection incoming_connection_; -}; +#warning This header is obsolete, please include message_filters/pass_through.hpp instead -} // namespace message_filters +#include #endif // MESSAGE_FILTERS__PASS_THROUGH_H_ diff --git a/include/message_filters/pass_through.hpp b/include/message_filters/pass_through.hpp new file mode 100644 index 0000000..2de55cb --- /dev/null +++ b/include/message_filters/pass_through.hpp @@ -0,0 +1,94 @@ +// Copyright 2010, Willow Garage, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef MESSAGE_FILTERS__PASS_THROUGH_HPP_ +#define MESSAGE_FILTERS__PASS_THROUGH_HPP_ + +#include +#include +#include + +#include "message_filters/simple_filter.hpp" + +namespace message_filters +{ +/** + * \brief Simple passthrough filter. What comes in goes out immediately. + */ +template +class PassThrough : public SimpleFilter +{ +public: + typedef std::shared_ptr MConstPtr; + typedef MessageEvent EventType; + + PassThrough() + { + } + + + template + PassThrough(F & f) // NOLINT(runtime/explicit) + { + connectInput(f); + } + + template + void connectInput(F & f) + { + incoming_connection_.disconnect(); + incoming_connection_ = + f.registerCallback( + typename SimpleFilter::EventCallback( + std::bind( + &PassThrough::cb, this, + std::placeholders::_1))); + } + + void add(const MConstPtr & msg) + { + add(EventType(msg)); + } + + void add(const EventType & evt) + { + this->signalMessage(evt); + } + +private: + void cb(const EventType & evt) + { + add(evt); + } + + Connection incoming_connection_; +}; + +} // namespace message_filters + +#endif // MESSAGE_FILTERS__PASS_THROUGH_HPP_ diff --git a/include/message_filters/signal1.h b/include/message_filters/signal1.h index 4ea243d..81ef56b 100644 --- a/include/message_filters/signal1.h +++ b/include/message_filters/signal1.h @@ -1,122 +1,22 @@ -// Copyright 2010, Willow Garage, Inc. All rights reserved. +// Copyright 2024 Open Source Robotics Foundation, Inc. All rights reserved. // -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. +// http://www.apache.org/licenses/LICENSE-2.0 // -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the Willow Garage nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef MESSAGE_FILTERS__SIGNAL1_H_ #define MESSAGE_FILTERS__SIGNAL1_H_ -#include -#include -#include -#include - -#include "message_filters/connection.h" -#include "message_filters/message_event.h" -#include "message_filters/parameter_adapter.h" - -namespace message_filters -{ -template -class CallbackHelper1 -{ -public: - virtual ~CallbackHelper1() {} - - virtual void call(const MessageEvent & event, bool nonconst_need_copy) = 0; - - typedef std::shared_ptr> Ptr; -}; - -template -class CallbackHelper1T : public CallbackHelper1 -{ -public: - typedef ParameterAdapter

Adapter; - typedef std::function Callback; - typedef typename Adapter::Event Event; - - CallbackHelper1T(const Callback & cb) // NOLINT(runtime/explicit) - : callback_(cb) - { - } - - virtual void call(const MessageEvent & event, bool nonconst_force_copy) - { - Event my_event(event, nonconst_force_copy || event.nonConstWillCopy()); - callback_(Adapter::getParameter(my_event)); - } - -private: - Callback callback_; -}; - -template -class Signal1 -{ - typedef std::shared_ptr> CallbackHelper1Ptr; - typedef std::vector V_CallbackHelper1; - -public: - template - CallbackHelper1Ptr addCallback(const std::function & callback) - { - CallbackHelper1T * helper = new CallbackHelper1T(callback); - - std::lock_guard lock(mutex_); - callbacks_.push_back(CallbackHelper1Ptr(helper)); - return callbacks_.back(); - } - - 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()) { - callbacks_.erase(it); - } - } - - 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; - helper->call(event, nonconst_force_copy); - } - } +#warning This header is obsolete, please include message_filters/signal1.hpp instead -private: - std::mutex mutex_; - V_CallbackHelper1 callbacks_; -}; -} // namespace message_filters +#include #endif // MESSAGE_FILTERS__SIGNAL1_H_ diff --git a/include/message_filters/signal1.hpp b/include/message_filters/signal1.hpp new file mode 100644 index 0000000..b1c81a7 --- /dev/null +++ b/include/message_filters/signal1.hpp @@ -0,0 +1,122 @@ +// Copyright 2010, Willow Garage, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef MESSAGE_FILTERS__SIGNAL1_HPP_ +#define MESSAGE_FILTERS__SIGNAL1_HPP_ + +#include +#include +#include +#include + +#include "message_filters/connection.hpp" +#include "message_filters/message_event.hpp" +#include "message_filters/parameter_adapter.hpp" + +namespace message_filters +{ +template +class CallbackHelper1 +{ +public: + virtual ~CallbackHelper1() {} + + virtual void call(const MessageEvent & event, bool nonconst_need_copy) = 0; + + typedef std::shared_ptr> Ptr; +}; + +template +class CallbackHelper1T : public CallbackHelper1 +{ +public: + typedef ParameterAdapter

Adapter; + typedef std::function Callback; + typedef typename Adapter::Event Event; + + CallbackHelper1T(const Callback & cb) // NOLINT(runtime/explicit) + : callback_(cb) + { + } + + virtual void call(const MessageEvent & event, bool nonconst_force_copy) + { + Event my_event(event, nonconst_force_copy || event.nonConstWillCopy()); + callback_(Adapter::getParameter(my_event)); + } + +private: + Callback callback_; +}; + +template +class Signal1 +{ + typedef std::shared_ptr> CallbackHelper1Ptr; + typedef std::vector V_CallbackHelper1; + +public: + template + CallbackHelper1Ptr addCallback(const std::function & callback) + { + CallbackHelper1T * helper = new CallbackHelper1T(callback); + + std::lock_guard lock(mutex_); + callbacks_.push_back(CallbackHelper1Ptr(helper)); + return callbacks_.back(); + } + + 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()) { + callbacks_.erase(it); + } + } + + 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; + helper->call(event, nonconst_force_copy); + } + } + +private: + std::mutex mutex_; + V_CallbackHelper1 callbacks_; +}; +} // namespace message_filters + +#endif // MESSAGE_FILTERS__SIGNAL1_HPP_ diff --git a/include/message_filters/signal9.h b/include/message_filters/signal9.h index ad01494..5304ff8 100644 --- a/include/message_filters/signal9.h +++ b/include/message_filters/signal9.h @@ -1,399 +1,22 @@ -// Copyright 2010, Willow Garage, Inc. All rights reserved. +// Copyright 2024 Open Source Robotics Foundation, Inc. All rights reserved. // -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. +// http://www.apache.org/licenses/LICENSE-2.0 // -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the Willow Garage nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef MESSAGE_FILTERS__SIGNAL9_H_ #define MESSAGE_FILTERS__SIGNAL9_H_ +#warning This header is obsolete, please include message_filters/signal9.hpp instead -#include -#include -#include -#include - -#include "message_filters/connection.h" -#include "message_filters/null_types.h" -#include "message_filters/message_event.h" -#include "message_filters/parameter_adapter.h" - -namespace message_filters -{ - -template -class CallbackHelper9 -{ -public: - typedef MessageEvent M0Event; - typedef MessageEvent M1Event; - typedef MessageEvent M2Event; - typedef MessageEvent M3Event; - typedef MessageEvent M4Event; - typedef MessageEvent M5Event; - typedef MessageEvent M6Event; - typedef MessageEvent M7Event; - typedef MessageEvent M8Event; - - virtual ~CallbackHelper9() {} - - virtual void call( - bool nonconst_force_copy, const M0Event & e0, const M1Event & e1, - const M2Event & e2, const M3Event & e3, const M4Event & e4, const M5Event & e5, - const M6Event & e6, const M7Event & e7, const M8Event & e8) = 0; - - 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> -{ -private: - typedef ParameterAdapter A0; - typedef ParameterAdapter A1; - typedef ParameterAdapter A2; - typedef ParameterAdapter A3; - typedef ParameterAdapter A4; - typedef ParameterAdapter A5; - typedef ParameterAdapter A6; - typedef ParameterAdapter A7; - typedef ParameterAdapter A8; - typedef typename A0::Event M0Event; - typedef typename A1::Event M1Event; - typedef typename A2::Event M2Event; - typedef typename A3::Event M3Event; - typedef typename A4::Event M4Event; - typedef typename A5::Event M5Event; - typedef typename A6::Event M6Event; - typedef typename A7::Event M7Event; - typedef typename A8::Event M8Event; - -public: - typedef std::function Callback; - - CallbackHelper9T(const Callback & cb) // NOLINT(runtime/explicit) - : callback_(cb) - { - } - - virtual void call( - bool nonconst_force_copy, const M0Event & e0, const M1Event & e1, - const M2Event & e2, const M3Event & e3, const M4Event & e4, const M5Event & e5, - const M6Event & e6, const M7Event & e7, const M8Event & e8) - { - M0Event my_e0(e0, nonconst_force_copy || e0.nonConstWillCopy()); - M1Event my_e1(e1, nonconst_force_copy || e0.nonConstWillCopy()); - M2Event my_e2(e2, nonconst_force_copy || e0.nonConstWillCopy()); - M3Event my_e3(e3, nonconst_force_copy || e0.nonConstWillCopy()); - M4Event my_e4(e4, nonconst_force_copy || e0.nonConstWillCopy()); - M5Event my_e5(e5, nonconst_force_copy || e0.nonConstWillCopy()); - M6Event my_e6(e6, nonconst_force_copy || e0.nonConstWillCopy()); - M7Event my_e7(e7, nonconst_force_copy || e0.nonConstWillCopy()); - M8Event my_e8(e8, nonconst_force_copy || e0.nonConstWillCopy()); - callback_( - A0::getParameter(e0), - A1::getParameter(e1), - A2::getParameter(e2), - A3::getParameter(e3), - A4::getParameter(e4), - A5::getParameter(e5), - A6::getParameter(e6), - A7::getParameter(e7), - A8::getParameter(e8)); - } - -private: - Callback callback_; -}; - -template -class Signal9 -{ - typedef std::shared_ptr> CallbackHelper9Ptr; - typedef std::vector V_CallbackHelper9; - -public: - typedef MessageEvent M0Event; - typedef MessageEvent M1Event; - typedef MessageEvent M2Event; - typedef MessageEvent M3Event; - typedef MessageEvent M4Event; - typedef MessageEvent M5Event; - typedef MessageEvent M6Event; - typedef MessageEvent M7Event; - typedef MessageEvent M8Event; - typedef std::shared_ptr M0ConstPtr; - typedef std::shared_ptr M1ConstPtr; - typedef std::shared_ptr M2ConstPtr; - typedef std::shared_ptr M3ConstPtr; - typedef std::shared_ptr M4ConstPtr; - typedef std::shared_ptr M5ConstPtr; - typedef std::shared_ptr M6ConstPtr; - typedef std::shared_ptr M7ConstPtr; - typedef std::shared_ptr M8ConstPtr; - typedef const std::shared_ptr & NullP; - - - template - Connection addCallback(const std::function & callback) - { - CallbackHelper9T * helper = - new CallbackHelper9T(callback); - - std::lock_guard lock(mutex_); - callbacks_.push_back(CallbackHelper9Ptr(helper)); - return Connection(std::bind(&Signal9::removeCallback, this, callbacks_.back())); - } - - template - Connection addCallback(void (* callback)(P0, P1)) - { - return addCallback( - std::function( - std::bind( - callback, std::placeholders::_1, std::placeholders::_2))); - } - - template - Connection addCallback(void (* callback)(P0, P1, P2)) - { - 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)) - { - return addCallback( - std::function( - std::bind( - callback, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - std::placeholders::_4))); - } - - template - Connection addCallback(void (* callback)(P0, P1, P2, P3, P4)) - { - return addCallback( - std::function( - std::bind( - callback, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - std::placeholders::_4, std::placeholders::_5))); - } - - template - Connection addCallback(void (* callback)(P0, P1, P2, P3, P4, P5)) - { - return addCallback( - std::function( - std::bind( - callback, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - std::placeholders::_4, std::placeholders::_5, std::placeholders::_6))); - } - - template - Connection addCallback(void (* callback)(P0, P1, P2, P3, P4, P5, P6)) - { - return addCallback( - std::function( - std::bind( - callback, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, - std::placeholders::_7))); - } - - template - Connection addCallback(void (* callback)(P0, P1, P2, P3, P4, P5, P6, P7)) - { - return addCallback( - std::function( - std::bind( - callback, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, - std::placeholders::_7, std::placeholders::_8))); - } - - template - Connection addCallback(void (* callback)(P0, P1, P2, P3, P4, P5, P6, P7, P8)) - { - return addCallback( - std::function( - std::bind( - callback, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, - std::placeholders::_7, std::placeholders::_8, std::placeholders::_9))); - } - - template - Connection addCallback(void (T::* callback)(P0, P1), T * t) - { - return addCallback( - std::function( - std::bind( - callback, t, std::placeholders::_1, std::placeholders::_2))); - } - - template - Connection addCallback(void (T::* callback)(P0, P1, P2), T * t) - { - return addCallback( - std::function( - std::bind( - callback, t, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3))); - } - - template - Connection addCallback(void (T::* callback)(P0, P1, P2, P3), T * t) - { - return addCallback( - std::function( - std::bind( - callback, t, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, std::placeholders::_4))); - } - - template - Connection addCallback(void (T::* callback)(P0, P1, P2, P3, P4), T * t) - { - return addCallback( - std::function( - std::bind( - callback, t, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, std::placeholders::_4, std::placeholders::_5))); - } - - template - Connection addCallback(void (T::* callback)(P0, P1, P2, P3, P4, P5), T * t) - { - return addCallback( - std::function( - std::bind( - callback, t, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, - std::placeholders::_6))); - } - - template - Connection addCallback(void (T::* callback)(P0, P1, P2, P3, P4, P5, P6), T * t) - { - return addCallback( - std::function( - std::bind( - callback, t, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, - std::placeholders::_6, std::placeholders::_7))); - } - - template - Connection addCallback(void (T::* callback)(P0, P1, P2, P3, P4, P5, P6, P7), T * t) - { - return addCallback( - std::function( - std::bind( - callback, t, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, - std::placeholders::_6, std::placeholders::_7, std::placeholders::_8))); - } - - template - Connection addCallback(C & callback) - { - return addCallback( - std::bind( - callback, std::placeholders::_1, - std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, - std::placeholders::_5, std::placeholders::_6, std::placeholders::_7, - std::placeholders::_8, std::placeholders::_9)); - } - - 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()) { - 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) - { - 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; - helper->call(nonconst_force_copy, e0, e1, e2, e3, e4, e5, e6, e7, e8); - } - } - -private: - std::mutex mutex_; - V_CallbackHelper9 callbacks_; -}; - -} // namespace message_filters +#include #endif // MESSAGE_FILTERS__SIGNAL9_H_ diff --git a/include/message_filters/signal9.hpp b/include/message_filters/signal9.hpp new file mode 100644 index 0000000..31d476f --- /dev/null +++ b/include/message_filters/signal9.hpp @@ -0,0 +1,399 @@ +// Copyright 2010, Willow Garage, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef MESSAGE_FILTERS__SIGNAL9_HPP_ +#define MESSAGE_FILTERS__SIGNAL9_HPP_ + + +#include +#include +#include +#include + +#include "message_filters/connection.hpp" +#include "message_filters/null_types.hpp" +#include "message_filters/message_event.hpp" +#include "message_filters/parameter_adapter.hpp" + +namespace message_filters +{ + +template +class CallbackHelper9 +{ +public: + typedef MessageEvent M0Event; + typedef MessageEvent M1Event; + typedef MessageEvent M2Event; + typedef MessageEvent M3Event; + typedef MessageEvent M4Event; + typedef MessageEvent M5Event; + typedef MessageEvent M6Event; + typedef MessageEvent M7Event; + typedef MessageEvent M8Event; + + virtual ~CallbackHelper9() {} + + virtual void call( + bool nonconst_force_copy, const M0Event & e0, const M1Event & e1, + const M2Event & e2, const M3Event & e3, const M4Event & e4, const M5Event & e5, + const M6Event & e6, const M7Event & e7, const M8Event & e8) = 0; + + 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> +{ +private: + typedef ParameterAdapter A0; + typedef ParameterAdapter A1; + typedef ParameterAdapter A2; + typedef ParameterAdapter A3; + typedef ParameterAdapter A4; + typedef ParameterAdapter A5; + typedef ParameterAdapter A6; + typedef ParameterAdapter A7; + typedef ParameterAdapter A8; + typedef typename A0::Event M0Event; + typedef typename A1::Event M1Event; + typedef typename A2::Event M2Event; + typedef typename A3::Event M3Event; + typedef typename A4::Event M4Event; + typedef typename A5::Event M5Event; + typedef typename A6::Event M6Event; + typedef typename A7::Event M7Event; + typedef typename A8::Event M8Event; + +public: + typedef std::function Callback; + + CallbackHelper9T(const Callback & cb) // NOLINT(runtime/explicit) + : callback_(cb) + { + } + + virtual void call( + bool nonconst_force_copy, const M0Event & e0, const M1Event & e1, + const M2Event & e2, const M3Event & e3, const M4Event & e4, const M5Event & e5, + const M6Event & e6, const M7Event & e7, const M8Event & e8) + { + M0Event my_e0(e0, nonconst_force_copy || e0.nonConstWillCopy()); + M1Event my_e1(e1, nonconst_force_copy || e0.nonConstWillCopy()); + M2Event my_e2(e2, nonconst_force_copy || e0.nonConstWillCopy()); + M3Event my_e3(e3, nonconst_force_copy || e0.nonConstWillCopy()); + M4Event my_e4(e4, nonconst_force_copy || e0.nonConstWillCopy()); + M5Event my_e5(e5, nonconst_force_copy || e0.nonConstWillCopy()); + M6Event my_e6(e6, nonconst_force_copy || e0.nonConstWillCopy()); + M7Event my_e7(e7, nonconst_force_copy || e0.nonConstWillCopy()); + M8Event my_e8(e8, nonconst_force_copy || e0.nonConstWillCopy()); + callback_( + A0::getParameter(e0), + A1::getParameter(e1), + A2::getParameter(e2), + A3::getParameter(e3), + A4::getParameter(e4), + A5::getParameter(e5), + A6::getParameter(e6), + A7::getParameter(e7), + A8::getParameter(e8)); + } + +private: + Callback callback_; +}; + +template +class Signal9 +{ + typedef std::shared_ptr> CallbackHelper9Ptr; + typedef std::vector V_CallbackHelper9; + +public: + typedef MessageEvent M0Event; + typedef MessageEvent M1Event; + typedef MessageEvent M2Event; + typedef MessageEvent M3Event; + typedef MessageEvent M4Event; + typedef MessageEvent M5Event; + typedef MessageEvent M6Event; + typedef MessageEvent M7Event; + typedef MessageEvent M8Event; + typedef std::shared_ptr M0ConstPtr; + typedef std::shared_ptr M1ConstPtr; + typedef std::shared_ptr M2ConstPtr; + typedef std::shared_ptr M3ConstPtr; + typedef std::shared_ptr M4ConstPtr; + typedef std::shared_ptr M5ConstPtr; + typedef std::shared_ptr M6ConstPtr; + typedef std::shared_ptr M7ConstPtr; + typedef std::shared_ptr M8ConstPtr; + typedef const std::shared_ptr & NullP; + + + template + Connection addCallback(const std::function & callback) + { + CallbackHelper9T * helper = + new CallbackHelper9T(callback); + + std::lock_guard lock(mutex_); + callbacks_.push_back(CallbackHelper9Ptr(helper)); + return Connection(std::bind(&Signal9::removeCallback, this, callbacks_.back())); + } + + template + Connection addCallback(void (* callback)(P0, P1)) + { + return addCallback( + std::function( + std::bind( + callback, std::placeholders::_1, std::placeholders::_2))); + } + + template + Connection addCallback(void (* callback)(P0, P1, P2)) + { + 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)) + { + return addCallback( + std::function( + std::bind( + callback, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4))); + } + + template + Connection addCallback(void (* callback)(P0, P1, P2, P3, P4)) + { + return addCallback( + std::function( + std::bind( + callback, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5))); + } + + template + Connection addCallback(void (* callback)(P0, P1, P2, P3, P4, P5)) + { + return addCallback( + std::function( + std::bind( + callback, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5, std::placeholders::_6))); + } + + template + Connection addCallback(void (* callback)(P0, P1, P2, P3, P4, P5, P6)) + { + return addCallback( + std::function( + std::bind( + callback, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, + std::placeholders::_7))); + } + + template + Connection addCallback(void (* callback)(P0, P1, P2, P3, P4, P5, P6, P7)) + { + return addCallback( + std::function( + std::bind( + callback, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, + std::placeholders::_7, std::placeholders::_8))); + } + + template + Connection addCallback(void (* callback)(P0, P1, P2, P3, P4, P5, P6, P7, P8)) + { + return addCallback( + std::function( + std::bind( + callback, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, + std::placeholders::_7, std::placeholders::_8, std::placeholders::_9))); + } + + template + Connection addCallback(void (T::* callback)(P0, P1), T * t) + { + return addCallback( + std::function( + std::bind( + callback, t, std::placeholders::_1, std::placeholders::_2))); + } + + template + Connection addCallback(void (T::* callback)(P0, P1, P2), T * t) + { + return addCallback( + std::function( + std::bind( + callback, t, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3))); + } + + template + Connection addCallback(void (T::* callback)(P0, P1, P2, P3), T * t) + { + return addCallback( + std::function( + std::bind( + callback, t, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4))); + } + + template + Connection addCallback(void (T::* callback)(P0, P1, P2, P3, P4), T * t) + { + return addCallback( + std::function( + std::bind( + callback, t, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4, std::placeholders::_5))); + } + + template + Connection addCallback(void (T::* callback)(P0, P1, P2, P3, P4, P5), T * t) + { + return addCallback( + std::function( + std::bind( + callback, t, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, + std::placeholders::_6))); + } + + template + Connection addCallback(void (T::* callback)(P0, P1, P2, P3, P4, P5, P6), T * t) + { + return addCallback( + std::function( + std::bind( + callback, t, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, + std::placeholders::_6, std::placeholders::_7))); + } + + template + Connection addCallback(void (T::* callback)(P0, P1, P2, P3, P4, P5, P6, P7), T * t) + { + return addCallback( + std::function( + std::bind( + callback, t, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, + std::placeholders::_6, std::placeholders::_7, std::placeholders::_8))); + } + + template + Connection addCallback(C & callback) + { + return addCallback( + std::bind( + callback, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, + std::placeholders::_5, std::placeholders::_6, std::placeholders::_7, + std::placeholders::_8, std::placeholders::_9)); + } + + 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()) { + 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) + { + 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; + helper->call(nonconst_force_copy, e0, e1, e2, e3, e4, e5, e6, e7, e8); + } + } + +private: + std::mutex mutex_; + V_CallbackHelper9 callbacks_; +}; + +} // namespace message_filters + +#endif // MESSAGE_FILTERS__SIGNAL9_HPP_ diff --git a/include/message_filters/simple_filter.h b/include/message_filters/simple_filter.h index 4c6251f..1aa10ee 100644 --- a/include/message_filters/simple_filter.h +++ b/include/message_filters/simple_filter.h @@ -1,142 +1,22 @@ -// Copyright 2008, Willow Garage, Inc. All rights reserved. +// Copyright 2024 Open Source Robotics Foundation, Inc. All rights reserved. // -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. +// http://www.apache.org/licenses/LICENSE-2.0 // -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the Willow Garage nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef MESSAGE_FILTERS__SIMPLE_FILTER_H_ #define MESSAGE_FILTERS__SIMPLE_FILTER_H_ -#include -#include -#include - -#include "message_filters/connection.h" -#include "message_filters/signal1.h" -#include "message_filters/message_event.h" - -namespace message_filters -{ - -/** - * \brief Convenience base-class for simple filters which output a single message - * - * SimpleFilter provides some of the tricky callback registering functionality, so that - * simple filters do not have to duplicate it. It also provides getName()/setName() for debugging - * purposes. - */ -template -class SimpleFilter : public noncopyable -{ -public: - typedef std::shared_ptr MConstPtr; - typedef std::function Callback; - typedef MessageEvent EventType; - 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) - { - typename CallbackHelper1::Ptr helper = signal_.addCallback(Callback(callback)); - return Connection(std::bind(&Signal::removeCallback, &signal_, helper)); - } - - /** - * \brief Register a callback to be called when this filter has passed - * \param callback The callback to call - */ - template - Connection registerCallback(const std::function & callback) - { - return Connection(std::bind(&Signal::removeCallback, &signal_, signal_.addCallback(callback))); - } - - /** - * \brief Register a callback to be called when this filter has passed - * \param callback The callback to call - */ - template - Connection registerCallback(void (* callback)(P)) - { - typename CallbackHelper1::Ptr helper = - signal_.template addCallback

(std::bind(callback, std::placeholders::_1)); - return Connection(std::bind(&Signal::removeCallback, &signal_, helper)); - } - - /** - * \brief Register a callback to be called when this filter has passed - * \param callback The callback to call - */ - template - Connection registerCallback(void (T::* callback)(P), T * t) - { - 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;} - /** - * \brief Get the name of this filter. For debugging use. - */ - const std::string & getName() {return name_;} - -protected: - /** - * \brief Call all registered callbacks, passing them the specified message - */ - void signalMessage(const MConstPtr & msg) - { - MessageEvent event(msg); - - signal_.call(event); - } - - /** - * \brief Call all registered callbacks, passing them the specified message - */ - void signalMessage(const MessageEvent & event) - { - signal_.call(event); - } - -private: - typedef Signal1 Signal; - - Signal signal_; - - std::string name_; -}; +#warning This header is obsolete, please include message_filters/simple_filter.hpp instead -} // namespace message_filters +#include #endif // MESSAGE_FILTERS__SIMPLE_FILTER_H_ diff --git a/include/message_filters/simple_filter.hpp b/include/message_filters/simple_filter.hpp new file mode 100644 index 0000000..d8d875b --- /dev/null +++ b/include/message_filters/simple_filter.hpp @@ -0,0 +1,142 @@ +// Copyright 2008, Willow Garage, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef MESSAGE_FILTERS__SIMPLE_FILTER_HPP_ +#define MESSAGE_FILTERS__SIMPLE_FILTER_HPP_ + +#include +#include +#include + +#include "message_filters/connection.hpp" +#include "message_filters/signal1.hpp" +#include "message_filters/message_event.hpp" + +namespace message_filters +{ + +/** + * \brief Convenience base-class for simple filters which output a single message + * + * SimpleFilter provides some of the tricky callback registering functionality, so that + * simple filters do not have to duplicate it. It also provides getName()/setName() for debugging + * purposes. + */ +template +class SimpleFilter : public noncopyable +{ +public: + typedef std::shared_ptr MConstPtr; + typedef std::function Callback; + typedef MessageEvent EventType; + 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) + { + typename CallbackHelper1::Ptr helper = signal_.addCallback(Callback(callback)); + return Connection(std::bind(&Signal::removeCallback, &signal_, helper)); + } + + /** + * \brief Register a callback to be called when this filter has passed + * \param callback The callback to call + */ + template + Connection registerCallback(const std::function & callback) + { + return Connection(std::bind(&Signal::removeCallback, &signal_, signal_.addCallback(callback))); + } + + /** + * \brief Register a callback to be called when this filter has passed + * \param callback The callback to call + */ + template + Connection registerCallback(void (* callback)(P)) + { + typename CallbackHelper1::Ptr helper = + signal_.template addCallback

(std::bind(callback, std::placeholders::_1)); + return Connection(std::bind(&Signal::removeCallback, &signal_, helper)); + } + + /** + * \brief Register a callback to be called when this filter has passed + * \param callback The callback to call + */ + template + Connection registerCallback(void (T::* callback)(P), T * t) + { + 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;} + /** + * \brief Get the name of this filter. For debugging use. + */ + const std::string & getName() {return name_;} + +protected: + /** + * \brief Call all registered callbacks, passing them the specified message + */ + void signalMessage(const MConstPtr & msg) + { + MessageEvent event(msg); + + signal_.call(event); + } + + /** + * \brief Call all registered callbacks, passing them the specified message + */ + void signalMessage(const MessageEvent & event) + { + signal_.call(event); + } + +private: + typedef Signal1 Signal; + + Signal signal_; + + std::string name_; +}; + +} // namespace message_filters + +#endif // MESSAGE_FILTERS__SIMPLE_FILTER_HPP_ diff --git a/include/message_filters/subscriber.h b/include/message_filters/subscriber.h index 7649ee0..90ab265 100644 --- a/include/message_filters/subscriber.h +++ b/include/message_filters/subscriber.h @@ -1,602 +1,22 @@ -// Copyright 2009, Willow Garage, Inc. All rights reserved. +// Copyright 2024 Open Source Robotics Foundation, Inc. All rights reserved. // -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. +// http://www.apache.org/licenses/LICENSE-2.0 // -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the Willow Garage nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef MESSAGE_FILTERS__SUBSCRIBER_H_ #define MESSAGE_FILTERS__SUBSCRIBER_H_ -#include -#include -#include -#include - -#include - -#include "message_filters/connection.h" -#include "message_filters/simple_filter.h" - -namespace message_filters -{ - -template -class SubscriberBase -{ -public: - typedef std::shared_ptr NodePtr; - - virtual ~SubscriberBase() = default; - /** - * \brief Subscribe to a topic. - * - * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. - * - * \param node The rclcpp::Node::SharedPtr to use to subscribe. - * \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 rclcpp::QoS & qos) = 0; - - /** - * \brief Subscribe to a topic. - * - * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. - * - * \param node The rclcpp::Node to use to subscribe. - * \param topic The topic to subscribe to. - * \param qos (optional) The rclcpp::rmw qos profile to use to subscribe - */ - virtual void subscribe( - NodeType * node, const std::string & topic, - const rclcpp::QoS & qos) = 0; - - /** - * \brief Subscribe to a topic. - * - * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. - * This override allows SubscriptionOptions to be passed into the class without changing API. - * - * \param node The rclcpp::Node::SharedPtr to use to subscribe. - * \param topic The topic to subscribe to. - * \param qos (optional) The rclcpp qos profile to use to subscribe. - * \param options The subscription options to use to subscribe. - */ - virtual void subscribe( - NodePtr node, - const std::string & topic, - const rclcpp::QoS & qos, - rclcpp::SubscriptionOptions options) - { - this->subscribe(node.get(), topic, qos, options); - } - - /** - * \brief Subscribe to a topic. - * - * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. - * - * \param node The rclcpp::Node to use to subscribe. - * \param topic The topic to subscribe to. - * \param qos The rmw qos profile to use to subscribe. - * \param options The subscription options to use to subscribe. - */ - virtual void subscribe( - NodeType * node, - const std::string & topic, - const rclcpp::QoS & qos, - rclcpp::SubscriptionOptions options) = 0; - - /** - * \brief Subscribe to a topic. - * - * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. - * - * \param node The rclcpp::Node::SharedPtr to use to subscribe. - * \param topic The topic to subscribe to. - * \param qos (optional) The rmw qos profile to use to subscribe - */ - [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] - 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. - * - * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. - * - * \param node The rclcpp::Node to use to subscribe. - * \param topic The topic to subscribe to. - * \param qos (optional) The rmw qos profile to use to subscribe - */ - [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] - 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. - * - * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. - * This override allows SubscriptionOptions to be passed into the class without changing API. - * - * \param node The rclcpp::Node::SharedPtr to use to subscribe. - * \param topic The topic to subscribe to. - * \param qos (optional) The rmw qos profile to use to subscribe. - * \param options The subscription options to use to subscribe. - */ - [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] - virtual void subscribe( - NodePtr node, - const std::string & topic, - const rmw_qos_profile_t qos, - rclcpp::SubscriptionOptions options) - { - this->subscribe( - node.get(), topic, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos)), options); - } - - /** - * \brief Subscribe to a topic. - * - * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. - * - * \param node The rclcpp::Node to use to subscribe. - * \param topic The topic to subscribe to. - * \param qos The rmw qos profile to use to subscribe. - * \param options The subscription options to use to subscribe. - */ - [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] - virtual void subscribe( - NodeType * node, - const std::string & topic, - const rmw_qos_profile_t qos, - rclcpp::SubscriptionOptions options) = 0; - - /** - * \brief Re-subscribe to a topic. Only works if this subscriber has previously been subscribed to a topic. - */ - virtual void subscribe() = 0; - /** - * \brief Force immediate unsubscription of this subscriber from its topic - */ - virtual void unsubscribe() = 0; -}; -template -using SubscriberBasePtr = std::shared_ptr>; - -/** - * \brief ROS subscription filter. - * - * This class acts as a highest-level filter, simply passing messages from a ROS subscription through to the - * filters which have connected to it. - * - * When this object is destroyed it will unsubscribe from the ROS subscription. - * - * The Subscriber object is templated on the type of message being subscribed to. - * - * \section connections CONNECTIONS - * - * Subscriber has no input connection. - * - * The output connection for the Subscriber object is the same signature as for rclcpp subscription callbacks, ie. -\verbatim -void callback(const std::shared_ptr &); -\endverbatim - */ - -template::value> -struct message_type; - -template -struct message_type -{ - using type = typename M::custom_type; -}; - -template -struct message_type -{ - using type = M; -}; - -template -using message_type_t = typename message_type::type; - -template -class Subscriber - : public SubscriberBase, - public SimpleFilter> -{ -public: - typedef std::shared_ptr NodePtr; - typedef message_type_t MessageType; - typedef MessageEvent EventType; - /** - * \brief Constructor - * - * See the rclcpp::Node::subscribe() variants for more information on the parameters - * - * \param node The rclcpp::Node::SharedPtr to use to subscribe. - * \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 rclcpp::QoS & qos) - { - subscribe(node, topic, qos); - } - - Subscriber( - NodeType * node, const std::string & topic, - const rclcpp::QoS & qos) - { - subscribe(node, topic, qos); - } - - /** - * \brief Constructor - * - * See the rclcpp::Node::subscribe() variants for more information on the parameters - * - * \param node The rclcpp::Node::SharedPtr to use to subscribe. - * \param topic The topic to subscribe to. - * \param qos The rmw qos profile to use to subscribe. - * \param options The subscription options to use to subscribe. - */ - Subscriber( - NodePtr node, - const std::string & topic, - const rclcpp::QoS & qos, - rclcpp::SubscriptionOptions options) - { - subscribe(node.get(), topic, qos, options); - } - - Subscriber( - NodeType * node, - const std::string & topic, - const rclcpp::QoS & qos, - rclcpp::SubscriptionOptions options) - { - subscribe(node, topic, qos, options); - } - - /** - * \brief Constructor - * - * See the rclcpp::Node::subscribe() variants for more information on the parameters - * - * \param node The rclcpp::Node::SharedPtr to use to subscribe. - * \param topic The topic to subscribe to. - * \param qos (optional) The rmw qos profile to use to subscribe - */ - [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] - Subscriber( - NodePtr node, const std::string & topic, - const rmw_qos_profile_t qos = rmw_qos_profile_default) - { - subscribe(node, topic, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos))); - } - - [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] - Subscriber( - NodeType * node, const std::string & topic, - const rmw_qos_profile_t qos = rmw_qos_profile_default) - { - subscribe(node, topic, qos); - } - - /** - * \brief Constructor - * - * See the rclcpp::Node::subscribe() variants for more information on the parameters - * - * \param node The rclcpp::Node::SharedPtr to use to subscribe. - * \param topic The topic to subscribe to. - * \param qos The rmw qos profile to use to subscribe. - * \param options The subscription options to use to subscribe. - */ - [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] - Subscriber( - NodePtr node, - const std::string & topic, - const rmw_qos_profile_t qos, - rclcpp::SubscriptionOptions options) - { - subscribe(node.get(), topic, qos, options); - } - - [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] - Subscriber( - NodeType * node, - const std::string & topic, - const rmw_qos_profile_t qos, - rclcpp::SubscriptionOptions options) - { - subscribe(node, topic, qos, options); - } - - /** - * \brief Empty constructor, use subscribe() to subscribe to a topic - */ - Subscriber() = default; - - ~Subscriber() - { - unsubscribe(); - } - - /** - * \brief Subscribe to a topic. - * - * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. - * - * \param node The rclcpp::Node::SharedPtr to use to subscribe. - * \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 rclcpp::QoS & qos) override - { - subscribe(node.get(), topic, qos, rclcpp::SubscriptionOptions()); - } - - /** - * \brief Subscribe to a topic. - * - * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. - * - * \param node The rclcpp::Node to use to subscribe. - * \param topic The topic to subscribe to. - * \param qos (optional) The rmw qos profile to use to subscribe - */ - void subscribe( - NodeType * node, const std::string & topic, - const rclcpp::QoS & qos) override - { - subscribe(node, topic, qos, rclcpp::SubscriptionOptions()); - } - - /** - * \brief Subscribe to a topic. - * - * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. - * - * \param node The rclcpp::Node::SharedPtr to use to subscribe. - * \param topic The topic to subscribe to. - * \param qos The rmw qos profile to use to subscribe. - * \param options The subscription options to use to subscribe. - */ - void subscribe( - NodePtr node, - const std::string & topic, - const rclcpp::QoS & qos, - rclcpp::SubscriptionOptions options) override - { - subscribe(node.get(), topic, qos, options); - node_raw_ = nullptr; - node_shared_ = node; - } - - /** - * \brief Subscribe to a topic. - * - * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. - * - * \param node The rclcpp::Node to use to subscribe. - * \param topic The topic to subscribe to. - * \param qos The rmw qos profile to use to subscribe - * \param options The subscription options to use to subscribe. - */ - void subscribe( - NodeType * node, - const std::string & topic, - const rclcpp::QoS & qos, - rclcpp::SubscriptionOptions options) override - { - unsubscribe(); - - if (!topic.empty()) { - topic_ = topic; - qos_ = qos; - options_ = options; - sub_ = node->template create_subscription( - topic, qos, - [this](const std::shared_ptr msg) { - this->cb(EventType(msg)); - }, options); - - node_raw_ = node; - } - } - - /** - * \brief Subscribe to a topic. - * - * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. - * - * \param node The rclcpp::Node::SharedPtr to use to subscribe. - * \param topic The topic to subscribe to. - * \param qos (optional) The rmw qos profile to use to subscribe - */ - [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] - void subscribe( - NodePtr node, const std::string & topic, - const rmw_qos_profile_t qos = rmw_qos_profile_default) override - { - subscribe( - node.get(), topic, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos)), - rclcpp::SubscriptionOptions()); - } - - /** - * \brief Subscribe to a topic. - * - * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. - * - * \param node The rclcpp::Node to use to subscribe. - * \param topic The topic to subscribe to. - * \param qos (optional) The rmw qos profile to use to subscribe - */ - [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] - void subscribe( - NodeType * node, const std::string & topic, - const rmw_qos_profile_t qos = rmw_qos_profile_default) override - { - subscribe( - node, topic, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos)), - rclcpp::SubscriptionOptions()); - } - - /** - * \brief Subscribe to a topic. - * - * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. - * - * \param node The rclcpp::Node::SharedPtr to use to subscribe. - * \param topic The topic to subscribe to. - * \param qos The rmw qos profile to use to subscribe. - * \param options The subscription options to use to subscribe. - */ - [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] - void subscribe( - NodePtr node, - const std::string & topic, - const rmw_qos_profile_t qos, - rclcpp::SubscriptionOptions options) override - { - subscribe(node.get(), topic, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos)), options); - node_raw_ = nullptr; - node_shared_ = node; - } - - /** - * \brief Subscribe to a topic. - * - * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. - * - * \param node The rclcpp::Node to use to subscribe. - * \param topic The topic to subscribe to. - * \param qos The rmw qos profile to use to subscribe - * \param options The subscription options to use to subscribe. - */ - [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] - void subscribe( - NodeType * node, - const std::string & topic, - const rmw_qos_profile_t qos, - rclcpp::SubscriptionOptions options) override - { - unsubscribe(); - - if (!topic.empty()) { - topic_ = topic; - rclcpp::QoS rclcpp_qos(rclcpp::QoSInitialization::from_rmw(qos)); - qos_ = rclcpp_qos; - options_ = options; - sub_ = node->template create_subscription( - topic, rclcpp_qos, - [this](const std::shared_ptr msg) { - this->cb(EventType(msg)); - }, options); - - node_raw_ = node; - } - } - - /** - * \brief Re-subscribe to a topic. Only works if this subscriber has previously been subscribed to a topic. - */ - void subscribe() override - { - if (!topic_.empty()) { - if (node_raw_ != nullptr) { - subscribe(node_raw_, topic_, qos_, options_); - } else if (node_shared_ != nullptr) { - subscribe(node_shared_, topic_, qos_, options_); - } - } - } - - /** - * \brief Force immediate unsubscription of this subscriber from its topic - */ - void unsubscribe() override - { - sub_.reset(); - } - - std::string getTopic() const - { - return this->topic_; - } - - /** - * \brief Returns the internal rclcpp::Subscription::SharedPtr object - */ - 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)f; - } - - /** - * \brief Does nothing. Provided so that Subscriber may be used in a message_filters::Chain - */ - void add(const EventType & e) - { - (void)e; - } - -private: - void cb(const EventType & e) - { - this->signalMessage(e); - } - - typename rclcpp::Subscription::SharedPtr sub_; - - NodePtr node_shared_; - NodeType * node_raw_ {nullptr}; - - std::string topic_; - rclcpp::QoS qos_ = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); - rclcpp::SubscriptionOptions options_; -}; +#warning This header is obsolete, please include message_filters/subscriber.hpp instead -} // namespace message_filters +#include #endif // MESSAGE_FILTERS__SUBSCRIBER_H_ diff --git a/include/message_filters/subscriber.hpp b/include/message_filters/subscriber.hpp new file mode 100644 index 0000000..73471b2 --- /dev/null +++ b/include/message_filters/subscriber.hpp @@ -0,0 +1,602 @@ +// Copyright 2009, Willow Garage, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef MESSAGE_FILTERS__SUBSCRIBER_HPP_ +#define MESSAGE_FILTERS__SUBSCRIBER_HPP_ + +#include +#include +#include +#include + +#include + +#include "message_filters/connection.hpp" +#include "message_filters/simple_filter.hpp" + +namespace message_filters +{ + +template +class SubscriberBase +{ +public: + typedef std::shared_ptr NodePtr; + + virtual ~SubscriberBase() = default; + /** + * \brief Subscribe to a topic. + * + * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. + * + * \param node The rclcpp::Node::SharedPtr to use to subscribe. + * \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 rclcpp::QoS & qos) = 0; + + /** + * \brief Subscribe to a topic. + * + * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. + * + * \param node The rclcpp::Node to use to subscribe. + * \param topic The topic to subscribe to. + * \param qos (optional) The rclcpp::rmw qos profile to use to subscribe + */ + virtual void subscribe( + NodeType * node, const std::string & topic, + const rclcpp::QoS & qos) = 0; + + /** + * \brief Subscribe to a topic. + * + * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. + * This override allows SubscriptionOptions to be passed into the class without changing API. + * + * \param node The rclcpp::Node::SharedPtr to use to subscribe. + * \param topic The topic to subscribe to. + * \param qos (optional) The rclcpp qos profile to use to subscribe. + * \param options The subscription options to use to subscribe. + */ + virtual void subscribe( + NodePtr node, + const std::string & topic, + const rclcpp::QoS & qos, + rclcpp::SubscriptionOptions options) + { + this->subscribe(node.get(), topic, qos, options); + } + + /** + * \brief Subscribe to a topic. + * + * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. + * + * \param node The rclcpp::Node to use to subscribe. + * \param topic The topic to subscribe to. + * \param qos The rmw qos profile to use to subscribe. + * \param options The subscription options to use to subscribe. + */ + virtual void subscribe( + NodeType * node, + const std::string & topic, + const rclcpp::QoS & qos, + rclcpp::SubscriptionOptions options) = 0; + + /** + * \brief Subscribe to a topic. + * + * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. + * + * \param node The rclcpp::Node::SharedPtr to use to subscribe. + * \param topic The topic to subscribe to. + * \param qos (optional) The rmw qos profile to use to subscribe + */ + [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] + 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. + * + * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. + * + * \param node The rclcpp::Node to use to subscribe. + * \param topic The topic to subscribe to. + * \param qos (optional) The rmw qos profile to use to subscribe + */ + [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] + 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. + * + * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. + * This override allows SubscriptionOptions to be passed into the class without changing API. + * + * \param node The rclcpp::Node::SharedPtr to use to subscribe. + * \param topic The topic to subscribe to. + * \param qos (optional) The rmw qos profile to use to subscribe. + * \param options The subscription options to use to subscribe. + */ + [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] + virtual void subscribe( + NodePtr node, + const std::string & topic, + const rmw_qos_profile_t qos, + rclcpp::SubscriptionOptions options) + { + this->subscribe( + node.get(), topic, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos)), options); + } + + /** + * \brief Subscribe to a topic. + * + * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. + * + * \param node The rclcpp::Node to use to subscribe. + * \param topic The topic to subscribe to. + * \param qos The rmw qos profile to use to subscribe. + * \param options The subscription options to use to subscribe. + */ + [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] + virtual void subscribe( + NodeType * node, + const std::string & topic, + const rmw_qos_profile_t qos, + rclcpp::SubscriptionOptions options) = 0; + + /** + * \brief Re-subscribe to a topic. Only works if this subscriber has previously been subscribed to a topic. + */ + virtual void subscribe() = 0; + /** + * \brief Force immediate unsubscription of this subscriber from its topic + */ + virtual void unsubscribe() = 0; +}; +template +using SubscriberBasePtr = std::shared_ptr>; + +/** + * \brief ROS subscription filter. + * + * This class acts as a highest-level filter, simply passing messages from a ROS subscription through to the + * filters which have connected to it. + * + * When this object is destroyed it will unsubscribe from the ROS subscription. + * + * The Subscriber object is templated on the type of message being subscribed to. + * + * \section connections CONNECTIONS + * + * Subscriber has no input connection. + * + * The output connection for the Subscriber object is the same signature as for rclcpp subscription callbacks, ie. +\verbatim +void callback(const std::shared_ptr &); +\endverbatim + */ + +template::value> +struct message_type; + +template +struct message_type +{ + using type = typename M::custom_type; +}; + +template +struct message_type +{ + using type = M; +}; + +template +using message_type_t = typename message_type::type; + +template +class Subscriber + : public SubscriberBase, + public SimpleFilter> +{ +public: + typedef std::shared_ptr NodePtr; + typedef message_type_t MessageType; + typedef MessageEvent EventType; + /** + * \brief Constructor + * + * See the rclcpp::Node::subscribe() variants for more information on the parameters + * + * \param node The rclcpp::Node::SharedPtr to use to subscribe. + * \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 rclcpp::QoS & qos) + { + subscribe(node, topic, qos); + } + + Subscriber( + NodeType * node, const std::string & topic, + const rclcpp::QoS & qos) + { + subscribe(node, topic, qos); + } + + /** + * \brief Constructor + * + * See the rclcpp::Node::subscribe() variants for more information on the parameters + * + * \param node The rclcpp::Node::SharedPtr to use to subscribe. + * \param topic The topic to subscribe to. + * \param qos The rmw qos profile to use to subscribe. + * \param options The subscription options to use to subscribe. + */ + Subscriber( + NodePtr node, + const std::string & topic, + const rclcpp::QoS & qos, + rclcpp::SubscriptionOptions options) + { + subscribe(node.get(), topic, qos, options); + } + + Subscriber( + NodeType * node, + const std::string & topic, + const rclcpp::QoS & qos, + rclcpp::SubscriptionOptions options) + { + subscribe(node, topic, qos, options); + } + + /** + * \brief Constructor + * + * See the rclcpp::Node::subscribe() variants for more information on the parameters + * + * \param node The rclcpp::Node::SharedPtr to use to subscribe. + * \param topic The topic to subscribe to. + * \param qos (optional) The rmw qos profile to use to subscribe + */ + [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] + Subscriber( + NodePtr node, const std::string & topic, + const rmw_qos_profile_t qos = rmw_qos_profile_default) + { + subscribe(node, topic, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos))); + } + + [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] + Subscriber( + NodeType * node, const std::string & topic, + const rmw_qos_profile_t qos = rmw_qos_profile_default) + { + subscribe(node, topic, qos); + } + + /** + * \brief Constructor + * + * See the rclcpp::Node::subscribe() variants for more information on the parameters + * + * \param node The rclcpp::Node::SharedPtr to use to subscribe. + * \param topic The topic to subscribe to. + * \param qos The rmw qos profile to use to subscribe. + * \param options The subscription options to use to subscribe. + */ + [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] + Subscriber( + NodePtr node, + const std::string & topic, + const rmw_qos_profile_t qos, + rclcpp::SubscriptionOptions options) + { + subscribe(node.get(), topic, qos, options); + } + + [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] + Subscriber( + NodeType * node, + const std::string & topic, + const rmw_qos_profile_t qos, + rclcpp::SubscriptionOptions options) + { + subscribe(node, topic, qos, options); + } + + /** + * \brief Empty constructor, use subscribe() to subscribe to a topic + */ + Subscriber() = default; + + ~Subscriber() + { + unsubscribe(); + } + + /** + * \brief Subscribe to a topic. + * + * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. + * + * \param node The rclcpp::Node::SharedPtr to use to subscribe. + * \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 rclcpp::QoS & qos) override + { + subscribe(node.get(), topic, qos, rclcpp::SubscriptionOptions()); + } + + /** + * \brief Subscribe to a topic. + * + * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. + * + * \param node The rclcpp::Node to use to subscribe. + * \param topic The topic to subscribe to. + * \param qos (optional) The rmw qos profile to use to subscribe + */ + void subscribe( + NodeType * node, const std::string & topic, + const rclcpp::QoS & qos) override + { + subscribe(node, topic, qos, rclcpp::SubscriptionOptions()); + } + + /** + * \brief Subscribe to a topic. + * + * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. + * + * \param node The rclcpp::Node::SharedPtr to use to subscribe. + * \param topic The topic to subscribe to. + * \param qos The rmw qos profile to use to subscribe. + * \param options The subscription options to use to subscribe. + */ + void subscribe( + NodePtr node, + const std::string & topic, + const rclcpp::QoS & qos, + rclcpp::SubscriptionOptions options) override + { + subscribe(node.get(), topic, qos, options); + node_raw_ = nullptr; + node_shared_ = node; + } + + /** + * \brief Subscribe to a topic. + * + * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. + * + * \param node The rclcpp::Node to use to subscribe. + * \param topic The topic to subscribe to. + * \param qos The rmw qos profile to use to subscribe + * \param options The subscription options to use to subscribe. + */ + void subscribe( + NodeType * node, + const std::string & topic, + const rclcpp::QoS & qos, + rclcpp::SubscriptionOptions options) override + { + unsubscribe(); + + if (!topic.empty()) { + topic_ = topic; + qos_ = qos; + options_ = options; + sub_ = node->template create_subscription( + topic, qos, + [this](const std::shared_ptr msg) { + this->cb(EventType(msg)); + }, options); + + node_raw_ = node; + } + } + + /** + * \brief Subscribe to a topic. + * + * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. + * + * \param node The rclcpp::Node::SharedPtr to use to subscribe. + * \param topic The topic to subscribe to. + * \param qos (optional) The rmw qos profile to use to subscribe + */ + [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] + void subscribe( + NodePtr node, const std::string & topic, + const rmw_qos_profile_t qos = rmw_qos_profile_default) override + { + subscribe( + node.get(), topic, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos)), + rclcpp::SubscriptionOptions()); + } + + /** + * \brief Subscribe to a topic. + * + * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. + * + * \param node The rclcpp::Node to use to subscribe. + * \param topic The topic to subscribe to. + * \param qos (optional) The rmw qos profile to use to subscribe + */ + [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] + void subscribe( + NodeType * node, const std::string & topic, + const rmw_qos_profile_t qos = rmw_qos_profile_default) override + { + subscribe( + node, topic, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos)), + rclcpp::SubscriptionOptions()); + } + + /** + * \brief Subscribe to a topic. + * + * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. + * + * \param node The rclcpp::Node::SharedPtr to use to subscribe. + * \param topic The topic to subscribe to. + * \param qos The rmw qos profile to use to subscribe. + * \param options The subscription options to use to subscribe. + */ + [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] + void subscribe( + NodePtr node, + const std::string & topic, + const rmw_qos_profile_t qos, + rclcpp::SubscriptionOptions options) override + { + subscribe(node.get(), topic, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos)), options); + node_raw_ = nullptr; + node_shared_ = node; + } + + /** + * \brief Subscribe to a topic. + * + * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. + * + * \param node The rclcpp::Node to use to subscribe. + * \param topic The topic to subscribe to. + * \param qos The rmw qos profile to use to subscribe + * \param options The subscription options to use to subscribe. + */ + [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] + void subscribe( + NodeType * node, + const std::string & topic, + const rmw_qos_profile_t qos, + rclcpp::SubscriptionOptions options) override + { + unsubscribe(); + + if (!topic.empty()) { + topic_ = topic; + rclcpp::QoS rclcpp_qos(rclcpp::QoSInitialization::from_rmw(qos)); + qos_ = rclcpp_qos; + options_ = options; + sub_ = node->template create_subscription( + topic, rclcpp_qos, + [this](const std::shared_ptr msg) { + this->cb(EventType(msg)); + }, options); + + node_raw_ = node; + } + } + + /** + * \brief Re-subscribe to a topic. Only works if this subscriber has previously been subscribed to a topic. + */ + void subscribe() override + { + if (!topic_.empty()) { + if (node_raw_ != nullptr) { + subscribe(node_raw_, topic_, qos_, options_); + } else if (node_shared_ != nullptr) { + subscribe(node_shared_, topic_, qos_, options_); + } + } + } + + /** + * \brief Force immediate unsubscription of this subscriber from its topic + */ + void unsubscribe() override + { + sub_.reset(); + } + + std::string getTopic() const + { + return this->topic_; + } + + /** + * \brief Returns the internal rclcpp::Subscription::SharedPtr object + */ + 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)f; + } + + /** + * \brief Does nothing. Provided so that Subscriber may be used in a message_filters::Chain + */ + void add(const EventType & e) + { + (void)e; + } + +private: + void cb(const EventType & e) + { + this->signalMessage(e); + } + + typename rclcpp::Subscription::SharedPtr sub_; + + NodePtr node_shared_; + NodeType * node_raw_ {nullptr}; + + std::string topic_; + rclcpp::QoS qos_ = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); + rclcpp::SubscriptionOptions options_; +}; + +} // namespace message_filters + +#endif // MESSAGE_FILTERS__SUBSCRIBER_HPP_ diff --git a/include/message_filters/sync_policies/approximate_epsilon_time.h b/include/message_filters/sync_policies/approximate_epsilon_time.h index 1ae6630..33eb411 100644 --- a/include/message_filters/sync_policies/approximate_epsilon_time.h +++ b/include/message_filters/sync_policies/approximate_epsilon_time.h @@ -1,351 +1,23 @@ -// Copyright 2022, Open Source Robotics Foundation, Inc. All rights reserved. +// Copyright 2024 Open Source Robotics Foundation, Inc. All rights reserved. // -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. +// http://www.apache.org/licenses/LICENSE-2.0 // -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the Willow Garage nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #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 - -#include "message_filters/connection.h" -#include "message_filters/message_traits.h" -#include "message_filters/null_types.h" -#include "message_filters/signal9.h" -#include "message_filters/synchronizer.h" - -namespace message_filters -{ -namespace sync_policies -{ - -template -class ApproximateEpsilonTime : public PolicyBase -{ -public: - typedef Synchronizer Sync; - typedef PolicyBase Super; - typedef typename Super::Messages Messages; - typedef typename Super::Signal Signal; - typedef typename Super::Events Events; - typedef typename Super::RealTypeCount RealTypeCount; - typedef typename Super::M0Event M0Event; - typedef typename Super::M1Event M1Event; - typedef typename Super::M2Event M2Event; - typedef typename Super::M3Event M3Event; - typedef typename Super::M4Event M4Event; - typedef typename Super::M5Event M5Event; - typedef typename Super::M6Event M6Event; - typedef typename Super::M7Event M7Event; - typedef typename Super::M8Event M8Event; - typedef Events Tuple; - - ApproximateEpsilonTime(uint32_t queue_size, rclcpp::Duration epsilon) - : parent_(nullptr) - , queue_size_(queue_size) - , epsilon_{epsilon} - { - } - - ApproximateEpsilonTime(const ApproximateEpsilonTime & e) - : epsilon_{e.epsilon_} - { - *this = e; - } - - ApproximateEpsilonTime & operator=(const ApproximateEpsilonTime & rhs) - { - parent_ = rhs.parent_; - queue_size_ = rhs.queue_size_; - events_ = rhs.events_; - epsilon_ = rhs.epsilon_; - - return *this; - } - - void initParent(Sync * parent) - { - parent_ = parent; - } - - template - void add(const typename std::tuple_element::type & evt) - { - assert(parent_); - - std::lock_guard lock(mutex_); - - auto & events_of_this_type = std::get(events_); - if (0u == events_of_this_type.size()) { - ++number_of_non_empty_events_; - } - events_of_this_type.push_back(evt); - if (number_of_non_empty_events_ == RealTypeCount::value) { - process(); - } else if (events_of_this_type.size() > queue_size_) { - erase_beginning_of_vector(); - } - } - -private: - using TimeIndexPair = std::pair; - - template - TimeIndexPair - get_older_timestamp_between(const TimeIndexPair & current) - { - namespace mt = message_filters::message_traits; - using ThisEventType = typename std::tuple_element::type; - if constexpr (Is >= RealTypeCount::value) { - return current; - } - const auto & events_of_this_type = std::get(events_); - if (0u == events_of_this_type.size()) { - // this condition should not happen - return current; - } - auto candidate = mt::TimeStamp::value( - *events_of_this_type.at(0).getMessage()); - if (current.first > candidate) { - return std::make_pair(candidate, Is); - } - return current; - } - - 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()}; - ((older = get_older_timestamp_between(older)), ...); - return older; - } - - TimeIndexPair - get_older_timestamp() - { - return get_older_timestamp_helper(std::make_index_sequence<9u>()); - } - - template - bool - check_timestamp_within_epsilon(const TimeIndexPair & older) - { - namespace mt = message_filters::message_traits; - using ThisEventType = typename std::tuple_element::type; - if constexpr (Is >= RealTypeCount::value) { - return true; - } - if (Is == older.second) { - return true; - } - const auto & events_of_this_type = std::get(events_); - if (0u == events_of_this_type.size()) { - // this condition should not happen - return false; - } - auto ts = mt::TimeStamp::value( - *events_of_this_type.at(0).getMessage()); - if (older.first + epsilon_ >= ts) { - return true; - } - return false; - } - - template - bool - check_all_timestamp_within_epsilon_helper( - const TimeIndexPair & older, std::index_sequence const &) - { - bool valid = true; - ((valid &= check_timestamp_within_epsilon(older)), ...); - return valid; - } - - bool - check_all_timestamp_within_epsilon(const TimeIndexPair & older) - { - return check_all_timestamp_within_epsilon_helper( - older, std::make_index_sequence<9u>()); - } - - template - void - erase_beginning_of_vector() - { - if constexpr (Is >= RealTypeCount::value) { - return; - } - auto & this_vector = std::get(events_); - if (this_vector.begin() != this_vector.end()) { - this_vector.erase(this_vector.begin()); - if (this_vector.empty()) { - --number_of_non_empty_events_; - } - } - } - - template - void - erase_beginning_of_vectors_helper(std::index_sequence const &) - { - ((erase_beginning_of_vector()), ...); - } - - void erase_beginning_of_vectors() - { - return erase_beginning_of_vectors_helper(std::make_index_sequence<9u>()); - } - - template - void - erase_beginning_of_vector_if_on_sync_with_ts(rclcpp::Time timestamp) - { - namespace mt = message_filters::message_traits; - using ThisEventType = typename std::tuple_element::type; - if constexpr (Is >= RealTypeCount::value) { - return; - } - auto & this_vector = std::get(events_); - if (this_vector.begin() == this_vector.end()) { - return; - } - auto event_ts = mt::TimeStamp::value( - *this_vector.at(0).getMessage()); - if (timestamp + epsilon_ < event_ts) { - return; - } - this_vector.erase(this_vector.begin()); - if (this_vector.empty()) { - --number_of_non_empty_events_; - } - } - - template - void - 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)), ...); - } - - void erase_old_events_if_on_sync_with_ts(rclcpp::Time timestamp) - { - return erase_old_events_if_on_sync_with_ts_helper(timestamp, std::make_index_sequence<9u>()); - } - - void signal() - { - if constexpr (RealTypeCount::value == 2) { - parent_->signal( - std::get<0>(events_).at(0), std::get<1>(events_).at(0), - M2Event{}, M3Event{}, M4Event{}, M5Event{}, M6Event{}, M7Event{}, M8Event{}); - } else if constexpr (RealTypeCount::value == 3) { - parent_->signal( - std::get<0>(events_).at(0), std::get<1>(events_).at(0), std::get<2>(events_).at(0), - M3Event{}, M4Event{}, M5Event{}, M6Event{}, M7Event{}, M8Event{}); - } else if constexpr (RealTypeCount::value == 4) { - parent_->signal( - std::get<0>(events_).at(0), std::get<1>(events_).at(0), std::get<2>(events_).at(0), - std::get<3>(events_).at(0), - M4Event{}, M5Event{}, M6Event{}, M7Event{}, M8Event{}); - } else if constexpr (RealTypeCount::value == 5) { - parent_->signal( - std::get<0>(events_).at(0), std::get<1>(events_).at(0), std::get<2>(events_).at(0), - std::get<3>(events_).at(0), std::get<4>(events_).at(0), - M5Event{}, M6Event{}, M7Event{}, M8Event{}); - } else if constexpr (RealTypeCount::value == 6) { - parent_->signal( - std::get<0>(events_).at(0), std::get<1>(events_).at(0), std::get<2>(events_).at(0), - std::get<3>(events_).at(0), std::get<4>(events_).at(0), std::get<5>(events_).at(0), - M6Event{}, M7Event{}, M8Event{}); - } else if constexpr (RealTypeCount::value == 7) { - parent_->signal( - std::get<0>(events_).at(0), std::get<1>(events_).at(0), std::get<2>(events_).at(0), - std::get<3>(events_).at(0), std::get<4>(events_).at(0), std::get<5>(events_).at(0), - std::get<6>(events_).at(0), - M7Event{}, M8Event{}); - } else if constexpr (RealTypeCount::value == 8) { - parent_->signal( - std::get<0>(events_).at(0), std::get<1>(events_).at(0), std::get<2>(events_).at(0), - std::get<3>(events_).at(0), std::get<4>(events_).at(0), std::get<5>(events_).at(0), - std::get<6>(events_).at(0), std::get<7>(events_).at(0), - M8Event{}); - } else if constexpr (RealTypeCount::value == 9) { - parent_->signal( - std::get<0>(events_).at(0), std::get<1>(events_).at(0), std::get<2>(events_).at(0), - std::get<3>(events_).at(0), std::get<4>(events_).at(0), std::get<5>(events_).at(0), - std::get<6>(events_).at(0), std::get<7>(events_).at(0), std::get<8>(events_).at(0)); - } else { - static_assert("RealTypeCount::value should be >=2 and <=9"); - } - } - - // assumes mutex_ is already locked - void process() - { - while (number_of_non_empty_events_ == RealTypeCount::value) { - auto old_ts = get_older_timestamp(); - if (check_all_timestamp_within_epsilon(old_ts)) { - signal(); - erase_beginning_of_vectors(); - } else { - erase_old_events_if_on_sync_with_ts(old_ts.first); - } - } - } - - Sync * parent_; - - uint32_t queue_size_; - rclcpp::Duration epsilon_; - size_t number_of_non_empty_events_{0}; - using TupleOfVecOfEvents = std::tuple< - std::vector, std::vector, std::vector, - std::vector, std::vector, std::vector, - std::vector, std::vector, std::vector>; - TupleOfVecOfEvents events_; - - std::mutex mutex_; -}; +#warning This header is obsolete, please include message_filters/approximate_epsilon_time.hpp \ + header -} // namespace sync_policies -} // namespace message_filters +#include #endif // MESSAGE_FILTERS__SYNC_POLICIES__APPROXIMATE_EPSILON_TIME_H_ diff --git a/include/message_filters/sync_policies/approximate_epsilon_time.hpp b/include/message_filters/sync_policies/approximate_epsilon_time.hpp new file mode 100644 index 0000000..84324b4 --- /dev/null +++ b/include/message_filters/sync_policies/approximate_epsilon_time.hpp @@ -0,0 +1,351 @@ +// Copyright 2022, Open Source Robotics Foundation, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef MESSAGE_FILTERS__SYNC_POLICIES__APPROXIMATE_EPSILON_TIME_HPP_ +#define MESSAGE_FILTERS__SYNC_POLICIES__APPROXIMATE_EPSILON_TIME_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "message_filters/connection.hpp" +#include "message_filters/message_traits.hpp" +#include "message_filters/null_types.hpp" +#include "message_filters/signal9.hpp" +#include "message_filters/synchronizer.hpp" + +namespace message_filters +{ +namespace sync_policies +{ + +template +class ApproximateEpsilonTime : public PolicyBase +{ +public: + typedef Synchronizer Sync; + typedef PolicyBase Super; + typedef typename Super::Messages Messages; + typedef typename Super::Signal Signal; + typedef typename Super::Events Events; + typedef typename Super::RealTypeCount RealTypeCount; + typedef typename Super::M0Event M0Event; + typedef typename Super::M1Event M1Event; + typedef typename Super::M2Event M2Event; + typedef typename Super::M3Event M3Event; + typedef typename Super::M4Event M4Event; + typedef typename Super::M5Event M5Event; + typedef typename Super::M6Event M6Event; + typedef typename Super::M7Event M7Event; + typedef typename Super::M8Event M8Event; + typedef Events Tuple; + + ApproximateEpsilonTime(uint32_t queue_size, rclcpp::Duration epsilon) + : parent_(nullptr) + , queue_size_(queue_size) + , epsilon_{epsilon} + { + } + + ApproximateEpsilonTime(const ApproximateEpsilonTime & e) + : epsilon_{e.epsilon_} + { + *this = e; + } + + ApproximateEpsilonTime & operator=(const ApproximateEpsilonTime & rhs) + { + parent_ = rhs.parent_; + queue_size_ = rhs.queue_size_; + events_ = rhs.events_; + epsilon_ = rhs.epsilon_; + + return *this; + } + + void initParent(Sync * parent) + { + parent_ = parent; + } + + template + void add(const typename std::tuple_element::type & evt) + { + assert(parent_); + + std::lock_guard lock(mutex_); + + auto & events_of_this_type = std::get(events_); + if (0u == events_of_this_type.size()) { + ++number_of_non_empty_events_; + } + events_of_this_type.push_back(evt); + if (number_of_non_empty_events_ == RealTypeCount::value) { + process(); + } else if (events_of_this_type.size() > queue_size_) { + erase_beginning_of_vector(); + } + } + +private: + using TimeIndexPair = std::pair; + + template + TimeIndexPair + get_older_timestamp_between(const TimeIndexPair & current) + { + namespace mt = message_filters::message_traits; + using ThisEventType = typename std::tuple_element::type; + if constexpr (Is >= RealTypeCount::value) { + return current; + } + const auto & events_of_this_type = std::get(events_); + if (0u == events_of_this_type.size()) { + // this condition should not happen + return current; + } + auto candidate = mt::TimeStamp::value( + *events_of_this_type.at(0).getMessage()); + if (current.first > candidate) { + return std::make_pair(candidate, Is); + } + return current; + } + + 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()}; + ((older = get_older_timestamp_between(older)), ...); + return older; + } + + TimeIndexPair + get_older_timestamp() + { + return get_older_timestamp_helper(std::make_index_sequence<9u>()); + } + + template + bool + check_timestamp_within_epsilon(const TimeIndexPair & older) + { + namespace mt = message_filters::message_traits; + using ThisEventType = typename std::tuple_element::type; + if constexpr (Is >= RealTypeCount::value) { + return true; + } + if (Is == older.second) { + return true; + } + const auto & events_of_this_type = std::get(events_); + if (0u == events_of_this_type.size()) { + // this condition should not happen + return false; + } + auto ts = mt::TimeStamp::value( + *events_of_this_type.at(0).getMessage()); + if (older.first + epsilon_ >= ts) { + return true; + } + return false; + } + + template + bool + check_all_timestamp_within_epsilon_helper( + const TimeIndexPair & older, std::index_sequence const &) + { + bool valid = true; + ((valid &= check_timestamp_within_epsilon(older)), ...); + return valid; + } + + bool + check_all_timestamp_within_epsilon(const TimeIndexPair & older) + { + return check_all_timestamp_within_epsilon_helper( + older, std::make_index_sequence<9u>()); + } + + template + void + erase_beginning_of_vector() + { + if constexpr (Is >= RealTypeCount::value) { + return; + } + auto & this_vector = std::get(events_); + if (this_vector.begin() != this_vector.end()) { + this_vector.erase(this_vector.begin()); + if (this_vector.empty()) { + --number_of_non_empty_events_; + } + } + } + + template + void + erase_beginning_of_vectors_helper(std::index_sequence const &) + { + ((erase_beginning_of_vector()), ...); + } + + void erase_beginning_of_vectors() + { + return erase_beginning_of_vectors_helper(std::make_index_sequence<9u>()); + } + + template + void + erase_beginning_of_vector_if_on_sync_with_ts(rclcpp::Time timestamp) + { + namespace mt = message_filters::message_traits; + using ThisEventType = typename std::tuple_element::type; + if constexpr (Is >= RealTypeCount::value) { + return; + } + auto & this_vector = std::get(events_); + if (this_vector.begin() == this_vector.end()) { + return; + } + auto event_ts = mt::TimeStamp::value( + *this_vector.at(0).getMessage()); + if (timestamp + epsilon_ < event_ts) { + return; + } + this_vector.erase(this_vector.begin()); + if (this_vector.empty()) { + --number_of_non_empty_events_; + } + } + + template + void + 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)), ...); + } + + void erase_old_events_if_on_sync_with_ts(rclcpp::Time timestamp) + { + return erase_old_events_if_on_sync_with_ts_helper(timestamp, std::make_index_sequence<9u>()); + } + + void signal() + { + if constexpr (RealTypeCount::value == 2) { + parent_->signal( + std::get<0>(events_).at(0), std::get<1>(events_).at(0), + M2Event{}, M3Event{}, M4Event{}, M5Event{}, M6Event{}, M7Event{}, M8Event{}); + } else if constexpr (RealTypeCount::value == 3) { + parent_->signal( + std::get<0>(events_).at(0), std::get<1>(events_).at(0), std::get<2>(events_).at(0), + M3Event{}, M4Event{}, M5Event{}, M6Event{}, M7Event{}, M8Event{}); + } else if constexpr (RealTypeCount::value == 4) { + parent_->signal( + std::get<0>(events_).at(0), std::get<1>(events_).at(0), std::get<2>(events_).at(0), + std::get<3>(events_).at(0), + M4Event{}, M5Event{}, M6Event{}, M7Event{}, M8Event{}); + } else if constexpr (RealTypeCount::value == 5) { + parent_->signal( + std::get<0>(events_).at(0), std::get<1>(events_).at(0), std::get<2>(events_).at(0), + std::get<3>(events_).at(0), std::get<4>(events_).at(0), + M5Event{}, M6Event{}, M7Event{}, M8Event{}); + } else if constexpr (RealTypeCount::value == 6) { + parent_->signal( + std::get<0>(events_).at(0), std::get<1>(events_).at(0), std::get<2>(events_).at(0), + std::get<3>(events_).at(0), std::get<4>(events_).at(0), std::get<5>(events_).at(0), + M6Event{}, M7Event{}, M8Event{}); + } else if constexpr (RealTypeCount::value == 7) { + parent_->signal( + std::get<0>(events_).at(0), std::get<1>(events_).at(0), std::get<2>(events_).at(0), + std::get<3>(events_).at(0), std::get<4>(events_).at(0), std::get<5>(events_).at(0), + std::get<6>(events_).at(0), + M7Event{}, M8Event{}); + } else if constexpr (RealTypeCount::value == 8) { + parent_->signal( + std::get<0>(events_).at(0), std::get<1>(events_).at(0), std::get<2>(events_).at(0), + std::get<3>(events_).at(0), std::get<4>(events_).at(0), std::get<5>(events_).at(0), + std::get<6>(events_).at(0), std::get<7>(events_).at(0), + M8Event{}); + } else if constexpr (RealTypeCount::value == 9) { + parent_->signal( + std::get<0>(events_).at(0), std::get<1>(events_).at(0), std::get<2>(events_).at(0), + std::get<3>(events_).at(0), std::get<4>(events_).at(0), std::get<5>(events_).at(0), + std::get<6>(events_).at(0), std::get<7>(events_).at(0), std::get<8>(events_).at(0)); + } else { + static_assert("RealTypeCount::value should be >=2 and <=9"); + } + } + + // assumes mutex_ is already locked + void process() + { + while (number_of_non_empty_events_ == RealTypeCount::value) { + auto old_ts = get_older_timestamp(); + if (check_all_timestamp_within_epsilon(old_ts)) { + signal(); + erase_beginning_of_vectors(); + } else { + erase_old_events_if_on_sync_with_ts(old_ts.first); + } + } + } + + Sync * parent_; + + uint32_t queue_size_; + rclcpp::Duration epsilon_; + size_t number_of_non_empty_events_{0}; + using TupleOfVecOfEvents = std::tuple< + std::vector, std::vector, std::vector, + std::vector, std::vector, std::vector, + std::vector, std::vector, std::vector>; + TupleOfVecOfEvents events_; + + std::mutex mutex_; +}; + +} // namespace sync_policies +} // namespace message_filters + +#endif // MESSAGE_FILTERS__SYNC_POLICIES__APPROXIMATE_EPSILON_TIME_HPP_ diff --git a/include/message_filters/sync_policies/approximate_time.h b/include/message_filters/sync_policies/approximate_time.h index f89d3d9..ec58db0 100644 --- a/include/message_filters/sync_policies/approximate_time.h +++ b/include/message_filters/sync_policies/approximate_time.h @@ -1,797 +1,22 @@ -// Copyright 2009, Willow Garage, Inc. All rights reserved. +// Copyright 2024 Open Source Robotics Foundation, Inc. All rights reserved. // -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. +// http://www.apache.org/licenses/LICENSE-2.0 // -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the Willow Garage nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef MESSAGE_FILTERS__SYNC_POLICIES__APPROXIMATE_TIME_H_ #define MESSAGE_FILTERS__SYNC_POLICIES__APPROXIMATE_TIME_H_ -#include - -#include - -#include -#include -#include -#include -#include -#include - -#include - -#include "message_filters/connection.h" -#include "message_filters/message_traits.h" -#include "message_filters/null_types.h" -#include "message_filters/signal9.h" -#include "message_filters/synchronizer.h" - -namespace message_filters -{ -namespace sync_policies -{ - -template -struct ApproximateTime : public PolicyBase -{ - typedef Synchronizer Sync; - typedef PolicyBase Super; - typedef typename Super::Messages Messages; - typedef typename Super::Signal Signal; - typedef typename Super::Events Events; - typedef typename Super::RealTypeCount RealTypeCount; - typedef typename Super::M0Event M0Event; - typedef typename Super::M1Event M1Event; - typedef typename Super::M2Event M2Event; - typedef typename Super::M3Event M3Event; - typedef typename Super::M4Event M4Event; - typedef typename Super::M5Event M5Event; - typedef typename Super::M6Event M6Event; - typedef typename Super::M7Event M7Event; - typedef typename Super::M8Event M8Event; - typedef std::deque M0Deque; - typedef std::deque M1Deque; - typedef std::deque M2Deque; - typedef std::deque M3Deque; - typedef std::deque M4Deque; - typedef std::deque M5Deque; - typedef std::deque M6Deque; - typedef std::deque M7Deque; - typedef std::deque M8Deque; - typedef std::vector M0Vector; - typedef std::vector M1Vector; - typedef std::vector M2Vector; - typedef std::vector M3Vector; - typedef std::vector M4Vector; - typedef std::vector M5Vector; - typedef std::vector M6Vector; - typedef std::vector M7Vector; - typedef std::vector M8Vector; - typedef Events Tuple; - typedef std::tuple DequeTuple; - typedef std::tuple VectorTuple; - - ApproximateTime(uint32_t queue_size) // NOLINT(runtime/explicit) - : 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) - { - // The synchronizer will tend to drop many messages with a queue size of 1. - // At least 2 is recommended. - assert(queue_size_ > 0); - } - - ApproximateTime(const ApproximateTime & e) - : max_interval_duration_(rclcpp::Duration(std::numeric_limits::max(), 999999999)) - { - *this = e; - } - - ApproximateTime & operator=(const ApproximateTime & rhs) - { - parent_ = rhs.parent_; - queue_size_ = rhs.queue_size_; - num_non_empty_deques_ = rhs.num_non_empty_deques_; - pivot_time_ = rhs.pivot_time_; - pivot_ = rhs.pivot_; - max_interval_duration_ = rhs.max_interval_duration_; - age_penalty_ = rhs.age_penalty_; - candidate_start_ = rhs.candidate_start_; - candidate_end_ = rhs.candidate_end_; - deques_ = rhs.deques_; - past_ = rhs.past_; - has_dropped_messages_ = rhs.has_dropped_messages_; - inter_message_lower_bounds_ = rhs.inter_message_lower_bounds_; - warned_about_incorrect_bound_ = rhs.warned_about_incorrect_bound_; - - return *this; - } - - void initParent(Sync * parent) - { - parent_ = parent; - } - - template - void checkInterMessageBound() - { - namespace mt = message_filters::message_traits; - if (warned_about_incorrect_bound_[i]) { - return; - } - std::deque::type> & deque = std::get(deques_); - std::vector::type> & v = std::get(past_); - assert(!deque.empty()); - 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; - } - 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); - } - 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 (" - "%" PRId64 ") than the lower bound you provided (" - "%" PRId64 ") (will print only once)", - i, - (msg_time - previous_msg_time).nanoseconds(), - inter_message_lower_bounds_[i].nanoseconds()); - warned_about_incorrect_bound_[i] = true; - } - } - - - template - void add(const typename std::tuple_element::type & evt) - { - std::lock_guard lock(data_mutex_); - - 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) { - // All deques have messages - process(); - } - } 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_) { - // Cancel ongoing candidate search, if any: - num_non_empty_deques_ = 0; // We will recompute it from scratch - recover<0>(); - recover<1>(); - recover<2>(); - recover<3>(); - recover<4>(); - recover<5>(); - recover<6>(); - recover<7>(); - recover<8>(); - // Drop the oldest message in the offending topic - 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(); - } - } - } - - void setAgePenalty(double age_penalty) - { - // For correctness we only need age_penalty > -1.0, - // but most likely a negative age_penalty is a mistake. - assert(age_penalty >= 0); - age_penalty_ = age_penalty; - } - - 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. - assert(lower_bound >= rclcpp::Duration(0, 0)); - inter_message_lower_bounds_[i] = lower_bound; - } - - 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. - assert(max_interval_duration >= rclcpp::Duration(0, 0)); - max_interval_duration_ = max_interval_duration; - } - -private: - // Assumes that deque number is non empty - template - void dequeDeleteFront() - { - std::deque::type> & deque = std::get(deques_); - assert(!deque.empty()); - deque.pop_front(); - if (deque.empty()) { - --num_non_empty_deques_; - } - } - - // 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: - std::abort(); - } - } - - // Assumes that deque number is non empty - template - void dequeMoveFrontToPast() - { - std::deque::type> & deque = std::get(deques_); - std::vector::type> & vector = std::get(past_); - assert(!deque.empty()); - vector.push_back(deque.front()); - deque.pop_front(); - 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: - std::abort(); - } - } - - void makeCandidate() - { - // Create candidate tuple - candidate_ = Tuple(); // Discards old one if any - std::get<0>(candidate_) = std::get<0>(deques_).front(); - std::get<1>(candidate_) = std::get<1>(deques_).front(); - if (RealTypeCount::value > 2) { - std::get<2>(candidate_) = std::get<2>(deques_).front(); - if (RealTypeCount::value > 3) { - std::get<3>(candidate_) = std::get<3>(deques_).front(); - if (RealTypeCount::value > 4) { - std::get<4>(candidate_) = std::get<4>(deques_).front(); - if (RealTypeCount::value > 5) { - std::get<5>(candidate_) = std::get<5>(deques_).front(); - if (RealTypeCount::value > 6) { - std::get<6>(candidate_) = std::get<6>(deques_).front(); - if (RealTypeCount::value > 7) { - std::get<7>(candidate_) = std::get<7>(deques_).front(); - if (RealTypeCount::value > 8) { - std::get<8>(candidate_) = std::get<8>(deques_).front(); - } - } - } - } - } - } - } - // Delete all past messages, since we have found a better candidate - std::get<0>(past_).clear(); - std::get<1>(past_).clear(); - std::get<2>(past_).clear(); - std::get<3>(past_).clear(); - std::get<4>(past_).clear(); - std::get<5>(past_).clear(); - std::get<6>(past_).clear(); - std::get<7>(past_).clear(); - std::get<8>(past_).clear(); - } - - - // ASSUMES: num_messages <= past_[i].size() - template - void recover(size_t num_messages) - { - if (i >= RealTypeCount::value) { - return; - } - - std::vector::type> & v = std::get(past_); - std::deque::type> & q = std::get(deques_); - assert(num_messages <= v.size()); - while (num_messages > 0) { - q.push_front(v.back()); - v.pop_back(); - num_messages--; - } - - if (!q.empty()) { - ++num_non_empty_deques_; - } - } - - - template - void recover() - { - if (i >= RealTypeCount::value) { - return; - } - - std::vector::type> & v = std::get(past_); - std::deque::type> & q = std::get(deques_); - while (!v.empty()) { - q.push_front(v.back()); - v.pop_back(); - } - - if (!q.empty()) { - ++num_non_empty_deques_; - } - } - - - template - void recoverAndDelete() - { - if (i >= RealTypeCount::value) { - return; - } - - std::vector::type> & v = std::get(past_); - std::deque::type> & q = std::get(deques_); - while (!v.empty()) { - q.push_front(v.back()); - v.pop_back(); - } - - assert(!q.empty()); - - q.pop_front(); - if (!q.empty()) { - ++num_non_empty_deques_; - } - } - - // Assumes: all deques are non empty, i.e. num_non_empty_deques_ == RealTypeCount::value - void publishCandidate() - { - // Publish - parent_->signal( - std::get<0>(candidate_), std::get<1>(candidate_), std::get<2>(candidate_), - std::get<3>(candidate_), - std::get<4>(candidate_), std::get<5>(candidate_), std::get<6>(candidate_), - std::get<7>(candidate_), - std::get<8>(candidate_)); - // Delete this candidate - candidate_ = Tuple(); - pivot_ = NO_PIVOT; - - // Recover hidden messages, and delete the ones corresponding to the candidate - num_non_empty_deques_ = 0; // We will recompute it from scratch - recoverAndDelete<0>(); - recoverAndDelete<1>(); - recoverAndDelete<2>(); - recoverAndDelete<3>(); - recoverAndDelete<4>(); - recoverAndDelete<5>(); - recoverAndDelete<6>(); - recoverAndDelete<7>(); - recoverAndDelete<8>(); - } - - // Assumes: all deques are non empty, i.e. num_non_empty_deques_ == RealTypeCount::value - // Returns: the oldest message on the deques - void getCandidateStart(uint32_t & start_index, rclcpp::Time & start_time) - { - return getCandidateBoundary(start_index, start_time, false); - } - - // Assumes: all deques are non empty, i.e. num_non_empty_deques_ == RealTypeCount::value - // 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) - { - return getCandidateBoundary(end_index, end_time, true); - } - - // ASSUMES: all deques are non-empty - // end = true: look for the latest head of deque - // false: look for the earliest head of deque - void getCandidateBoundary(uint32_t & index, rclcpp::Time & time, bool end) - { - namespace mt = message_filters::message_traits; - - M0Event & m0 = std::get<0>(deques_).front(); - time = mt::TimeStamp::value(*m0.getMessage()); - index = 0; - if (RealTypeCount::value > 1) { - M1Event & m1 = std::get<1>(deques_).front(); - if ((mt::TimeStamp::value(*m1.getMessage()) < time) ^ end) { - time = mt::TimeStamp::value(*m1.getMessage()); - index = 1; - } - } - if (RealTypeCount::value > 2) { - M2Event & m2 = std::get<2>(deques_).front(); - if ((mt::TimeStamp::value(*m2.getMessage()) < time) ^ end) { - time = mt::TimeStamp::value(*m2.getMessage()); - index = 2; - } - } - if (RealTypeCount::value > 3) { - M3Event & m3 = std::get<3>(deques_).front(); - if ((mt::TimeStamp::value(*m3.getMessage()) < time) ^ end) { - time = mt::TimeStamp::value(*m3.getMessage()); - index = 3; - } - } - if (RealTypeCount::value > 4) { - M4Event & m4 = std::get<4>(deques_).front(); - if ((mt::TimeStamp::value(*m4.getMessage()) < time) ^ end) { - time = mt::TimeStamp::value(*m4.getMessage()); - index = 4; - } - } - if (RealTypeCount::value > 5) { - M5Event & m5 = std::get<5>(deques_).front(); - if ((mt::TimeStamp::value(*m5.getMessage()) < time) ^ end) { - time = mt::TimeStamp::value(*m5.getMessage()); - index = 5; - } - } - if (RealTypeCount::value > 6) { - M6Event & m6 = std::get<6>(deques_).front(); - if ((mt::TimeStamp::value(*m6.getMessage()) < time) ^ end) { - time = mt::TimeStamp::value(*m6.getMessage()); - index = 6; - } - } - if (RealTypeCount::value > 7) { - M7Event & m7 = std::get<7>(deques_).front(); - if ((mt::TimeStamp::value(*m7.getMessage()) < time) ^ end) { - time = mt::TimeStamp::value(*m7.getMessage()); - index = 7; - } - } - if (RealTypeCount::value > 8) { - M8Event & m8 = std::get<8>(deques_).front(); - if ((mt::TimeStamp::value(*m8.getMessage()) < time) ^ end) { - time = mt::TimeStamp::value(*m8.getMessage()); - index = 8; - } - } - } - - - // ASSUMES: we have a pivot and candidate - template - rclcpp::Time getVirtualTime() - { - namespace mt = message_filters::message_traits; - - if (i >= RealTypeCount::value) { - return rclcpp::Time(0, 0); // Dummy return value - } - assert(pivot_ != NO_PIVOT); - - std::vector::type> & v = std::get(past_); - std::deque::type> & q = std::get(deques_); - if (q.empty()) { - assert(!v.empty()); // Because we have a candidate - 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 - return msg_time_lower_bound; - } - return pivot_time_; - } - 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) - { - return getVirtualCandidateBoundary(start_index, start_time, false); - } - - // ASSUMES: we have a pivot and candidate - void getVirtualCandidateEnd(uint32_t & end_index, rclcpp::Time & end_time) - { - return getVirtualCandidateBoundary(end_index, end_time, true); - } - - // ASSUMES: we have a pivot and candidate - // end = true: look for the latest head of deque - // false: look for the earliest head of deque - void getVirtualCandidateBoundary(uint32_t & index, rclcpp::Time & time, bool end) - { - std::vector virtual_times(9); - virtual_times[0] = getVirtualTime<0>(); - virtual_times[1] = getVirtualTime<1>(); - virtual_times[2] = getVirtualTime<2>(); - virtual_times[3] = getVirtualTime<3>(); - virtual_times[4] = getVirtualTime<4>(); - virtual_times[5] = getVirtualTime<5>(); - virtual_times[6] = getVirtualTime<6>(); - virtual_times[7] = getVirtualTime<7>(); - virtual_times[8] = getVirtualTime<8>(); - - 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; - } - } - } - - - // assumes data_mutex_ is already locked - void process() - { - // While no deque is empty - while (num_non_empty_deques_ == (uint32_t)RealTypeCount::value) { - // Find the start and end of the current interval - rclcpp::Time end_time, start_time; - uint32_t end_index, start_index; - getCandidateEnd(end_index, end_time); - getCandidateStart(start_index, start_time); - for (uint32_t i = 0; i < (uint32_t)RealTypeCount::value; i++) { - 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) { - // 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_) { - // 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 { - // 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_)) - { - // This is not a better candidate, move to the next - dequeMoveFrontToPast(start_index); - } else { - // This is a better candidate - makeCandidate(); - candidate_start_ = start_time; - candidate_end_ = end_time; - dequeMoveFrontToPast(start_index); - // Keep the same pivot (and pivot time) - } - } - // INVARIANT: we have a candidate and pivot - assert(pivot_ != NO_PIVOT); - rclcpp::Duration age_check = (end_time - candidate_end_) * (1 + age_penalty_); - if (start_index == pivot_) { // TODO(anyone): 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 (age_check >= (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 - // is already too big. - // 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) { - 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) { - 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_)) - { - // We have proved optimality - // As above, any future candidate must contain the interval [pivot_time_ end_time], - // which is already too big. - 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_)) - { - // 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]); - (void)num_non_empty_deques_before_virtual_search; // unused variable warning stopper - assert(num_non_empty_deques_before_virtual_search == num_non_empty_deques_); - break; - } - // 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. - assert(start_index != pivot_); - assert(start_time < pivot_time_); - dequeMoveFrontToPast(start_index); - num_virtual_moves[start_index]++; - } // while(1) - } - } // while(num_non_empty_deques_ == (uint32_t)RealTypeCount::value) - } - - Sync * parent_; - uint32_t queue_size_; - - // Special value for the pivot indicating that no pivot has been selected - static const uint32_t NO_PIVOT = 9; - - DequeTuple deques_; - uint32_t num_non_empty_deques_; - VectorTuple past_; - Tuple candidate_; // NULL if there is no candidate, in which case there is no pivot. - rclcpp::Time candidate_start_; - rclcpp::Time candidate_end_; - rclcpp::Time pivot_time_; - uint32_t pivot_; // Equal to NO_PIVOT if there is no candidate - std::mutex data_mutex_; // Protects all of the above - - rclcpp::Duration max_interval_duration_; // TODO(anyone): initialize with a parameter - double age_penalty_; - - std::vector has_dropped_messages_; - std::vector inter_message_lower_bounds_; - std::vector warned_about_incorrect_bound_; -}; +#warning This header is obsolete, please include message_filters/approximate_time.hpp instead -} // namespace sync_policies -} // namespace message_filters +#include #endif // MESSAGE_FILTERS__SYNC_POLICIES__APPROXIMATE_TIME_H_ diff --git a/include/message_filters/sync_policies/approximate_time.hpp b/include/message_filters/sync_policies/approximate_time.hpp new file mode 100644 index 0000000..e64671f --- /dev/null +++ b/include/message_filters/sync_policies/approximate_time.hpp @@ -0,0 +1,797 @@ +// Copyright 2009, Willow Garage, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef MESSAGE_FILTERS__SYNC_POLICIES__APPROXIMATE_TIME_HPP_ +#define MESSAGE_FILTERS__SYNC_POLICIES__APPROXIMATE_TIME_HPP_ + +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include "message_filters/connection.hpp" +#include "message_filters/message_traits.hpp" +#include "message_filters/null_types.hpp" +#include "message_filters/signal9.hpp" +#include "message_filters/synchronizer.hpp" + +namespace message_filters +{ +namespace sync_policies +{ + +template +struct ApproximateTime : public PolicyBase +{ + typedef Synchronizer Sync; + typedef PolicyBase Super; + typedef typename Super::Messages Messages; + typedef typename Super::Signal Signal; + typedef typename Super::Events Events; + typedef typename Super::RealTypeCount RealTypeCount; + typedef typename Super::M0Event M0Event; + typedef typename Super::M1Event M1Event; + typedef typename Super::M2Event M2Event; + typedef typename Super::M3Event M3Event; + typedef typename Super::M4Event M4Event; + typedef typename Super::M5Event M5Event; + typedef typename Super::M6Event M6Event; + typedef typename Super::M7Event M7Event; + typedef typename Super::M8Event M8Event; + typedef std::deque M0Deque; + typedef std::deque M1Deque; + typedef std::deque M2Deque; + typedef std::deque M3Deque; + typedef std::deque M4Deque; + typedef std::deque M5Deque; + typedef std::deque M6Deque; + typedef std::deque M7Deque; + typedef std::deque M8Deque; + typedef std::vector M0Vector; + typedef std::vector M1Vector; + typedef std::vector M2Vector; + typedef std::vector M3Vector; + typedef std::vector M4Vector; + typedef std::vector M5Vector; + typedef std::vector M6Vector; + typedef std::vector M7Vector; + typedef std::vector M8Vector; + typedef Events Tuple; + typedef std::tuple DequeTuple; + typedef std::tuple VectorTuple; + + ApproximateTime(uint32_t queue_size) // NOLINT(runtime/explicit) + : 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) + { + // The synchronizer will tend to drop many messages with a queue size of 1. + // At least 2 is recommended. + assert(queue_size_ > 0); + } + + ApproximateTime(const ApproximateTime & e) + : max_interval_duration_(rclcpp::Duration(std::numeric_limits::max(), 999999999)) + { + *this = e; + } + + ApproximateTime & operator=(const ApproximateTime & rhs) + { + parent_ = rhs.parent_; + queue_size_ = rhs.queue_size_; + num_non_empty_deques_ = rhs.num_non_empty_deques_; + pivot_time_ = rhs.pivot_time_; + pivot_ = rhs.pivot_; + max_interval_duration_ = rhs.max_interval_duration_; + age_penalty_ = rhs.age_penalty_; + candidate_start_ = rhs.candidate_start_; + candidate_end_ = rhs.candidate_end_; + deques_ = rhs.deques_; + past_ = rhs.past_; + has_dropped_messages_ = rhs.has_dropped_messages_; + inter_message_lower_bounds_ = rhs.inter_message_lower_bounds_; + warned_about_incorrect_bound_ = rhs.warned_about_incorrect_bound_; + + return *this; + } + + void initParent(Sync * parent) + { + parent_ = parent; + } + + template + void checkInterMessageBound() + { + namespace mt = message_filters::message_traits; + if (warned_about_incorrect_bound_[i]) { + return; + } + std::deque::type> & deque = std::get(deques_); + std::vector::type> & v = std::get(past_); + assert(!deque.empty()); + 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() == static_cast(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 { + // 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); + } + 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 (" + "%" PRId64 ") than the lower bound you provided (" + "%" PRId64 ") (will print only once)", + i, + (msg_time - previous_msg_time).nanoseconds(), + inter_message_lower_bounds_[i].nanoseconds()); + warned_about_incorrect_bound_[i] = true; + } + } + + + template + void add(const typename std::tuple_element::type & evt) + { + std::lock_guard lock(data_mutex_); + + std::deque::type> & deque = std::get(deques_); + deque.push_back(evt); + if (deque.size() == static_cast(1)) { + // We have just added the first message, so it was empty before + ++num_non_empty_deques_; + if (num_non_empty_deques_ == (uint32_t)RealTypeCount::value) { + // All deques have messages + process(); + } + } 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_) { + // Cancel ongoing candidate search, if any: + num_non_empty_deques_ = 0; // We will recompute it from scratch + recover<0>(); + recover<1>(); + recover<2>(); + recover<3>(); + recover<4>(); + recover<5>(); + recover<6>(); + recover<7>(); + recover<8>(); + // Drop the oldest message in the offending topic + 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(); + } + } + } + + void setAgePenalty(double age_penalty) + { + // For correctness we only need age_penalty > -1.0, + // but most likely a negative age_penalty is a mistake. + assert(age_penalty >= 0); + age_penalty_ = age_penalty; + } + + 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. + assert(lower_bound >= rclcpp::Duration(0, 0)); + inter_message_lower_bounds_[i] = lower_bound; + } + + 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. + assert(max_interval_duration >= rclcpp::Duration(0, 0)); + max_interval_duration_ = max_interval_duration; + } + +private: + // Assumes that deque number is non empty + template + void dequeDeleteFront() + { + std::deque::type> & deque = std::get(deques_); + assert(!deque.empty()); + deque.pop_front(); + if (deque.empty()) { + --num_non_empty_deques_; + } + } + + // 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: + std::abort(); + } + } + + // Assumes that deque number is non empty + template + void dequeMoveFrontToPast() + { + std::deque::type> & deque = std::get(deques_); + std::vector::type> & vector = std::get(past_); + assert(!deque.empty()); + vector.push_back(deque.front()); + deque.pop_front(); + 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: + std::abort(); + } + } + + void makeCandidate() + { + // Create candidate tuple + candidate_ = Tuple(); // Discards old one if any + std::get<0>(candidate_) = std::get<0>(deques_).front(); + std::get<1>(candidate_) = std::get<1>(deques_).front(); + if (RealTypeCount::value > 2) { + std::get<2>(candidate_) = std::get<2>(deques_).front(); + if (RealTypeCount::value > 3) { + std::get<3>(candidate_) = std::get<3>(deques_).front(); + if (RealTypeCount::value > 4) { + std::get<4>(candidate_) = std::get<4>(deques_).front(); + if (RealTypeCount::value > 5) { + std::get<5>(candidate_) = std::get<5>(deques_).front(); + if (RealTypeCount::value > 6) { + std::get<6>(candidate_) = std::get<6>(deques_).front(); + if (RealTypeCount::value > 7) { + std::get<7>(candidate_) = std::get<7>(deques_).front(); + if (RealTypeCount::value > 8) { + std::get<8>(candidate_) = std::get<8>(deques_).front(); + } + } + } + } + } + } + } + // Delete all past messages, since we have found a better candidate + std::get<0>(past_).clear(); + std::get<1>(past_).clear(); + std::get<2>(past_).clear(); + std::get<3>(past_).clear(); + std::get<4>(past_).clear(); + std::get<5>(past_).clear(); + std::get<6>(past_).clear(); + std::get<7>(past_).clear(); + std::get<8>(past_).clear(); + } + + + // ASSUMES: num_messages <= past_[i].size() + template + void recover(size_t num_messages) + { + if (i >= RealTypeCount::value) { + return; + } + + std::vector::type> & v = std::get(past_); + std::deque::type> & q = std::get(deques_); + assert(num_messages <= v.size()); + while (num_messages > 0) { + q.push_front(v.back()); + v.pop_back(); + num_messages--; + } + + if (!q.empty()) { + ++num_non_empty_deques_; + } + } + + + template + void recover() + { + if (i >= RealTypeCount::value) { + return; + } + + std::vector::type> & v = std::get(past_); + std::deque::type> & q = std::get(deques_); + while (!v.empty()) { + q.push_front(v.back()); + v.pop_back(); + } + + if (!q.empty()) { + ++num_non_empty_deques_; + } + } + + + template + void recoverAndDelete() + { + if (i >= RealTypeCount::value) { + return; + } + + std::vector::type> & v = std::get(past_); + std::deque::type> & q = std::get(deques_); + while (!v.empty()) { + q.push_front(v.back()); + v.pop_back(); + } + + assert(!q.empty()); + + q.pop_front(); + if (!q.empty()) { + ++num_non_empty_deques_; + } + } + + // Assumes: all deques are non empty, i.e. num_non_empty_deques_ == RealTypeCount::value + void publishCandidate() + { + // Publish + parent_->signal( + std::get<0>(candidate_), std::get<1>(candidate_), std::get<2>(candidate_), + std::get<3>(candidate_), + std::get<4>(candidate_), std::get<5>(candidate_), std::get<6>(candidate_), + std::get<7>(candidate_), + std::get<8>(candidate_)); + // Delete this candidate + candidate_ = Tuple(); + pivot_ = NO_PIVOT; + + // Recover hidden messages, and delete the ones corresponding to the candidate + num_non_empty_deques_ = 0; // We will recompute it from scratch + recoverAndDelete<0>(); + recoverAndDelete<1>(); + recoverAndDelete<2>(); + recoverAndDelete<3>(); + recoverAndDelete<4>(); + recoverAndDelete<5>(); + recoverAndDelete<6>(); + recoverAndDelete<7>(); + recoverAndDelete<8>(); + } + + // Assumes: all deques are non empty, i.e. num_non_empty_deques_ == RealTypeCount::value + // Returns: the oldest message on the deques + void getCandidateStart(uint32_t & start_index, rclcpp::Time & start_time) + { + return getCandidateBoundary(start_index, start_time, false); + } + + // Assumes: all deques are non empty, i.e. num_non_empty_deques_ == RealTypeCount::value + // 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) + { + return getCandidateBoundary(end_index, end_time, true); + } + + // ASSUMES: all deques are non-empty + // end = true: look for the latest head of deque + // false: look for the earliest head of deque + void getCandidateBoundary(uint32_t & index, rclcpp::Time & time, bool end) + { + namespace mt = message_filters::message_traits; + + M0Event & m0 = std::get<0>(deques_).front(); + time = mt::TimeStamp::value(*m0.getMessage()); + index = 0; + if (RealTypeCount::value > 1) { + M1Event & m1 = std::get<1>(deques_).front(); + if ((mt::TimeStamp::value(*m1.getMessage()) < time) ^ end) { + time = mt::TimeStamp::value(*m1.getMessage()); + index = 1; + } + } + if (RealTypeCount::value > 2) { + M2Event & m2 = std::get<2>(deques_).front(); + if ((mt::TimeStamp::value(*m2.getMessage()) < time) ^ end) { + time = mt::TimeStamp::value(*m2.getMessage()); + index = 2; + } + } + if (RealTypeCount::value > 3) { + M3Event & m3 = std::get<3>(deques_).front(); + if ((mt::TimeStamp::value(*m3.getMessage()) < time) ^ end) { + time = mt::TimeStamp::value(*m3.getMessage()); + index = 3; + } + } + if (RealTypeCount::value > 4) { + M4Event & m4 = std::get<4>(deques_).front(); + if ((mt::TimeStamp::value(*m4.getMessage()) < time) ^ end) { + time = mt::TimeStamp::value(*m4.getMessage()); + index = 4; + } + } + if (RealTypeCount::value > 5) { + M5Event & m5 = std::get<5>(deques_).front(); + if ((mt::TimeStamp::value(*m5.getMessage()) < time) ^ end) { + time = mt::TimeStamp::value(*m5.getMessage()); + index = 5; + } + } + if (RealTypeCount::value > 6) { + M6Event & m6 = std::get<6>(deques_).front(); + if ((mt::TimeStamp::value(*m6.getMessage()) < time) ^ end) { + time = mt::TimeStamp::value(*m6.getMessage()); + index = 6; + } + } + if (RealTypeCount::value > 7) { + M7Event & m7 = std::get<7>(deques_).front(); + if ((mt::TimeStamp::value(*m7.getMessage()) < time) ^ end) { + time = mt::TimeStamp::value(*m7.getMessage()); + index = 7; + } + } + if (RealTypeCount::value > 8) { + M8Event & m8 = std::get<8>(deques_).front(); + if ((mt::TimeStamp::value(*m8.getMessage()) < time) ^ end) { + time = mt::TimeStamp::value(*m8.getMessage()); + index = 8; + } + } + } + + + // ASSUMES: we have a pivot and candidate + template + rclcpp::Time getVirtualTime() + { + namespace mt = message_filters::message_traits; + + if (i >= RealTypeCount::value) { + return rclcpp::Time(0, 0); // Dummy return value + } + assert(pivot_ != NO_PIVOT); + + std::vector::type> & v = std::get(past_); + std::deque::type> & q = std::get(deques_); + if (q.empty()) { + assert(!v.empty()); // Because we have a candidate + 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 + return msg_time_lower_bound; + } + return pivot_time_; + } + 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) + { + return getVirtualCandidateBoundary(start_index, start_time, false); + } + + // ASSUMES: we have a pivot and candidate + void getVirtualCandidateEnd(uint32_t & end_index, rclcpp::Time & end_time) + { + return getVirtualCandidateBoundary(end_index, end_time, true); + } + + // ASSUMES: we have a pivot and candidate + // end = true: look for the latest head of deque + // false: look for the earliest head of deque + void getVirtualCandidateBoundary(uint32_t & index, rclcpp::Time & time, bool end) + { + std::vector virtual_times(9); + virtual_times[0] = getVirtualTime<0>(); + virtual_times[1] = getVirtualTime<1>(); + virtual_times[2] = getVirtualTime<2>(); + virtual_times[3] = getVirtualTime<3>(); + virtual_times[4] = getVirtualTime<4>(); + virtual_times[5] = getVirtualTime<5>(); + virtual_times[6] = getVirtualTime<6>(); + virtual_times[7] = getVirtualTime<7>(); + virtual_times[8] = getVirtualTime<8>(); + + 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; + } + } + } + + + // assumes data_mutex_ is already locked + void process() + { + // While no deque is empty + while (num_non_empty_deques_ == (uint32_t)RealTypeCount::value) { + // Find the start and end of the current interval + rclcpp::Time end_time, start_time; + uint32_t end_index, start_index; + getCandidateEnd(end_index, end_time); + getCandidateStart(start_index, start_time); + for (uint32_t i = 0; i < (uint32_t)RealTypeCount::value; i++) { + 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) { + // 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_) { + // 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 { + // 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_)) + { + // This is not a better candidate, move to the next + dequeMoveFrontToPast(start_index); + } else { + // This is a better candidate + makeCandidate(); + candidate_start_ = start_time; + candidate_end_ = end_time; + dequeMoveFrontToPast(start_index); + // Keep the same pivot (and pivot time) + } + } + // INVARIANT: we have a candidate and pivot + assert(pivot_ != NO_PIVOT); + rclcpp::Duration age_check = (end_time - candidate_end_) * (1 + age_penalty_); + if (start_index == pivot_) { // TODO(anyone): 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 (age_check >= (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 + // is already too big. + // 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) { + 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) { + 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_)) + { + // We have proved optimality + // As above, any future candidate must contain the interval [pivot_time_ end_time], + // which is already too big. + 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_)) + { + // 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]); + (void)num_non_empty_deques_before_virtual_search; // unused variable warning stopper + assert(num_non_empty_deques_before_virtual_search == num_non_empty_deques_); + break; + } + // 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. + assert(start_index != pivot_); + assert(start_time < pivot_time_); + dequeMoveFrontToPast(start_index); + num_virtual_moves[start_index]++; + } // while(1) + } + } // while(num_non_empty_deques_ == (uint32_t)RealTypeCount::value) + } + + Sync * parent_; + uint32_t queue_size_; + + // Special value for the pivot indicating that no pivot has been selected + static const uint32_t NO_PIVOT = 9; + + DequeTuple deques_; + uint32_t num_non_empty_deques_; + VectorTuple past_; + Tuple candidate_; // NULL if there is no candidate, in which case there is no pivot. + rclcpp::Time candidate_start_; + rclcpp::Time candidate_end_; + rclcpp::Time pivot_time_; + uint32_t pivot_; // Equal to NO_PIVOT if there is no candidate + std::mutex data_mutex_; // Protects all of the above + + rclcpp::Duration max_interval_duration_; // TODO(anyone): initialize with a parameter + double age_penalty_; + + std::vector has_dropped_messages_; + std::vector inter_message_lower_bounds_; + std::vector warned_about_incorrect_bound_; +}; + +} // namespace sync_policies +} // namespace message_filters + +#endif // MESSAGE_FILTERS__SYNC_POLICIES__APPROXIMATE_TIME_HPP_ diff --git a/include/message_filters/sync_policies/exact_time.h b/include/message_filters/sync_policies/exact_time.h index 4f4e4c5..2e4fa48 100644 --- a/include/message_filters/sync_policies/exact_time.h +++ b/include/message_filters/sync_policies/exact_time.h @@ -1,225 +1,22 @@ -// Copyright 2009, Willow Garage, Inc. All rights reserved. +// Copyright 2024 Open Source Robotics Foundation, Inc. All rights reserved. // -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. +// http://www.apache.org/licenses/LICENSE-2.0 // -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the Willow Garage nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef MESSAGE_FILTERS__SYNC_POLICIES__EXACT_TIME_H_ #define MESSAGE_FILTERS__SYNC_POLICIES__EXACT_TIME_H_ -#include -#include -#include -#include -#include - -#include - -#include "message_filters/connection.h" -#include "message_filters/message_traits.h" -#include "message_filters/null_types.h" -#include "message_filters/signal9.h" -#include "message_filters/synchronizer.h" - -namespace message_filters -{ -namespace sync_policies -{ - -template -struct ExactTime : public PolicyBase -{ - typedef Synchronizer Sync; - typedef PolicyBase Super; - typedef typename Super::Messages Messages; - typedef typename Super::Signal Signal; - typedef typename Super::Events Events; - typedef typename Super::RealTypeCount RealTypeCount; - typedef typename Super::M0Event M0Event; - typedef typename Super::M1Event M1Event; - typedef typename Super::M2Event M2Event; - typedef typename Super::M3Event M3Event; - typedef typename Super::M4Event M4Event; - typedef typename Super::M5Event M5Event; - typedef typename Super::M6Event M6Event; - typedef typename Super::M7Event M7Event; - typedef typename Super::M8Event M8Event; - typedef Events Tuple; - - ExactTime(uint32_t queue_size) // NOLINT(runtime/explicit) - : parent_(0) - , queue_size_(queue_size) - { - } - - ExactTime(const ExactTime & e) - { - *this = e; - } - - ExactTime & operator=(const ExactTime & rhs) - { - parent_ = rhs.parent_; - queue_size_ = rhs.queue_size_; - last_signal_time_ = rhs.last_signal_time_; - tuples_ = rhs.tuples_; - - return *this; - } - - void initParent(Sync * parent) - { - parent_ = parent; - } - - template - void add(const typename std::tuple_element::type & evt) - { - assert(parent_); - - namespace mt = message_filters::message_traits; - - std::lock_guard lock(mutex_); - - Tuple & t = tuples_[mt::TimeStamp::type>::value( - *evt.getMessage())]; - std::get(t) = evt; - - checkTuple(t); - } - - template - Connection registerDropCallback(const C & callback) - { - return drop_signal_.addCallback(callback); - } - - template - Connection registerDropCallback(C & callback) - { - return drop_signal_.addCallback(callback); - } - - template - Connection registerDropCallback(const C & callback, T * t) - { - return drop_signal_.addCallback(callback, t); - } - - template - Connection registerDropCallback(C & callback, T * t) - { - return drop_signal_.addCallback(callback, t); - } - - rclcpp::Time getLastSignalTime() const - { - return last_signal_time_; - } - -private: - // assumes mutex_ is already locked - void checkTuple(Tuple & t) - { - namespace mt = message_filters::message_traits; - - bool full = true; - full = full && (bool)std::get<0>(t).getMessage(); - full = full && (bool)std::get<1>(t).getMessage(); - full = full && (RealTypeCount::value > 2 ? (bool)std::get<2>(t).getMessage() : true); - full = full && (RealTypeCount::value > 3 ? (bool)std::get<3>(t).getMessage() : true); - full = full && (RealTypeCount::value > 4 ? (bool)std::get<4>(t).getMessage() : true); - full = full && (RealTypeCount::value > 5 ? (bool)std::get<5>(t).getMessage() : true); - full = full && (RealTypeCount::value > 6 ? (bool)std::get<6>(t).getMessage() : true); - 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)); - - last_signal_time_ = mt::TimeStamp::value(*std::get<0>(t).getMessage()); - - tuples_.erase(last_signal_time_); - - 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)); - tuples_.erase(tuples_.begin()); - } - } - } - - // assumes mutex_ is already locked - void clearOldTuples() - { - typename M_TimeToTuple::iterator it = tuples_.begin(); - typename M_TimeToTuple::iterator end = tuples_.end(); - 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)); - tuples_.erase(old); - } else { - // the map is sorted by time, so we can ignore anything after this if this one's time is ok - break; - } - } - } - -private: - Sync * parent_; - - uint32_t queue_size_; - typedef std::map M_TimeToTuple; - M_TimeToTuple tuples_; - rclcpp::Time last_signal_time_; - - Signal drop_signal_; - - std::mutex mutex_; -}; +#warning This header is obsolete, please include message_filters/exact_time.hpp instead -} // namespace sync_policies -} // namespace message_filters +#include #endif // MESSAGE_FILTERS__SYNC_POLICIES__EXACT_TIME_H_ diff --git a/include/message_filters/sync_policies/exact_time.hpp b/include/message_filters/sync_policies/exact_time.hpp new file mode 100644 index 0000000..16f1ba3 --- /dev/null +++ b/include/message_filters/sync_policies/exact_time.hpp @@ -0,0 +1,232 @@ +// Copyright 2009, Willow Garage, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef MESSAGE_FILTERS__SYNC_POLICIES__EXACT_TIME_HPP_ +#define MESSAGE_FILTERS__SYNC_POLICIES__EXACT_TIME_HPP_ + +#include +#include +#include +#include +#include + +#include + +#include "message_filters/connection.hpp" +#include "message_filters/message_traits.hpp" +#include "message_filters/null_types.hpp" +#include "message_filters/signal9.hpp" +#include "message_filters/synchronizer.hpp" + +namespace message_filters +{ +namespace sync_policies +{ + +template +struct ExactTime : public PolicyBase +{ + typedef Synchronizer Sync; + typedef PolicyBase Super; + typedef typename Super::Messages Messages; + typedef typename Super::Signal Signal; + typedef typename Super::Events Events; + typedef typename Super::RealTypeCount RealTypeCount; + typedef typename Super::M0Event M0Event; + typedef typename Super::M1Event M1Event; + typedef typename Super::M2Event M2Event; + typedef typename Super::M3Event M3Event; + typedef typename Super::M4Event M4Event; + typedef typename Super::M5Event M5Event; + typedef typename Super::M6Event M6Event; + typedef typename Super::M7Event M7Event; + typedef typename Super::M8Event M8Event; + typedef Events Tuple; + + ExactTime(uint32_t queue_size) // NOLINT(runtime/explicit) + : parent_(0) + , queue_size_(queue_size) + { + } + + ExactTime(const ExactTime & e) + { + *this = e; + } + + ExactTime & operator=(const ExactTime & rhs) + { + parent_ = rhs.parent_; + queue_size_ = rhs.queue_size_; + last_signal_time_ = rhs.last_signal_time_; + tuples_ = rhs.tuples_; + + return *this; + } + + void initParent(Sync * parent) + { + parent_ = parent; + } + + template + void add(const typename std::tuple_element::type & evt) + { + assert(parent_); + + namespace mt = message_filters::message_traits; + + std::lock_guard lock(mutex_); + + Tuple & t = tuples_[mt::TimeStamp::type>::value( + *evt.getMessage())]; + std::get(t) = evt; + + checkTuple(t); + } + + template + Connection registerDropCallback(const C & callback) + { + return drop_signal_.addCallback(callback); + } + + template + Connection registerDropCallback(C & callback) + { + return drop_signal_.addCallback(callback); + } + + template + Connection registerDropCallback(const C & callback, T * t) + { + return drop_signal_.addCallback(callback, t); + } + + template + Connection registerDropCallback(C & callback, T * t) + { + return drop_signal_.addCallback(callback, t); + } + + rclcpp::Time getLastSignalTime() const + { + return last_signal_time_; + } + +private: + // assumes mutex_ is already locked + void checkTuple(Tuple & t) + { + namespace mt = message_filters::message_traits; + + bool full = true; + full = full && static_cast(std::get<0>(t).getMessage()); + full = full && static_cast(std::get<1>(t).getMessage()); + full = full && (RealTypeCount::value > 2 ? + static_cast(std::get<2>(t).getMessage()) : true); + full = full && (RealTypeCount::value > 3 ? + static_cast(std::get<3>(t).getMessage()) : true); + full = full && (RealTypeCount::value > 4 ? + static_cast(std::get<4>(t).getMessage()) : true); + full = full && (RealTypeCount::value > 5 ? + static_cast(std::get<5>(t).getMessage()) : true); + full = full && (RealTypeCount::value > 6 ? + static_cast(std::get<6>(t).getMessage()) : true); + full = full && (RealTypeCount::value > 7 ? + static_cast(std::get<7>(t).getMessage()) : true); + full = full && (RealTypeCount::value > 8 ? + static_cast(std::get<8>(t).getMessage()) : true); + + 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()); + + tuples_.erase(last_signal_time_); + + 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)); + tuples_.erase(tuples_.begin()); + } + } + } + + // assumes mutex_ is already locked + void clearOldTuples() + { + typename M_TimeToTuple::iterator it = tuples_.begin(); + typename M_TimeToTuple::iterator end = tuples_.end(); + 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)); + tuples_.erase(old); + } else { + // the map is sorted by time, so we can ignore anything after this if this one's time is ok + break; + } + } + } + +private: + Sync * parent_; + + uint32_t queue_size_; + typedef std::map M_TimeToTuple; + M_TimeToTuple tuples_; + rclcpp::Time last_signal_time_; + + Signal drop_signal_; + + std::mutex mutex_; +}; + +} // namespace sync_policies +} // namespace message_filters + +#endif // MESSAGE_FILTERS__SYNC_POLICIES__EXACT_TIME_HPP_ diff --git a/include/message_filters/sync_policies/latest_time.h b/include/message_filters/sync_policies/latest_time.h index 6aaf2e6..c574d64 100644 --- a/include/message_filters/sync_policies/latest_time.h +++ b/include/message_filters/sync_policies/latest_time.h @@ -1,385 +1,22 @@ -// Copyright 2022, Open Source Robotics Foundation, Inc. All rights reserved. +// Copyright 2024 Open Source Robotics Foundation, Inc. All rights reserved. // -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. +// http://www.apache.org/licenses/LICENSE-2.0 // -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the Willow Garage nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. - -/** - * \brief Synchronizes up to 9 messages by their rates with upsampling via zero-order-hold. - * - * LatestTime policy synchronizes up to 9 incoming channels by the rates they are received. - * The callback with all the messages will be triggered whenever the fastest message is received. - * The slower messages will be repeated at the rate of the fastest message and will be updated - * whenever a new one is received. This is essentially an upsampling of slower messages using a - * zero-order hold (no interpolation). - - * \section usage USAGE - * Example usage would be: -\verbatim -typedef LatestTime latest_policy; -Synchronizer sync_policies(latest_policy(), caminfo_sub, limage_sub, rimage_sub); -sync_policies.registerCallback(callback); -\endverbatim - - * 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; -Synchronizer sync_policies(latest_policy(node->get_clock()), caminfo_sub, limage_sub, rimage_sub); -sync_policies.registerCallback(callback); -\endverbatim - - * 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&); -\endverbatim - * - */ +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef MESSAGE_FILTERS__SYNC_POLICIES__LATEST_TIME_H_ #define MESSAGE_FILTERS__SYNC_POLICIES__LATEST_TIME_H_ -#include -#include -#include -#include -#include -#include - -#include - -#include "message_filters/message_traits.h" -#include "message_filters/null_types.h" -#include "message_filters/signal9.h" -#include "message_filters/synchronizer.h" - - -namespace message_filters -{ -namespace sync_policies -{ - -template -struct LatestTime : public PolicyBase -{ - typedef Synchronizer Sync; - typedef PolicyBase Super; - typedef typename Super::Messages Messages; - typedef typename Super::Signal Signal; - typedef typename Super::Events Events; - typedef typename Super::RealTypeCount RealTypeCount; - - /// \brief filter coeffs and error margin factor: - /// - typedef std::tuple RateConfig; - - LatestTime() - : LatestTime(rclcpp::Clock::SharedPtr(new rclcpp::Clock(RCL_ROS_TIME))) - { - } - - explicit LatestTime(rclcpp::Clock::SharedPtr clock) - : parent_(0), - ros_clock_{clock} - { - } - - LatestTime(const LatestTime & e) - { - *this = e; - } - - LatestTime & operator=(const LatestTime & rhs) - { - parent_ = rhs.parent_; - events_ = rhs.events_; - rates_ = rhs.rates_; - ros_clock_ = rhs.ros_clock_; - - return *this; - } - - void initParent(Sync * parent) - { - parent_ = parent; - } - - void setRateConfigPerMessage(const std::vector & configs) - { - rate_configs_.assign(configs.begin(), configs.end()); - } - - void setRateConfig(const RateConfig & config) - { - rate_configs_.assign(1U, config); - } - - void setClock(rclcpp::Clock::SharedPtr clock) - { - ros_clock_ = clock; - } - - template - void add(const typename std::tuple_element::type & evt) - { - assert(parent_); - - std::lock_guard lock(data_mutex_); - - 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 - // NOTE: this will drop a few messages of the faster topics until - // we get one of the slowest so we can sync - std::get(events_) = evt; // adding here ensures we see even the slowest - // message twice before computing rate - return; - } - - 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()) { - publish(); - } - } - -private: - // assumed data_mutex_ is locked - template - void initialize_rate() - { - if (rate_configs_.size() > 0U) { - double rate_ema_alpha{Rate::DEFAULT_RATE_EMA_ALPHA}; - 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( - rate_ema_alpha, - error_ema_alpha, - rate_step_change_margin_factor) = rate_configs_[i]; - } else if (rate_configs_.size() == 1U) { - std::tie( - rate_ema_alpha, - error_ema_alpha, - rate_step_change_margin_factor) = rate_configs_[0U]; - } - rates_.push_back( - Rate( - ros_clock_->now(), - rate_ema_alpha, - error_ema_alpha, - rate_step_change_margin_factor)); - } else { - rates_.push_back(Rate(ros_clock_->now())); - } - } - - // 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_)); - } - - struct Rate - { - static constexpr double DEFAULT_RATE_EMA_ALPHA{0.9}; - static constexpr double DEFAULT_ERROR_EMA_ALPHA{0.3}; - static constexpr double DEFAULT_MARGIN_FACTOR{10.0}; - - rclcpp::Time prev; - double hz{0.0}; - double error{0.0}; - double rate_ema_alpha{DEFAULT_RATE_EMA_ALPHA}; - double error_ema_alpha{DEFAULT_ERROR_EMA_ALPHA}; - double rate_step_change_margin_factor{DEFAULT_MARGIN_FACTOR}; - bool do_hz_init{true}; - bool do_error_init{true}; - - explicit Rate(const rclcpp::Time & start) - : Rate(start, DEFAULT_RATE_EMA_ALPHA, DEFAULT_ERROR_EMA_ALPHA, DEFAULT_MARGIN_FACTOR) - { - } - - 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} - { - } - - bool operator>(const Rate & that) const - { - return this->hz > that.hz; - } - - bool compute_hz(const rclcpp::Time & now) - { - bool step_change_detected = false; - do { - 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; - do_hz_init = false; - step_change_detected = false; - } else { - if (do_error_init) { - 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) { - // detected step change in rate so reset - do_hz_init = true; - do_error_init = true; - step_change_detected = true; - continue; - } - // on-line mean error from mean - 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; - } - } while (step_change_detected); - prev = now; - return true; - } - }; - - // assumed data_mutex_ is locked - template - std::vector sort_indices(const std::vector & v) - { - // initialize original index locations - std::vector idx(v.size()); - std::iota(idx.begin(), idx.end(), 0U); - - // sort indexes based on comparing values in v - // 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];}); - - return idx; - } - - // assumed data_mutex_ is locked - template - bool received_msg() - { - return RealTypeCount::value > i ? (bool)std::get(events_).getMessage() : true; - } - - // assumed data_mutex_ is locked - bool is_full() - { - bool full = received_msg<0>(); - full = full && received_msg<1>(); - full = full && received_msg<2>(); - full = full && received_msg<3>(); - full = full && received_msg<4>(); - full = full && received_msg<5>(); - full = full && received_msg<6>(); - full = full && received_msg<7>(); - full = full && received_msg<8>(); - - return full; - } - - // assumed data_mutex_ is locked - int find_pivot(const rclcpp::Time & now) - { - // find arg max rate - std::vector sorted_idx = sort_indices(rates_); - - // use fastest message that isn't late as pivot - for (size_t pivot : sorted_idx) { - double period = (now - rates_[pivot].prev).seconds(); - if (period == 0.0) { - if (rates_[pivot].hz > 0.0) { - // we just updated updated this one, - // and it's fastest, so use as pivot - return static_cast(pivot); - } else { - // haven't calculated rate for this message yet - continue; - } - } - - if (!rates_[pivot].do_error_init) { - // can now check if new messages are late - 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 - continue; - } - } - - if (rates_[pivot].hz > 0.0) { - // found fastest message with a calculated rate - return static_cast(pivot); - } else { - // haven't calculated rate for this message yet - continue; - } - } - return NO_PIVOT; - } - - Sync * parent_; - Events events_; - std::vector rates_; - std::mutex data_mutex_; // Protects all of the above - - std::vector rate_configs_; - - const int NO_PIVOT{9}; - - rclcpp::Clock::SharedPtr ros_clock_{nullptr}; -}; +#warning This header is obsolete, please include message_filters/latest_time.hpp instead -} // namespace sync_policies -} // namespace message_filters +#include #endif // MESSAGE_FILTERS__SYNC_POLICIES__LATEST_TIME_H_ diff --git a/include/message_filters/sync_policies/latest_time.hpp b/include/message_filters/sync_policies/latest_time.hpp new file mode 100644 index 0000000..1539325 --- /dev/null +++ b/include/message_filters/sync_policies/latest_time.hpp @@ -0,0 +1,385 @@ +// Copyright 2022, Open Source Robotics Foundation, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +/** + * \brief Synchronizes up to 9 messages by their rates with upsampling via zero-order-hold. + * + * LatestTime policy synchronizes up to 9 incoming channels by the rates they are received. + * The callback with all the messages will be triggered whenever the fastest message is received. + * The slower messages will be repeated at the rate of the fastest message and will be updated + * whenever a new one is received. This is essentially an upsampling of slower messages using a + * zero-order hold (no interpolation). + + * \section usage USAGE + * Example usage would be: +\verbatim +typedef LatestTime latest_policy; +Synchronizer sync_policies(latest_policy(), caminfo_sub, limage_sub, rimage_sub); +sync_policies.registerCallback(callback); +\endverbatim + + * 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; +Synchronizer sync_policies(latest_policy(node->get_clock()), caminfo_sub, limage_sub, rimage_sub); +sync_policies.registerCallback(callback); +\endverbatim + + * 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&); +\endverbatim + * + */ + +#ifndef MESSAGE_FILTERS__SYNC_POLICIES__LATEST_TIME_HPP_ +#define MESSAGE_FILTERS__SYNC_POLICIES__LATEST_TIME_HPP_ + +#include +#include +#include +#include +#include +#include + +#include + +#include "message_filters/message_traits.hpp" +#include "message_filters/null_types.hpp" +#include "message_filters/signal9.hpp" +#include "message_filters/synchronizer.hpp" + + +namespace message_filters +{ +namespace sync_policies +{ + +template +struct LatestTime : public PolicyBase +{ + typedef Synchronizer Sync; + typedef PolicyBase Super; + typedef typename Super::Messages Messages; + typedef typename Super::Signal Signal; + typedef typename Super::Events Events; + typedef typename Super::RealTypeCount RealTypeCount; + + /// \brief filter coeffs and error margin factor: + /// + typedef std::tuple RateConfig; + + LatestTime() + : LatestTime(rclcpp::Clock::SharedPtr(new rclcpp::Clock(RCL_ROS_TIME))) + { + } + + explicit LatestTime(rclcpp::Clock::SharedPtr clock) + : parent_(0), + ros_clock_{clock} + { + } + + LatestTime(const LatestTime & e) + { + *this = e; + } + + LatestTime & operator=(const LatestTime & rhs) + { + parent_ = rhs.parent_; + events_ = rhs.events_; + rates_ = rhs.rates_; + ros_clock_ = rhs.ros_clock_; + + return *this; + } + + void initParent(Sync * parent) + { + parent_ = parent; + } + + void setRateConfigPerMessage(const std::vector & configs) + { + rate_configs_.assign(configs.begin(), configs.end()); + } + + void setRateConfig(const RateConfig & config) + { + rate_configs_.assign(1U, config); + } + + void setClock(rclcpp::Clock::SharedPtr clock) + { + ros_clock_ = clock; + } + + template + void add(const typename std::tuple_element::type & evt) + { + assert(parent_); + + std::lock_guard lock(data_mutex_); + + 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 + // NOTE: this will drop a few messages of the faster topics until + // we get one of the slowest so we can sync + std::get(events_) = evt; // adding here ensures we see even the slowest + // message twice before computing rate + return; + } + + 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()) { + publish(); + } + } + +private: + // assumed data_mutex_ is locked + template + void initialize_rate() + { + if (rate_configs_.size() > 0U) { + double rate_ema_alpha{Rate::DEFAULT_RATE_EMA_ALPHA}; + 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( + rate_ema_alpha, + error_ema_alpha, + rate_step_change_margin_factor) = rate_configs_[i]; + } else if (rate_configs_.size() == 1U) { + std::tie( + rate_ema_alpha, + error_ema_alpha, + rate_step_change_margin_factor) = rate_configs_[0U]; + } + rates_.push_back( + Rate( + ros_clock_->now(), + rate_ema_alpha, + error_ema_alpha, + rate_step_change_margin_factor)); + } else { + rates_.push_back(Rate(ros_clock_->now())); + } + } + + // 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_)); + } + + struct Rate + { + static constexpr double DEFAULT_RATE_EMA_ALPHA{0.9}; + static constexpr double DEFAULT_ERROR_EMA_ALPHA{0.3}; + static constexpr double DEFAULT_MARGIN_FACTOR{10.0}; + + rclcpp::Time prev; + double hz{0.0}; + double error{0.0}; + double rate_ema_alpha{DEFAULT_RATE_EMA_ALPHA}; + double error_ema_alpha{DEFAULT_ERROR_EMA_ALPHA}; + double rate_step_change_margin_factor{DEFAULT_MARGIN_FACTOR}; + bool do_hz_init{true}; + bool do_error_init{true}; + + explicit Rate(const rclcpp::Time & start) + : Rate(start, DEFAULT_RATE_EMA_ALPHA, DEFAULT_ERROR_EMA_ALPHA, DEFAULT_MARGIN_FACTOR) + { + } + + 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} + { + } + + bool operator>(const Rate & that) const + { + return this->hz > that.hz; + } + + bool compute_hz(const rclcpp::Time & now) + { + bool step_change_detected = false; + do { + 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; + do_hz_init = false; + step_change_detected = false; + } else { + if (do_error_init) { + 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) { + // detected step change in rate so reset + do_hz_init = true; + do_error_init = true; + step_change_detected = true; + continue; + } + // on-line mean error from mean + 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; + } + } while (step_change_detected); + prev = now; + return true; + } + }; + + // assumed data_mutex_ is locked + template + std::vector sort_indices(const std::vector & v) + { + // initialize original index locations + std::vector idx(v.size()); + std::iota(idx.begin(), idx.end(), 0U); + + // sort indexes based on comparing values in v + // 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];}); + + return idx; + } + + // assumed data_mutex_ is locked + template + bool received_msg() + { + return RealTypeCount::value > i ? static_cast(std::get(events_).getMessage()) : true; + } + + // assumed data_mutex_ is locked + bool is_full() + { + bool full = received_msg<0>(); + full = full && received_msg<1>(); + full = full && received_msg<2>(); + full = full && received_msg<3>(); + full = full && received_msg<4>(); + full = full && received_msg<5>(); + full = full && received_msg<6>(); + full = full && received_msg<7>(); + full = full && received_msg<8>(); + + return full; + } + + // assumed data_mutex_ is locked + int find_pivot(const rclcpp::Time & now) + { + // find arg max rate + std::vector sorted_idx = sort_indices(rates_); + + // use fastest message that isn't late as pivot + for (size_t pivot : sorted_idx) { + double period = (now - rates_[pivot].prev).seconds(); + if (period == 0.0) { + if (rates_[pivot].hz > 0.0) { + // we just updated updated this one, + // and it's fastest, so use as pivot + return static_cast(pivot); + } else { + // haven't calculated rate for this message yet + continue; + } + } + + if (!rates_[pivot].do_error_init) { + // can now check if new messages are late + 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 + continue; + } + } + + if (rates_[pivot].hz > 0.0) { + // found fastest message with a calculated rate + return static_cast(pivot); + } else { + // haven't calculated rate for this message yet + continue; + } + } + return NO_PIVOT; + } + + Sync * parent_; + Events events_; + std::vector rates_; + std::mutex data_mutex_; // Protects all of the above + + std::vector rate_configs_; + + const int NO_PIVOT{9}; + + rclcpp::Clock::SharedPtr ros_clock_{nullptr}; +}; + +} // namespace sync_policies +} // namespace message_filters + +#endif // MESSAGE_FILTERS__SYNC_POLICIES__LATEST_TIME_HPP_ diff --git a/include/message_filters/synchronizer.h b/include/message_filters/synchronizer.h index 0e7f679..29b688d 100644 --- a/include/message_filters/synchronizer.h +++ b/include/message_filters/synchronizer.h @@ -1,423 +1,22 @@ -// Copyright 2009, Willow Garage, Inc. All rights reserved. +// Copyright 2024 Open Source Robotics Foundation, Inc. All rights reserved. // -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. +// http://www.apache.org/licenses/LICENSE-2.0 // -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the Willow Garage nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef MESSAGE_FILTERS__SYNCHRONIZER_H_ #define MESSAGE_FILTERS__SYNCHRONIZER_H_ -#include -#include -#include -#include -#include -#include -#include - -#include "message_filters/connection.h" -#include "message_filters/null_types.h" -#include "message_filters/message_event.h" -#include "message_filters/signal9.h" - -namespace message_filters -{ - -template -class Synchronizer : public noncopyable, public Policy -{ -public: - typedef typename Policy::Messages Messages; - typedef typename Policy::Events Events; - typedef typename Policy::Signal Signal; - - typedef typename std::tuple_element<0, Messages>::type M0; - typedef typename std::tuple_element<1, Messages>::type M1; - typedef typename std::tuple_element<2, Messages>::type M2; - typedef typename std::tuple_element<3, Messages>::type M3; - typedef typename std::tuple_element<4, Messages>::type M4; - typedef typename std::tuple_element<5, Messages>::type M5; - typedef typename std::tuple_element<6, Messages>::type M6; - typedef typename std::tuple_element<7, Messages>::type M7; - typedef typename std::tuple_element<8, Messages>::type M8; - - - typedef typename std::tuple_element<0, Events>::type M0Event; - typedef typename std::tuple_element<1, Events>::type M1Event; - typedef typename std::tuple_element<2, Events>::type M2Event; - typedef typename std::tuple_element<3, Events>::type M3Event; - typedef typename std::tuple_element<4, Events>::type M4Event; - typedef typename std::tuple_element<5, Events>::type M5Event; - typedef typename std::tuple_element<6, Events>::type M6Event; - typedef typename std::tuple_element<7, Events>::type M7Event; - typedef typename std::tuple_element<8, Events>::type M8Event; - - static const uint8_t MAX_MESSAGES = 9; - - template - Synchronizer(F0 & f0, F1 & f1) - { - connectInput(f0, f1); - init(); - } - - template - Synchronizer(F0 & f0, F1 & f1, F2 & f2) - { - connectInput(f0, f1, f2); - init(); - } - - template - Synchronizer(F0 & f0, F1 & f1, F2 & f2, F3 & f3) - { - connectInput(f0, f1, f2, f3); - init(); - } - - template - Synchronizer(F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4) - { - connectInput(f0, f1, f2, f3, f4); - init(); - } - - template - Synchronizer(F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5) - { - connectInput(f0, f1, f2, f3, f4, f5); - init(); - } - - template - Synchronizer(F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5, F6 & f6) - { - connectInput(f0, f1, f2, f3, f4, f5, f6); - init(); - } - - template - Synchronizer(F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5, F6 & f6, F7 & f7) - { - connectInput(f0, f1, f2, f3, f4, f5, f6, f7); - init(); - } - - template - Synchronizer(F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5, F6 & f6, F7 & f7, F8 & f8) - { - connectInput(f0, f1, f2, f3, f4, f5, f6, f7, f8); - init(); - } - - Synchronizer() - { - init(); - } - - template - Synchronizer(const Policy & policy, F0 & f0, F1 & f1) - : Policy(policy) - { - connectInput(f0, f1); - init(); - } - - template - Synchronizer(const Policy & policy, F0 & f0, F1 & f1, F2 & f2) - : Policy(policy) - { - connectInput(f0, f1, f2); - init(); - } - - template - Synchronizer(const Policy & policy, F0 & f0, F1 & f1, F2 & f2, F3 & f3) - : Policy(policy) - { - connectInput(f0, f1, f2, f3); - init(); - } - - template - Synchronizer(const Policy & policy, F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4) - : Policy(policy) - { - connectInput(f0, f1, f2, f3, f4); - init(); - } - - template - Synchronizer(const Policy & policy, F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5) - : Policy(policy) - { - connectInput(f0, f1, f2, f3, f4, f5); - init(); - } - - template - Synchronizer(const Policy & policy, F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5, F6 & f6) - : Policy(policy) - { - connectInput(f0, f1, f2, f3, f4, f5, f6); - init(); - } - - template - Synchronizer( - const Policy & policy, F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5, F6 & f6, F7 & f7) - : Policy(policy) - { - connectInput(f0, f1, f2, f3, f4, f5, f6, f7); - init(); - } - - template - Synchronizer( - const Policy & policy, F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5, F6 & f6, F7 & f7, - F8 & f8) - : Policy(policy) - { - connectInput(f0, f1, f2, f3, f4, f5, f6, f7, f8); - init(); - } - - Synchronizer(const Policy & policy) // NOLINT(runtime/explicit) - : Policy(policy) - { - init(); - } - - ~Synchronizer() - { - disconnectAll(); - } - - void init() - { - Policy::initParent(this); - } - - template - void connectInput(F0 & f0, F1 & f1) - { - NullFilter f2; - connectInput(f0, f1, f2); - } - - template - 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) - { - NullFilter f4; - connectInput(f0, f1, f2, f3, f4); - } - - template - void connectInput(F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4) - { - NullFilter f5; - connectInput(f0, f1, f2, f3, f4, f5); - } - - template - void connectInput(F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5) - { - NullFilter f6; - connectInput(f0, f1, f2, f3, f4, f5, f6); - } - - template - void connectInput(F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5, F6 & f6) - { - NullFilter f7; - connectInput(f0, f1, f2, f3, f4, f5, f6, f7); - } - - template - void connectInput(F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5, F6 & f6, F7 & f7) - { - NullFilter f8; - connectInput(f0, f1, f2, f3, f4, f5, f6, f7, f8); - } - - template - void connectInput(F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5, F6 & f6, F7 & f7, F8 & f8) - { - 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))); - } - - template - Connection registerCallback(C & callback) - { - return signal_.addCallback(callback); - } - - template - Connection registerCallback(const C & callback) - { - return signal_.addCallback(callback); - } - - template - Connection registerCallback(const C & callback, T * t) - { - return signal_.addCallback(callback, t); - } - - template - 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 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);} - - using Policy::add; - - template - 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) { - input_connections_[i].disconnect(); - } - } - - template - void cb(const typename std::tuple_element::type & evt) - { - this->template add(evt); - } - - uint32_t queue_size_; - - Signal signal_; - - Connection input_connections_[MAX_MESSAGES]; - - std::string name_; -}; - -template -struct mp_plus; -template<> -struct mp_plus<> -{ - using type = std::integral_constant; -}; -template -struct mp_plus -{ - static constexpr auto _v = !T1::value + mp_plus::type::value; - using type = std::integral_constant< - typename std::remove_const::type, _v>; -}; - -template -struct mp_count; -template class L, class ... T, class V> -struct mp_count, V> -{ - using type = typename mp_plus...>::type; -}; - -template -struct PolicyBase -{ - typedef typename mp_count, NullType>::type - RealTypeCount; - typedef std::tuple Messages; - typedef Signal9 Signal; - typedef std::tuple, MessageEvent, MessageEvent, - MessageEvent, MessageEvent, MessageEvent, - MessageEvent, MessageEvent, MessageEvent> Events; - - typedef typename std::tuple_element<0, Events>::type M0Event; - typedef typename std::tuple_element<1, Events>::type M1Event; - typedef typename std::tuple_element<2, Events>::type M2Event; - typedef typename std::tuple_element<3, Events>::type M3Event; - typedef typename std::tuple_element<4, Events>::type M4Event; - typedef typename std::tuple_element<5, Events>::type M5Event; - typedef typename std::tuple_element<6, Events>::type M6Event; - typedef typename std::tuple_element<7, Events>::type M7Event; - typedef typename std::tuple_element<8, Events>::type M8Event; -}; +#warning This header is obsolete, please include message_filters/synchronizer.hpp instead -} // namespace message_filters +#include #endif // MESSAGE_FILTERS__SYNCHRONIZER_H_ diff --git a/include/message_filters/synchronizer.hpp b/include/message_filters/synchronizer.hpp new file mode 100644 index 0000000..2ee0d7d --- /dev/null +++ b/include/message_filters/synchronizer.hpp @@ -0,0 +1,423 @@ +// Copyright 2009, Willow Garage, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef MESSAGE_FILTERS__SYNCHRONIZER_HPP_ +#define MESSAGE_FILTERS__SYNCHRONIZER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "message_filters/connection.hpp" +#include "message_filters/null_types.hpp" +#include "message_filters/message_event.hpp" +#include "message_filters/signal9.hpp" + +namespace message_filters +{ + +template +class Synchronizer : public noncopyable, public Policy +{ +public: + typedef typename Policy::Messages Messages; + typedef typename Policy::Events Events; + typedef typename Policy::Signal Signal; + + typedef typename std::tuple_element<0, Messages>::type M0; + typedef typename std::tuple_element<1, Messages>::type M1; + typedef typename std::tuple_element<2, Messages>::type M2; + typedef typename std::tuple_element<3, Messages>::type M3; + typedef typename std::tuple_element<4, Messages>::type M4; + typedef typename std::tuple_element<5, Messages>::type M5; + typedef typename std::tuple_element<6, Messages>::type M6; + typedef typename std::tuple_element<7, Messages>::type M7; + typedef typename std::tuple_element<8, Messages>::type M8; + + + typedef typename std::tuple_element<0, Events>::type M0Event; + typedef typename std::tuple_element<1, Events>::type M1Event; + typedef typename std::tuple_element<2, Events>::type M2Event; + typedef typename std::tuple_element<3, Events>::type M3Event; + typedef typename std::tuple_element<4, Events>::type M4Event; + typedef typename std::tuple_element<5, Events>::type M5Event; + typedef typename std::tuple_element<6, Events>::type M6Event; + typedef typename std::tuple_element<7, Events>::type M7Event; + typedef typename std::tuple_element<8, Events>::type M8Event; + + static const uint8_t MAX_MESSAGES = 9; + + template + Synchronizer(F0 & f0, F1 & f1) + { + connectInput(f0, f1); + init(); + } + + template + Synchronizer(F0 & f0, F1 & f1, F2 & f2) + { + connectInput(f0, f1, f2); + init(); + } + + template + Synchronizer(F0 & f0, F1 & f1, F2 & f2, F3 & f3) + { + connectInput(f0, f1, f2, f3); + init(); + } + + template + Synchronizer(F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4) + { + connectInput(f0, f1, f2, f3, f4); + init(); + } + + template + Synchronizer(F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5) + { + connectInput(f0, f1, f2, f3, f4, f5); + init(); + } + + template + Synchronizer(F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5, F6 & f6) + { + connectInput(f0, f1, f2, f3, f4, f5, f6); + init(); + } + + template + Synchronizer(F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5, F6 & f6, F7 & f7) + { + connectInput(f0, f1, f2, f3, f4, f5, f6, f7); + init(); + } + + template + Synchronizer(F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5, F6 & f6, F7 & f7, F8 & f8) + { + connectInput(f0, f1, f2, f3, f4, f5, f6, f7, f8); + init(); + } + + Synchronizer() + { + init(); + } + + template + Synchronizer(const Policy & policy, F0 & f0, F1 & f1) + : Policy(policy) + { + connectInput(f0, f1); + init(); + } + + template + Synchronizer(const Policy & policy, F0 & f0, F1 & f1, F2 & f2) + : Policy(policy) + { + connectInput(f0, f1, f2); + init(); + } + + template + Synchronizer(const Policy & policy, F0 & f0, F1 & f1, F2 & f2, F3 & f3) + : Policy(policy) + { + connectInput(f0, f1, f2, f3); + init(); + } + + template + Synchronizer(const Policy & policy, F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4) + : Policy(policy) + { + connectInput(f0, f1, f2, f3, f4); + init(); + } + + template + Synchronizer(const Policy & policy, F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5) + : Policy(policy) + { + connectInput(f0, f1, f2, f3, f4, f5); + init(); + } + + template + Synchronizer(const Policy & policy, F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5, F6 & f6) + : Policy(policy) + { + connectInput(f0, f1, f2, f3, f4, f5, f6); + init(); + } + + template + Synchronizer( + const Policy & policy, F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5, F6 & f6, F7 & f7) + : Policy(policy) + { + connectInput(f0, f1, f2, f3, f4, f5, f6, f7); + init(); + } + + template + Synchronizer( + const Policy & policy, F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5, F6 & f6, F7 & f7, + F8 & f8) + : Policy(policy) + { + connectInput(f0, f1, f2, f3, f4, f5, f6, f7, f8); + init(); + } + + Synchronizer(const Policy & policy) // NOLINT(runtime/explicit) + : Policy(policy) + { + init(); + } + + ~Synchronizer() + { + disconnectAll(); + } + + void init() + { + Policy::initParent(this); + } + + template + void connectInput(F0 & f0, F1 & f1) + { + NullFilter f2; + connectInput(f0, f1, f2); + } + + template + 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) + { + NullFilter f4; + connectInput(f0, f1, f2, f3, f4); + } + + template + void connectInput(F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4) + { + NullFilter f5; + connectInput(f0, f1, f2, f3, f4, f5); + } + + template + void connectInput(F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5) + { + NullFilter f6; + connectInput(f0, f1, f2, f3, f4, f5, f6); + } + + template + void connectInput(F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5, F6 & f6) + { + NullFilter f7; + connectInput(f0, f1, f2, f3, f4, f5, f6, f7); + } + + template + void connectInput(F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5, F6 & f6, F7 & f7) + { + NullFilter f8; + connectInput(f0, f1, f2, f3, f4, f5, f6, f7, f8); + } + + template + void connectInput(F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5, F6 & f6, F7 & f7, F8 & f8) + { + 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))); + } + + template + Connection registerCallback(C & callback) + { + return signal_.addCallback(callback); + } + + template + Connection registerCallback(const C & callback) + { + return signal_.addCallback(callback); + } + + template + Connection registerCallback(const C & callback, T * t) + { + return signal_.addCallback(callback, t); + } + + template + 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 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);} + + using Policy::add; + + template + 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) { + input_connections_[i].disconnect(); + } + } + + template + void cb(const typename std::tuple_element::type & evt) + { + this->template add(evt); + } + + uint32_t queue_size_; + + Signal signal_; + + Connection input_connections_[MAX_MESSAGES]; + + std::string name_; +}; + +template +struct mp_plus; +template<> +struct mp_plus<> +{ + using type = std::integral_constant; +}; +template +struct mp_plus +{ + static constexpr auto _v = !T1::value + mp_plus::type::value; + using type = std::integral_constant< + typename std::remove_const::type, _v>; +}; + +template +struct mp_count; +template class L, class ... T, class V> +struct mp_count, V> +{ + using type = typename mp_plus...>::type; +}; + +template +struct PolicyBase +{ + typedef typename mp_count, NullType>::type + RealTypeCount; + typedef std::tuple Messages; + typedef Signal9 Signal; + typedef std::tuple, MessageEvent, MessageEvent, + MessageEvent, MessageEvent, MessageEvent, + MessageEvent, MessageEvent, MessageEvent> Events; + + typedef typename std::tuple_element<0, Events>::type M0Event; + typedef typename std::tuple_element<1, Events>::type M1Event; + typedef typename std::tuple_element<2, Events>::type M2Event; + typedef typename std::tuple_element<3, Events>::type M3Event; + typedef typename std::tuple_element<4, Events>::type M4Event; + typedef typename std::tuple_element<5, Events>::type M5Event; + typedef typename std::tuple_element<6, Events>::type M6Event; + typedef typename std::tuple_element<7, Events>::type M7Event; + typedef typename std::tuple_element<8, Events>::type M8Event; +}; + +} // namespace message_filters + +#endif // MESSAGE_FILTERS__SYNCHRONIZER_HPP_ diff --git a/include/message_filters/time_sequencer.h b/include/message_filters/time_sequencer.h index 7350fb0..73955bd 100644 --- a/include/message_filters/time_sequencer.h +++ b/include/message_filters/time_sequencer.h @@ -1,244 +1,22 @@ -// Copyright 2008, Willow Garage, Inc. All rights reserved. +// Copyright 2024 Open Source Robotics Foundation, Inc. All rights reserved. // -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. +// http://www.apache.org/licenses/LICENSE-2.0 // -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the Willow Garage nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef MESSAGE_FILTERS__TIME_SEQUENCER_H_ #define MESSAGE_FILTERS__TIME_SEQUENCER_H_ -#include -#include -#include -#include -#include -#include - -#include - -#include "message_filters/connection.h" -#include "message_filters/message_traits.h" -#include "message_filters/simple_filter.h" - -namespace message_filters -{ - -/** - * \class TimeSequencer - * - * \brief Sequences messages based on the timestamp of their header. - * - * The TimeSequencer object is templated on the type of message being sequenced. - * - * \section behavior BEHAVIOR - - * At construction, the TimeSequencer takes a rclcpp::Duration - * "delay" which specifies how long to queue up messages to - * provide a time sequencing over them. As messages arrive they are - * sorted according to their time stamps. A callback for a message is - * never invoked until the messages' time stamp is out of date by at - * least delay. However, for all messages which are out of date - * by at least delay, their callback are invoked and guaranteed - * to be in temporal order. If a message arrives from a time \b prior - * to a message which has already had its callback invoked, it is - * thrown away. - * - * \section connections CONNECTIONS - * - * TimeSequencer's input and output connections are both of the same signature as rclcpp subscription callbacks, ie. -\verbatim -void callback(const std::shared_ptr&); -\endverbatim - * - */ -template -class TimeSequencer : public SimpleFilter -{ -public: - typedef std::shared_ptr MConstPtr; - typedef MessageEvent EventType; - - /** - * \brief Constructor - * \param f A filter to connect this sequencer's input to - * \param delay The minimum time to hold a message before passing it through. - * \param update_rate The rate at which to check for messages which have passed "delay" - * \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 - */ - template - 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) - { - init(); - connectInput(f); - } - - /** - * \brief Constructor - * - * This version of the constructor does not take a filter immediately. You can connect to a filter later with the connectInput() function - * - * \param delay The minimum time to hold a message before passing it through. - * \param update_rate The rate at which to check for messages which have passed "delay" - * \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) - : delay_(delay) - , update_rate_(update_rate) - , queue_size_(queue_size) - , node_(node) - { - init(); - } - - /** - * \brief Connect this filter's input to another filter's output. - */ - template - void connectInput(F & f) - { - incoming_connection_.disconnect(); - incoming_connection_ = - f.registerCallback( - typename SimpleFilter::EventCallback( - std::bind( - &TimeSequencer::cb, this, - std::placeholders::_1))); - } - - ~TimeSequencer() - { - update_timer_->cancel(); - incoming_connection_.disconnect(); - } - - 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_) { - return; - } - - messages_.insert(evt); - - if (queue_size_ != 0 && messages_.size() > queue_size_) { - messages_.erase(*messages_.begin()); - } - } - - /** - * \brief Manually add a message to the cache. - */ - void add(const MConstPtr & msg) - { - EventType evt(msg); - add(evt); - } - -private: - class MessageSort - { -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()); - } - }; - typedef std::multiset S_Message; - typedef std::vector V_Message; - - void cb(const EventType & evt) - { - add(evt); - } - - void dispatch() - { - namespace mt = message_filters::message_traits; - - V_Message to_call; - - { - std::lock_guard lock(messages_mutex_); - - while (!messages_.empty()) { - const EventType & e = *messages_.begin(); - rclcpp::Time stamp = mt::TimeStamp::value(*e.getMessage()); - if ((stamp + delay_) <= rclcpp::Clock().now()) { - last_time_ = stamp; - to_call.push_back(e); - messages_.erase(messages_.begin()); - } else { - break; - } - } - } - - { - typename V_Message::iterator it = to_call.begin(); - typename V_Message::iterator end = to_call.end(); - for (; it != end; ++it) { - this->signalMessage(*it); - } - } - } - - void init() - { - update_timer_ = node_->create_wall_timer( - std::chrono::nanoseconds(update_rate_.nanoseconds()), [this]() { - dispatch(); - }); - } - - rclcpp::Duration delay_; - rclcpp::Duration update_rate_; - uint32_t queue_size_; - rclcpp::Node::SharedPtr node_; - rclcpp::TimerBase::SharedPtr update_timer_; - Connection incoming_connection_; - - - S_Message messages_; - std::mutex messages_mutex_; - rclcpp::Time last_time_; -}; +#warning This header is obsolete, please include message_filters/time_sequencer.hpp instead -} // namespace message_filters +#include #endif // MESSAGE_FILTERS__TIME_SEQUENCER_H_ diff --git a/include/message_filters/time_sequencer.hpp b/include/message_filters/time_sequencer.hpp new file mode 100644 index 0000000..2690d74 --- /dev/null +++ b/include/message_filters/time_sequencer.hpp @@ -0,0 +1,244 @@ +// Copyright 2008, Willow Garage, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef MESSAGE_FILTERS__TIME_SEQUENCER_HPP_ +#define MESSAGE_FILTERS__TIME_SEQUENCER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include + +#include "message_filters/connection.hpp" +#include "message_filters/message_traits.hpp" +#include "message_filters/simple_filter.hpp" + +namespace message_filters +{ + +/** + * \class TimeSequencer + * + * \brief Sequences messages based on the timestamp of their header. + * + * The TimeSequencer object is templated on the type of message being sequenced. + * + * \section behavior BEHAVIOR + + * At construction, the TimeSequencer takes a rclcpp::Duration + * "delay" which specifies how long to queue up messages to + * provide a time sequencing over them. As messages arrive they are + * sorted according to their time stamps. A callback for a message is + * never invoked until the messages' time stamp is out of date by at + * least delay. However, for all messages which are out of date + * by at least delay, their callback are invoked and guaranteed + * to be in temporal order. If a message arrives from a time \b prior + * to a message which has already had its callback invoked, it is + * thrown away. + * + * \section connections CONNECTIONS + * + * TimeSequencer's input and output connections are both of the same signature as rclcpp subscription callbacks, ie. +\verbatim +void callback(const std::shared_ptr&); +\endverbatim + * + */ +template +class TimeSequencer : public SimpleFilter +{ +public: + typedef std::shared_ptr MConstPtr; + typedef MessageEvent EventType; + + /** + * \brief Constructor + * \param f A filter to connect this sequencer's input to + * \param delay The minimum time to hold a message before passing it through. + * \param update_rate The rate at which to check for messages which have passed "delay" + * \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 + */ + template + 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) + { + init(); + connectInput(f); + } + + /** + * \brief Constructor + * + * This version of the constructor does not take a filter immediately. You can connect to a filter later with the connectInput() function + * + * \param delay The minimum time to hold a message before passing it through. + * \param update_rate The rate at which to check for messages which have passed "delay" + * \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) + : delay_(delay) + , update_rate_(update_rate) + , queue_size_(queue_size) + , node_(node) + { + init(); + } + + /** + * \brief Connect this filter's input to another filter's output. + */ + template + void connectInput(F & f) + { + incoming_connection_.disconnect(); + incoming_connection_ = + f.registerCallback( + typename SimpleFilter::EventCallback( + std::bind( + &TimeSequencer::cb, this, + std::placeholders::_1))); + } + + ~TimeSequencer() + { + update_timer_->cancel(); + incoming_connection_.disconnect(); + } + + 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_) { + return; + } + + messages_.insert(evt); + + if (queue_size_ != 0 && messages_.size() > queue_size_) { + messages_.erase(*messages_.begin()); + } + } + + /** + * \brief Manually add a message to the cache. + */ + void add(const MConstPtr & msg) + { + EventType evt(msg); + add(evt); + } + +private: + class MessageSort + { +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()); + } + }; + typedef std::multiset S_Message; + typedef std::vector V_Message; + + void cb(const EventType & evt) + { + add(evt); + } + + void dispatch() + { + namespace mt = message_filters::message_traits; + + V_Message to_call; + + { + std::lock_guard lock(messages_mutex_); + + while (!messages_.empty()) { + const EventType & e = *messages_.begin(); + rclcpp::Time stamp = mt::TimeStamp::value(*e.getMessage()); + if ((stamp + delay_) <= rclcpp::Clock().now()) { + last_time_ = stamp; + to_call.push_back(e); + messages_.erase(messages_.begin()); + } else { + break; + } + } + } + + { + typename V_Message::iterator it = to_call.begin(); + typename V_Message::iterator end = to_call.end(); + for (; it != end; ++it) { + this->signalMessage(*it); + } + } + } + + void init() + { + update_timer_ = node_->create_wall_timer( + std::chrono::nanoseconds(update_rate_.nanoseconds()), [this]() { + dispatch(); + }); + } + + rclcpp::Duration delay_; + rclcpp::Duration update_rate_; + uint32_t queue_size_; + rclcpp::Node::SharedPtr node_; + rclcpp::TimerBase::SharedPtr update_timer_; + Connection incoming_connection_; + + + S_Message messages_; + std::mutex messages_mutex_; + rclcpp::Time last_time_; +}; + +} // namespace message_filters + +#endif // MESSAGE_FILTERS__TIME_SEQUENCER_HPP_ diff --git a/include/message_filters/time_synchronizer.h b/include/message_filters/time_synchronizer.h index f74c236..6c2da95 100644 --- a/include/message_filters/time_synchronizer.h +++ b/include/message_filters/time_synchronizer.h @@ -1,228 +1,22 @@ -// Copyright 2009, Willow Garage, Inc. All rights reserved. +// Copyright 2024 Open Source Robotics Foundation, Inc. All rights reserved. // -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. +// http://www.apache.org/licenses/LICENSE-2.0 // -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the Willow Garage nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. - +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #ifndef MESSAGE_FILTERS__TIME_SYNCHRONIZER_H_ #define MESSAGE_FILTERS__TIME_SYNCHRONIZER_H_ -#include - -#include "message_filters/message_event.h" -#include "message_filters/synchronizer.h" -#include "message_filters/sync_policies/exact_time.h" - -namespace message_filters -{ - -/** - * \brief Synchronizes up to 9 messages by their timestamps. - * - * TimeSynchronizer synchronizes up to 9 incoming channels by the timestamps contained in their messages' headers. - * TimeSynchronizer takes anywhere from 2 to 9 message types as template parameters, and passes them through to a - * callback which takes a shared pointer of each. - * - * The required queue size parameter when constructing the TimeSynchronizer tells it how many sets of messages it should - * store (by timestamp) while waiting for messages to arrive and complete their "set" - * - * \section connections CONNECTIONS - * - * The input connections for the TimeSynchronizer object is the same signature as for rclcpp subscription callbacks, ie. -\verbatim -void callback(const std::shared_ptr&); -\endverbatim - * The output connection for the TimeSynchronizer object is dependent on the number of messages being synchronized. For - * a 3-message synchronizer for example, it would be: -\verbatim -void callback(const std::shared_ptr&, const std::shared_ptr&, const std::shared_ptr&); -\endverbatim - * \section usage USAGE - * Example usage would be: -\verbatim -TimeSynchronizer sync_policies(caminfo_sub, limage_sub, rimage_sub, 3); -sync_policies.registerCallback(callback); -\endverbatim - - * The callback is then of the form: -\verbatim -void callback(const sensor_msgs::msg::CameraInfo::SharedPtr, const sensor_msgs::msg::Image::SharedPtr, const sensor_msgs::msg::Image::SharedPtr); -\endverbatim - * - */ -template -class TimeSynchronizer : public Synchronizer> -{ -public: - typedef sync_policies::ExactTime Policy; - typedef Synchronizer Base; - typedef std::shared_ptr M0ConstPtr; - typedef std::shared_ptr M1ConstPtr; - typedef std::shared_ptr M2ConstPtr; - typedef std::shared_ptr M3ConstPtr; - typedef std::shared_ptr M4ConstPtr; - typedef std::shared_ptr M5ConstPtr; - typedef std::shared_ptr M6ConstPtr; - typedef std::shared_ptr M7ConstPtr; - typedef std::shared_ptr M8ConstPtr; - - using Base::add; - using Base::connectInput; - using Base::registerCallback; - using Base::setName; - using Base::getName; - using Policy::registerDropCallback; - typedef typename Base::M0Event M0Event; - typedef typename Base::M1Event M1Event; - typedef typename Base::M2Event M2Event; - typedef typename Base::M3Event M3Event; - typedef typename Base::M4Event M4Event; - typedef typename Base::M5Event M5Event; - typedef typename Base::M6Event M6Event; - typedef typename Base::M7Event M7Event; - typedef typename Base::M8Event M8Event; - - template - 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) - : Base(Policy(queue_size)) - { - connectInput(f0, f1, f2); - } - - template - 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) - : 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) - : 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) - : 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) - : 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) - : Base(Policy(queue_size)) - { - connectInput(f0, f1, f2, f3, f4, f5, f6, f7, f8); - } - - TimeSynchronizer(uint32_t queue_size) // NOLINT(runtime/explicit) - : Base(Policy(queue_size)) - { - } - - //////////////////////////////////////////////////////////////// - // For backwards compatibility - //////////////////////////////////////////////////////////////// - void add0(const M0ConstPtr & msg) - { - this->template add<0>(M0Event(msg)); - } - - void add1(const M1ConstPtr & msg) - { - this->template add<1>(M1Event(msg)); - } - - void add2(const M2ConstPtr & msg) - { - this->template add<2>(M2Event(msg)); - } - - void add3(const M3ConstPtr & msg) - { - this->template add<3>(M3Event(msg)); - } - - void add4(const M4ConstPtr & msg) - { - this->template add<4>(M4Event(msg)); - } - - void add5(const M5ConstPtr & msg) - { - this->template add<5>(M5Event(msg)); - } - - void add6(const M6ConstPtr & msg) - { - this->template add<6>(M6Event(msg)); - } - - void add7(const M7ConstPtr & msg) - { - this->template add<7>(M7Event(msg)); - } - - void add8(const M8ConstPtr & msg) - { - this->template add<8>(M8Event(msg)); - } -}; +#warning This header is obsolete, please include message_filters/time_synchronizer.hpp instead -} // namespace message_filters +#include #endif // MESSAGE_FILTERS__TIME_SYNCHRONIZER_H_ diff --git a/include/message_filters/time_synchronizer.hpp b/include/message_filters/time_synchronizer.hpp new file mode 100644 index 0000000..cb82e4e --- /dev/null +++ b/include/message_filters/time_synchronizer.hpp @@ -0,0 +1,228 @@ +// Copyright 2009, Willow Garage, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + + +#ifndef MESSAGE_FILTERS__TIME_SYNCHRONIZER_HPP_ +#define MESSAGE_FILTERS__TIME_SYNCHRONIZER_HPP_ + +#include + +#include "message_filters/message_event.hpp" +#include "message_filters/synchronizer.hpp" +#include "message_filters/sync_policies/exact_time.hpp" + +namespace message_filters +{ + +/** + * \brief Synchronizes up to 9 messages by their timestamps. + * + * TimeSynchronizer synchronizes up to 9 incoming channels by the timestamps contained in their messages' headers. + * TimeSynchronizer takes anywhere from 2 to 9 message types as template parameters, and passes them through to a + * callback which takes a shared pointer of each. + * + * The required queue size parameter when constructing the TimeSynchronizer tells it how many sets of messages it should + * store (by timestamp) while waiting for messages to arrive and complete their "set" + * + * \section connections CONNECTIONS + * + * The input connections for the TimeSynchronizer object is the same signature as for rclcpp subscription callbacks, ie. +\verbatim +void callback(const std::shared_ptr&); +\endverbatim + * The output connection for the TimeSynchronizer object is dependent on the number of messages being synchronized. For + * a 3-message synchronizer for example, it would be: +\verbatim +void callback(const std::shared_ptr&, const std::shared_ptr&, const std::shared_ptr&); +\endverbatim + * \section usage USAGE + * Example usage would be: +\verbatim +TimeSynchronizer sync_policies(caminfo_sub, limage_sub, rimage_sub, 3); +sync_policies.registerCallback(callback); +\endverbatim + + * The callback is then of the form: +\verbatim +void callback(const sensor_msgs::msg::CameraInfo::SharedPtr, const sensor_msgs::msg::Image::SharedPtr, const sensor_msgs::msg::Image::SharedPtr); +\endverbatim + * + */ +template +class TimeSynchronizer : public Synchronizer> +{ +public: + typedef sync_policies::ExactTime Policy; + typedef Synchronizer Base; + typedef std::shared_ptr M0ConstPtr; + typedef std::shared_ptr M1ConstPtr; + typedef std::shared_ptr M2ConstPtr; + typedef std::shared_ptr M3ConstPtr; + typedef std::shared_ptr M4ConstPtr; + typedef std::shared_ptr M5ConstPtr; + typedef std::shared_ptr M6ConstPtr; + typedef std::shared_ptr M7ConstPtr; + typedef std::shared_ptr M8ConstPtr; + + using Base::add; + using Base::connectInput; + using Base::registerCallback; + using Base::setName; + using Base::getName; + using Policy::registerDropCallback; + typedef typename Base::M0Event M0Event; + typedef typename Base::M1Event M1Event; + typedef typename Base::M2Event M2Event; + typedef typename Base::M3Event M3Event; + typedef typename Base::M4Event M4Event; + typedef typename Base::M5Event M5Event; + typedef typename Base::M6Event M6Event; + typedef typename Base::M7Event M7Event; + typedef typename Base::M8Event M8Event; + + template + 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) + : Base(Policy(queue_size)) + { + connectInput(f0, f1, f2); + } + + template + 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) + : 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) + : 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) + : 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) + : 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) + : Base(Policy(queue_size)) + { + connectInput(f0, f1, f2, f3, f4, f5, f6, f7, f8); + } + + TimeSynchronizer(uint32_t queue_size) // NOLINT(runtime/explicit) + : Base(Policy(queue_size)) + { + } + + //////////////////////////////////////////////////////////////// + // For backwards compatibility + //////////////////////////////////////////////////////////////// + void add0(const M0ConstPtr & msg) + { + this->template add<0>(M0Event(msg)); + } + + void add1(const M1ConstPtr & msg) + { + this->template add<1>(M1Event(msg)); + } + + void add2(const M2ConstPtr & msg) + { + this->template add<2>(M2Event(msg)); + } + + void add3(const M3ConstPtr & msg) + { + this->template add<3>(M3Event(msg)); + } + + void add4(const M4ConstPtr & msg) + { + this->template add<4>(M4Event(msg)); + } + + void add5(const M5ConstPtr & msg) + { + this->template add<5>(M5Event(msg)); + } + + void add6(const M6ConstPtr & msg) + { + this->template add<6>(M6Event(msg)); + } + + void add7(const M7ConstPtr & msg) + { + this->template add<7>(M7Event(msg)); + } + + void add8(const M8ConstPtr & msg) + { + this->template add<8>(M8Event(msg)); + } +}; + +} // namespace message_filters + +#endif // MESSAGE_FILTERS__TIME_SYNCHRONIZER_HPP_ diff --git a/include/message_filters/visibility_control.h b/include/message_filters/visibility_control.h index 43d52e3..87f16ab 100644 --- a/include/message_filters/visibility_control.h +++ b/include/message_filters/visibility_control.h @@ -1,4 +1,4 @@ -// Copyright 2018 Open Source Robotics Foundation, Inc. +// Copyright 2024 Open Source Robotics Foundation, Inc. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -15,42 +15,8 @@ #ifndef MESSAGE_FILTERS__VISIBILITY_CONTROL_H_ #define MESSAGE_FILTERS__VISIBILITY_CONTROL_H_ -#ifdef __cplusplus -extern "C" -{ -#endif +#warning This header is obsolete, please include message_filters/visibility_control.hpp instead -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ - #ifdef __GNUC__ - #define MESSAGE_FILTERS_EXPORT __attribute__ ((dllexport)) - #define MESSAGE_FILTERS_IMPORT __attribute__ ((dllimport)) - #else - #define MESSAGE_FILTERS_EXPORT __declspec(dllexport) - #define MESSAGE_FILTERS_IMPORT __declspec(dllimport) - #endif - #ifdef MESSAGE_FILTERS_BUILDING_DLL - #define MESSAGE_FILTERS_PUBLIC MESSAGE_FILTERS_EXPORT - #else - #define MESSAGE_FILTERS_PUBLIC MESSAGE_FILTERS_IMPORT - #endif - #define MESSAGE_FILTERS_LOCAL -#else - #define MESSAGE_FILTERS_EXPORT __attribute__ ((visibility("default"))) - #define MESSAGE_FILTERS_IMPORT - #if __GNUC__ >= 4 - #define MESSAGE_FILTERS_PUBLIC __attribute__ ((visibility("default"))) - #define MESSAGE_FILTERS_LOCAL __attribute__ ((visibility("hidden"))) - #else - #define MESSAGE_FILTERS_PUBLIC - #define MESSAGE_FILTERS_LOCAL - #endif -#endif - -#ifdef __cplusplus -} -#endif +#include #endif // MESSAGE_FILTERS__VISIBILITY_CONTROL_H_ diff --git a/include/message_filters/visibility_control.hpp b/include/message_filters/visibility_control.hpp new file mode 100644 index 0000000..11d7fb4 --- /dev/null +++ b/include/message_filters/visibility_control.hpp @@ -0,0 +1,56 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MESSAGE_FILTERS__VISIBILITY_CONTROL_HPP_ +#define MESSAGE_FILTERS__VISIBILITY_CONTROL_HPP_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define MESSAGE_FILTERS_EXPORT __attribute__ ((dllexport)) + #define MESSAGE_FILTERS_IMPORT __attribute__ ((dllimport)) + #else + #define MESSAGE_FILTERS_EXPORT __declspec(dllexport) + #define MESSAGE_FILTERS_IMPORT __declspec(dllimport) + #endif + #ifdef MESSAGE_FILTERS_BUILDING_DLL + #define MESSAGE_FILTERS_PUBLIC MESSAGE_FILTERS_EXPORT + #else + #define MESSAGE_FILTERS_PUBLIC MESSAGE_FILTERS_IMPORT + #endif + #define MESSAGE_FILTERS_LOCAL +#else + #define MESSAGE_FILTERS_EXPORT __attribute__ ((visibility("default"))) + #define MESSAGE_FILTERS_IMPORT + #if __GNUC__ >= 4 + #define MESSAGE_FILTERS_PUBLIC __attribute__ ((visibility("default"))) + #define MESSAGE_FILTERS_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define MESSAGE_FILTERS_PUBLIC + #define MESSAGE_FILTERS_LOCAL + #endif +#endif + +#ifdef __cplusplus +} +#endif + +#endif // MESSAGE_FILTERS__VISIBILITY_CONTROL_HPP_ diff --git a/src/connection.cpp b/src/connection.cpp index 852102d..ed47575 100644 --- a/src/connection.cpp +++ b/src/connection.cpp @@ -26,7 +26,7 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. -#include "message_filters/connection.h" +#include "message_filters/connection.hpp" namespace message_filters { diff --git a/test/msg_cache_unittest.cpp b/test/msg_cache_unittest.cpp index f459ac3..53bcd10 100644 --- a/test/msg_cache_unittest.cpp +++ b/test/msg_cache_unittest.cpp @@ -33,8 +33,8 @@ #include #include -#include "message_filters/cache.h" -#include "message_filters/message_traits.h" +#include "message_filters/cache.hpp" +#include "message_filters/message_traits.hpp" struct Header { diff --git a/test/test_approximate_epsilon_time_policy.cpp b/test/test_approximate_epsilon_time_policy.cpp index caa5444..fe3dd3a 100644 --- a/test/test_approximate_epsilon_time_policy.cpp +++ b/test/test_approximate_epsilon_time_policy.cpp @@ -36,9 +36,9 @@ #include -#include "message_filters/synchronizer.h" -#include "message_filters/sync_policies/approximate_epsilon_time.h" -#include "message_filters/message_traits.h" +#include "message_filters/synchronizer.hpp" +#include "message_filters/sync_policies/approximate_epsilon_time.hpp" +#include "message_filters/message_traits.hpp" struct Header { diff --git a/test/test_approximate_time_policy.cpp b/test/test_approximate_time_policy.cpp index e778c5f..e804ee8 100644 --- a/test/test_approximate_time_policy.cpp +++ b/test/test_approximate_time_policy.cpp @@ -34,9 +34,9 @@ #include #include -#include "message_filters/synchronizer.h" -#include "message_filters/sync_policies/approximate_time.h" -#include "message_filters/message_traits.h" +#include "message_filters/synchronizer.hpp" +#include "message_filters/sync_policies/approximate_time.hpp" +#include "message_filters/message_traits.hpp" struct Header { diff --git a/test/test_chain.cpp b/test/test_chain.cpp index 49972cd..053b0d0 100644 --- a/test/test_chain.cpp +++ b/test/test_chain.cpp @@ -31,7 +31,7 @@ #include #include -#include "message_filters/chain.h" +#include "message_filters/chain.hpp" struct Msg { diff --git a/test/test_exact_time_policy.cpp b/test/test_exact_time_policy.cpp index 9d90374..17d8cde 100644 --- a/test/test_exact_time_policy.cpp +++ b/test/test_exact_time_policy.cpp @@ -32,8 +32,8 @@ #include #include -#include "message_filters/synchronizer.h" -#include "message_filters/sync_policies/exact_time.h" +#include "message_filters/synchronizer.hpp" +#include "message_filters/sync_policies/exact_time.hpp" struct Header { diff --git a/test/test_fuzz.cpp b/test/test_fuzz.cpp index 27b2f8d..030d646 100644 --- a/test/test_fuzz.cpp +++ b/test/test_fuzz.cpp @@ -36,10 +36,10 @@ #include #include -#include "message_filters/subscriber.h" -#include "message_filters/time_sequencer.h" -#include "message_filters/time_synchronizer.h" -#include "message_filters/chain.h" +#include "message_filters/subscriber.hpp" +#include "message_filters/time_sequencer.hpp" +#include "message_filters/time_synchronizer.hpp" +#include "message_filters/chain.hpp" #include "sensor_msgs/msg/imu.hpp" typedef sensor_msgs::msg::Imu Msg; diff --git a/test/test_latest_time_policy.cpp b/test/test_latest_time_policy.cpp index b171d46..d72b7d8 100644 --- a/test/test_latest_time_policy.cpp +++ b/test/test_latest_time_policy.cpp @@ -37,8 +37,8 @@ #include #include "rclcpp/time_source.hpp" -#include "message_filters/synchronizer.h" -#include "message_filters/sync_policies/latest_time.h" +#include "message_filters/synchronizer.hpp" +#include "message_filters/sync_policies/latest_time.hpp" #include "rosgraph_msgs/msg/clock.hpp" diff --git a/test/test_message_traits.cpp b/test/test_message_traits.cpp index 68ab770..c0df812 100644 --- a/test/test_message_traits.cpp +++ b/test/test_message_traits.cpp @@ -28,7 +28,7 @@ #include -#include "message_filters/message_traits.h" +#include "message_filters/message_traits.hpp" #include "rclcpp/time.hpp" #include "std_msgs/msg/header.hpp" diff --git a/test/test_simple.cpp b/test/test_simple.cpp index 287d271..e053a12 100644 --- a/test/test_simple.cpp +++ b/test/test_simple.cpp @@ -34,7 +34,7 @@ #include -#include "message_filters/simple_filter.h" +#include "message_filters/simple_filter.hpp" struct Msg { diff --git a/test/test_subscriber.cpp b/test/test_subscriber.cpp index 39c7cd9..18091df 100644 --- a/test/test_subscriber.cpp +++ b/test/test_subscriber.cpp @@ -34,8 +34,8 @@ #include #include -#include "message_filters/subscriber.h" -#include "message_filters/chain.h" +#include "message_filters/subscriber.hpp" +#include "message_filters/chain.hpp" #include "sensor_msgs/msg/imu.hpp" typedef sensor_msgs::msg::Imu Msg; diff --git a/test/test_synchronizer.cpp b/test/test_synchronizer.cpp index 472fd71..08c0d2a 100644 --- a/test/test_synchronizer.cpp +++ b/test/test_synchronizer.cpp @@ -32,7 +32,7 @@ #include #include -#include "message_filters/synchronizer.h" +#include "message_filters/synchronizer.hpp" struct Header { diff --git a/test/time_sequencer_unittest.cpp b/test/time_sequencer_unittest.cpp index e5ae6c6..0fe2fa6 100644 --- a/test/time_sequencer_unittest.cpp +++ b/test/time_sequencer_unittest.cpp @@ -31,7 +31,7 @@ #include #include -#include "message_filters/time_sequencer.h" +#include "message_filters/time_sequencer.hpp" struct Header { diff --git a/test/time_synchronizer_unittest.cpp b/test/time_synchronizer_unittest.cpp index 973802f..a41087e 100644 --- a/test/time_synchronizer_unittest.cpp +++ b/test/time_synchronizer_unittest.cpp @@ -30,9 +30,9 @@ #include -#include "message_filters/time_synchronizer.h" -#include "message_filters/pass_through.h" -#include "message_filters/message_traits.h" +#include "message_filters/time_synchronizer.hpp" +#include "message_filters/pass_through.hpp" +#include "message_filters/message_traits.hpp" #include struct Header