Program Listing for File exact_time.h

↰ Return to documentation for file (/tmp/ws/src/message_filters/include/message_filters/sync_policies/exact_time.h)

/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 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 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__EXACT_TIME_H_
#define MESSAGE_FILTERS__SYNC_POLICIES__EXACT_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>
struct ExactTime : public PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8>
{
  typedef Synchronizer<ExactTime> 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;

  ExactTime(uint32_t queue_size)
  : 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<int i>
  void add(const typename std::tuple_element<i, Events>::type& evt)
  {
    RCUTILS_ASSERT(parent_);

    namespace mt = message_filters::message_traits;

    std::lock_guard<std::mutex> lock(mutex_);

    Tuple& t = tuples_[mt::TimeStamp<typename std::tuple_element<i, Messages>::type>::value(*evt.getMessage())];
    std::get<i>(t) = evt;

    checkTuple(t);
  }

  template<class C>
  Connection registerDropCallback(const C& callback)
  {
    return drop_signal_.addCallback(callback);
  }

  template<class C>
  Connection registerDropCallback(C& callback)
  {
    return drop_signal_.addCallback(callback);
  }

  template<class C, typename T>
  Connection registerDropCallback(const C& callback, T* t)
  {
    return drop_signal_.addCallback(callback, t);
  }

  template<class C, typename T>
  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<M0>::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<rclcpp::Time, Tuple> 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_H_