approximate_time.h

Go to the documentation of this file.

/****************************************************************************
 *
 * fkie_message_filters
 * Copyright © 2018-2020 Fraunhofer FKIE
 * Author: Timo Röhling
 *
 * 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 INCLUDE_FKIE_MESSAGE_FILTERS_COMBINER_POLICIES_APPROXIMATE_TIME_H_
#define INCLUDE_FKIE_MESSAGE_FILTERS_COMBINER_POLICIES_APPROXIMATE_TIME_H_
 
#include "policy_base.h"
#include <deque>
#include <mutex>
#include <ros/duration.h>
 
namespace fkie_message_filters
{
namespace combiner_policies
{
 
template<typename... IOs>
class ApproximateTime : public PolicyBase<IOs...>
{
public:
    template<template<typename...> class, class...> friend class fkie_message_filters::Combiner;
    using typename PolicyBase<IOs...>::EmitterCB;
    using typename PolicyBase<IOs...>::IncomingTuples;
    using typename PolicyBase<IOs...>::OutgoingTuple;
    ApproximateTime();
    ApproximateTime& set_max_age(const ros::Duration& max_age) noexcept;
    ApproximateTime& set_max_queue_size (std::size_t queue_size, const boost::optional<ros::Duration>& max_age = boost::none) noexcept;
    ApproximateTime& set_max_timespan (const ros::Duration& max_delta) noexcept;
    ApproximateTime& set_min_distance(std::size_t i, const ros::Duration& min_dist) noexcept;
protected:
    template<std::size_t N>
    void add(std::unique_lock<std::mutex>&, const std::tuple_element_t<N, IncomingTuples>&);
    void reset() noexcept override;
private:
    static constexpr std::size_t NUM_SLOTS = sizeof...(IOs);
    static constexpr std::size_t UNSET = NUM_SLOTS;
    using typename PolicyBase<IOs...>::MaybeOutgoingTuples;
    using IncomingQueues = std::tuple<std::deque<helpers::io_tuple_t<IOs>>...>;
    template<std::size_t N>
    void discard_expired_at(const ros::Time& cutoff) noexcept;
    void discard_expired(const ros::Time& cutoff) noexcept;
    template<std::size_t N>
    void prune_queue_at(std::size_t queue_size) noexcept;
    template<std::size_t N>
    bool can_still_improve_at() noexcept;
    bool can_still_improve() noexcept;
    ros::Duration pivot_timedelta(const ros::Time& ts) noexcept;
    ros::Duration heads_timespan() noexcept;
    bool determine_pivot() noexcept;
    void drop_pivot() noexcept;
    void emit_heads(std::unique_lock<std::mutex>&);
    boost::optional<ros::Duration> max_age_;
    std::size_t max_queue_size_;
    boost::optional<ros::Duration> max_delta_;
    std::size_t pivot_;
    ros::Time pivot_ts_;
    std::array<ros::Duration, NUM_SLOTS> min_dist_;
    IncomingQueues queues_;
    MaybeOutgoingTuples heads_;
};
 
} // namespace combiner_policies
} // namespace fkie_message_filters
 
#include "approximate_time_impl.h"
 
#endif /* INCLUDE_FKIE_MESSAGE_FILTERS_COMBINER_POLICIES_APPROXIMATE_TIME_H_ */