.. _program_listing_file__tmp_ws_src_message_filters_include_message_filters_sync_policies_approximate_epsilon_time.h:

Program Listing for File approximate_epsilon_time.h
===================================================

|exhale_lsh| :ref:`Return to documentation for file <file__tmp_ws_src_message_filters_include_message_filters_sync_policies_approximate_epsilon_time.h>` (``/tmp/ws/src/message_filters/include/message_filters/sync_policies/approximate_epsilon_time.h``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   /*********************************************************************
   * Software License Agreement (BSD License)
   *
   *  Copyright (c) 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 OWNER 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_H_
   #define MESSAGE_FILTERS__SYNC_POLICIES__APPROXIMATE_EPSILON_TIME_H_
   
   #include <deque>
   #include <string>
   #include <tuple>
   
   #include <rclcpp/rclcpp.hpp>
   
   #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<typename M0, typename M1, typename M2 = NullType, typename M3 = NullType, typename M4 = NullType,
            typename M5 = NullType, typename M6 = NullType, typename M7 = NullType, typename M8 = NullType>
   class ApproximateEpsilonTime : public PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8>
   {
   public:
     typedef Synchronizer<ApproximateEpsilonTime> Sync;
     typedef PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8> 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<size_t i>
     void add(const typename std::tuple_element<i, Events>::type& evt)
     {
       RCUTILS_ASSERT(parent_);
   
       std::lock_guard<std::mutex> lock(mutex_);
   
       auto & events_of_this_type = std::get<i>(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<i>();
       }
     }
   
   private:
     using TimeIndexPair = std::pair<rclcpp::Time, size_t>;
   
     template <size_t Is>
     TimeIndexPair
     get_older_timestamp_between(const TimeIndexPair & current)
     {
       namespace mt = message_filters::message_traits;
       using ThisEventType = typename std::tuple_element<Is, Events>::type;
       if constexpr (Is >= RealTypeCount::value) {
         return current;
       }
       const auto & events_of_this_type = std::get<Is>(events_);
       if (0u == events_of_this_type.size()) {
         // this condition should not happen
         return current;
       }
       auto candidate = mt::TimeStamp<typename ThisEventType::Message>::value(*events_of_this_type.at(0).getMessage());
       if (current.first > candidate) {
         return std::make_pair(candidate, Is);
       }
       return current;
     }
   
     template <size_t ... Is>
     TimeIndexPair
     get_older_timestamp_helper(std::index_sequence<Is...> const &)
     {
       TimeIndexPair older{
         rclcpp::Time(std::numeric_limits<int64_t>::max(), RCL_ROS_TIME), std::numeric_limits<size_t>::max()};
       ((older = get_older_timestamp_between<Is>(older)), ...);
       return older;
     }
   
     TimeIndexPair
     get_older_timestamp()
     {
       return get_older_timestamp_helper(std::make_index_sequence<9u>());
     }
   
     template <size_t Is>
     bool
     check_timestamp_within_epsilon(const TimeIndexPair & older)
     {
       namespace mt = message_filters::message_traits;
       using ThisEventType = typename std::tuple_element<Is, Events>::type;
       if constexpr (Is >= RealTypeCount::value) {
         return true;
       }
       if (Is == older.second) {
         return true;
       }
       const auto & events_of_this_type = std::get<Is>(events_);
       if (0u == events_of_this_type.size()) {
         // this condition should not happen
         return false;
       }
       auto ts = mt::TimeStamp<typename ThisEventType::Message>::value(*events_of_this_type.at(0).getMessage());
       if (older.first + epsilon_ >= ts) {
         return true;
       }
       return false;
     }
   
     template <size_t ... Is>
     bool
     check_all_timestamp_within_epsilon_helper(
       const TimeIndexPair & older, std::index_sequence<Is...> const &)
     {
       bool valid = true;
       ((valid &= check_timestamp_within_epsilon<Is>(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<size_t Is>
     void
     erase_beginning_of_vector()
     {
       if constexpr (Is >= RealTypeCount::value) {
         return;
       }
       auto & this_vector = std::get<Is>(events_);
       if (this_vector.begin() != this_vector.end()) {
         this_vector.erase(this_vector.begin());
         if (this_vector.empty()) {
           --number_of_non_empty_events_;
         }
       }
     }
   
     template <size_t ... Is>
     void
     erase_beginning_of_vectors_helper(std::index_sequence<Is...> const &)
     {
       ((erase_beginning_of_vector<Is>()), ...);
     }
   
     void erase_beginning_of_vectors()
     {
       return erase_beginning_of_vectors_helper(std::make_index_sequence<9u>());
     }
   
     template<size_t Is>
     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<Is, Events>::type;
       if constexpr (Is >= RealTypeCount::value) {
         return;
       }
       auto & this_vector = std::get<Is>(events_);
       if (this_vector.begin() == this_vector.end()) {
         return;
       }
       auto event_ts = mt::TimeStamp<typename ThisEventType::Message>::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 <size_t ... Is>
     void
     erase_old_events_if_on_sync_with_ts_helper(rclcpp::Time timestamp, std::index_sequence<Is...> const &)
     {
       ((erase_beginning_of_vector_if_on_sync_with_ts<Is>(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<M0Event>, std::vector<M1Event>, std::vector<M2Event>,
       std::vector<M3Event>, std::vector<M4Event>, std::vector<M5Event>,
       std::vector<M6Event>, std::vector<M7Event>, std::vector<M8Event>>;
     TupleOfVecOfEvents events_;
   
     std::mutex mutex_;
   };
   
   }  // namespace sync_policies
   }  // namespace message_filters
   
   #endif  // MESSAGE_FILTERS__SYNC_POLICIES__APPROXIMATE_EPSILON_TIME_H_