21 #ifndef INCLUDE_FKIE_MESSAGE_FILTERS_COMBINER_POLICIES_EXACT_TIME_IMPL_H_
22 #define INCLUDE_FKIE_MESSAGE_FILTERS_COMBINER_POLICIES_EXACT_TIME_IMPL_H_
25 #include "../helpers/access_ros_header.h"
26 #include "../helpers/tuple.h"
27 #include "../helpers/scoped_unlock.h"
32 namespace combiner_policies
35 template<
typename... IOs>
37 : max_age_(
ros::Duration(1, 0)), max_queue_size_(0)
41 template<
typename... IOs>
42 ExactTime<IOs...>& ExactTime<IOs...>::set_max_age (
const ros::Duration& max_age) noexcept
49 template<
typename... IOs>
50 ExactTime<IOs...>& ExactTime<IOs...>::set_max_queue_size (std::size_t queue_size,
const boost::optional<ros::Duration>& max_age) noexcept
52 max_queue_size_ = queue_size;
57 template<
typename... IOs>
58 template<std::
size_t N>
59 void ExactTime<IOs...>::add(std::unique_lock<std::mutex>& lock,
const std::tuple_element_t<N, IncomingTuples>& in)
62 if (!std::get<N>(queues_).emplace(stamp, in).second)
64 ROS_WARN_STREAM_NAMED(
"Combiner<ExactTime>",
"message with repeating time stamp " << stamp <<
" is dropped");
68 MaybeOutgoingTuples out = try_assemble_output(stamp, complete);
69 if (max_age_ || complete)
71 ros::Time cutoff = complete ? stamp : stamp - *max_age_;
73 [
this, &cutoff](
auto I)
75 auto& queue = std::get<I>(this->queues_);
76 auto ub = queue.upper_bound(cutoff);
77 queue.erase(queue.begin(), ub);
81 if (max_queue_size_ > 0)
83 auto& queue = std::get<N>(queues_);
84 if (queue.size() > max_queue_size_) queue.erase(queue.begin());
89 [
this, &out, &lock](
auto... Is)
92 this->emit(std::tuple_cat(*std::get<Is>(out)...));
98 template<
typename... IOs>
99 typename ExactTime<IOs...>::MaybeOutgoingTuples ExactTime<IOs...>::try_assemble_output(
const ros::Time& time,
bool& complete) noexcept
102 MaybeOutgoingTuples result;
108 auto& queue = std::get<I>(this->queues_);
109 auto it = queue.find(time);
110 if (it != queue.end())
112 std::get<I>(result) = it->second;
124 template<
typename... IOs>
125 void ExactTime<IOs...>::reset() noexcept
130 auto& queue = std::get<I>(this->queues_);