exact_time.h
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2009, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 #ifndef MESSAGE_FILTERS_SYNC_EXACT_TIME_H
36 #define MESSAGE_FILTERS_SYNC_EXACT_TIME_H
37 
42 
43 #include <boost/tuple/tuple.hpp>
44 #include <boost/shared_ptr.hpp>
45 #include <boost/function.hpp>
46 #include <boost/thread/mutex.hpp>
47 
48 #include <boost/bind.hpp>
49 #include <boost/type_traits/is_same.hpp>
50 #include <boost/noncopyable.hpp>
51 #include <boost/mpl/or.hpp>
52 #include <boost/mpl/at.hpp>
53 #include <boost/mpl/vector.hpp>
54 
55 #include <ros/assert.h>
56 #include <ros/message_traits.h>
57 #include <ros/message_event.h>
58 
59 #include <deque>
60 #include <vector>
61 #include <string>
62 
63 namespace message_filters
64 {
65 namespace sync_policies
66 {
67 
68 namespace mpl = boost::mpl;
69 
70 
71 template<typename M0, typename M1, typename M2 = NullType, typename M3 = NullType, typename M4 = NullType,
72  typename M5 = NullType, typename M6 = NullType, typename M7 = NullType, typename M8 = NullType>
73 struct ExactTime : public PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8>
74 {
77  typedef typename Super::Messages Messages;
78  typedef typename Super::Signal Signal;
79  typedef typename Super::Events Events;
81  typedef typename Super::M0Event M0Event;
82  typedef typename Super::M1Event M1Event;
83  typedef typename Super::M2Event M2Event;
84  typedef typename Super::M3Event M3Event;
85  typedef typename Super::M4Event M4Event;
86  typedef typename Super::M5Event M5Event;
87  typedef typename Super::M6Event M6Event;
88  typedef typename Super::M7Event M7Event;
89  typedef typename Super::M8Event M8Event;
90  typedef boost::tuple<M0Event, M1Event, M2Event, M3Event, M4Event, M5Event, M6Event, M7Event, M8Event> Tuple;
91 
92  ExactTime(uint32_t queue_size)
93  : parent_(0)
94  , queue_size_(queue_size)
95  {
96  }
97 
98  ExactTime(const ExactTime& e)
99  {
100  *this = e;
101  }
102 
104  {
105  parent_ = rhs.parent_;
106  queue_size_ = rhs.queue_size_;
108  tuples_ = rhs.tuples_;
109 
110  return *this;
111  }
112 
113  void initParent(Sync* parent)
114  {
115  parent_ = parent;
116  }
117 
118  template<int i>
119  void add(const typename mpl::at_c<Events, i>::type& evt)
120  {
122 
123  namespace mt = ros::message_traits;
124 
125  boost::mutex::scoped_lock lock(mutex_);
126 
127  Tuple& t = tuples_[mt::TimeStamp<typename mpl::at_c<Messages, i>::type>::value(*evt.getMessage())];
128  boost::get<i>(t) = evt;
129 
130  checkTuple(t);
131  }
132 
133  template<class C>
134  Connection registerDropCallback(const C& callback)
135  {
136  #ifndef _WIN32
137  return drop_signal_.template addCallback(callback);
138  #else
139  return drop_signal_.addCallback(callback);
140  #endif
141  }
142 
143  template<class C>
145  {
146  #ifndef _WIN32
147  return drop_signal_.template addCallback(callback);
148  #else
149  return drop_signal_.addCallback(callback);
150  #endif
151  }
152 
153  template<class C, typename T>
154  Connection registerDropCallback(const C& callback, T* t)
155  {
156  #ifndef _WIN32
157  return drop_signal_.template addCallback(callback, t);
158  #else
159  return drop_signal_.addCallback(callback, t);
160  #endif
161  }
162 
163  template<class C, typename T>
164  Connection registerDropCallback(C& callback, T* t)
165  {
166  #ifndef _WIN32
167  return drop_signal_.template addCallback(callback, t);
168  #else
169  return drop_signal_.addCallback(callback, t);
170  #endif
171  }
172 
173 private:
174 
175  // assumes mutex_ is already locked
176  void checkTuple(Tuple& t)
177  {
178  namespace mt = ros::message_traits;
179 
180  bool full = true;
181  full = full && (bool)boost::get<0>(t).getMessage();
182  full = full && (bool)boost::get<1>(t).getMessage();
183  full = full && (RealTypeCount::value > 2 ? (bool)boost::get<2>(t).getMessage() : true);
184  full = full && (RealTypeCount::value > 3 ? (bool)boost::get<3>(t).getMessage() : true);
185  full = full && (RealTypeCount::value > 4 ? (bool)boost::get<4>(t).getMessage() : true);
186  full = full && (RealTypeCount::value > 5 ? (bool)boost::get<5>(t).getMessage() : true);
187  full = full && (RealTypeCount::value > 6 ? (bool)boost::get<6>(t).getMessage() : true);
188  full = full && (RealTypeCount::value > 7 ? (bool)boost::get<7>(t).getMessage() : true);
189  full = full && (RealTypeCount::value > 8 ? (bool)boost::get<8>(t).getMessage() : true);
190 
191  if (full)
192  {
193  parent_->signal(boost::get<0>(t), boost::get<1>(t), boost::get<2>(t),
194  boost::get<3>(t), boost::get<4>(t), boost::get<5>(t),
195  boost::get<6>(t), boost::get<7>(t), boost::get<8>(t));
196 
197  last_signal_time_ = mt::TimeStamp<M0>::value(*boost::get<0>(t).getMessage());
198 
199  tuples_.erase(last_signal_time_);
200 
201  clearOldTuples();
202  }
203 
204  if (queue_size_ > 0)
205  {
206  while (tuples_.size() > queue_size_)
207  {
208  Tuple& t2 = tuples_.begin()->second;
209  drop_signal_.call(boost::get<0>(t2), boost::get<1>(t2), boost::get<2>(t2),
210  boost::get<3>(t2), boost::get<4>(t2), boost::get<5>(t2),
211  boost::get<6>(t2), boost::get<7>(t2), boost::get<8>(t2));
212  tuples_.erase(tuples_.begin());
213  }
214  }
215  }
216 
217  // assumes mutex_ is already locked
219  {
220  typename M_TimeToTuple::iterator it = tuples_.begin();
221  typename M_TimeToTuple::iterator end = tuples_.end();
222  for (; it != end;)
223  {
224  if (it->first <= last_signal_time_)
225  {
226  typename M_TimeToTuple::iterator old = it;
227  ++it;
228 
229  Tuple& t = old->second;
230  drop_signal_.call(boost::get<0>(t), boost::get<1>(t), boost::get<2>(t),
231  boost::get<3>(t), boost::get<4>(t), boost::get<5>(t),
232  boost::get<6>(t), boost::get<7>(t), boost::get<8>(t));
233  tuples_.erase(old);
234  }
235  else
236  {
237  // the map is sorted by time, so we can ignore anything after this if this one's time is ok
238  break;
239  }
240  }
241  }
242 
243 private:
244  Sync* parent_;
245 
246  uint32_t queue_size_;
247  typedef std::map<ros::Time, Tuple> M_TimeToTuple;
248  M_TimeToTuple tuples_;
250 
251  Signal drop_signal_;
252 
253  boost::mutex mutex_;
254 };
255 
256 } // namespace sync
257 } // namespace message_filters
258 
259 #endif // MESSAGE_FILTERS_SYNC_EXACT_TIME_H
260 
mpl::vector< ros::MessageEvent< M0 const >, ros::MessageEvent< M1 const >, ros::MessageEvent< M2 const >, ros::MessageEvent< M3 const >, ros::MessageEvent< M4 const >, ros::MessageEvent< M5 const >, ros::MessageEvent< M6 const >, ros::MessageEvent< M7 const >, ros::MessageEvent< M8 const > > Events
Definition: synchronizer.h:378
Connection registerDropCallback(C &callback, T *t)
Definition: exact_time.h:164
std::map< ros::Time, Tuple > M_TimeToTuple
Definition: exact_time.h:247
Synchronizer< ExactTime > Sync
Definition: exact_time.h:75
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)
Definition: synchronizer.h:329
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)
Definition: signal9.h:294
mpl::at_c< Events, 1 >::type M1Event
Definition: synchronizer.h:381
mpl::fold< Messages, mpl::int_< 0 >, mpl::if_< mpl::not_< boost::is_same< mpl::_2, NullType > >, mpl::next< mpl::_1 >, mpl::_1 > >::type RealTypeCount
Definition: synchronizer.h:379
Connection registerDropCallback(const C &callback)
Definition: exact_time.h:134
mpl::at_c< Events, 5 >::type M5Event
Definition: synchronizer.h:385
mpl::at_c< Events, 2 >::type M2Event
Definition: synchronizer.h:382
mpl::vector< M0, M1, M2, M3, M4, M5, M6, M7, M8 > Messages
Definition: synchronizer.h:374
PolicyBase< M0, M1, M2, M3, M4, M5, M6, M7, M8 > Super
Definition: exact_time.h:76
mpl::at_c< Events, 0 >::type M0Event
Definition: synchronizer.h:380
mpl::at_c< Events, 3 >::type M3Event
Definition: synchronizer.h:383
Connection registerDropCallback(C &callback)
Definition: exact_time.h:144
ExactTime & operator=(const ExactTime &rhs)
Definition: exact_time.h:103
Connection addCallback(const boost::function< void(P0, P1, P2, P3, P4, P5, P6, P7, P8)> &callback)
Definition: signal9.h:171
#define ROS_ASSERT(cond)
Connection registerDropCallback(const C &callback, T *t)
Definition: exact_time.h:154
Encapsulates a connection from one filter to another (or to a user-specified callback) ...
Definition: connection.h:48
void add(const typename mpl::at_c< Events, i >::type &evt)
Definition: exact_time.h:119
mpl::at_c< Events, 7 >::type M7Event
Definition: synchronizer.h:387
mpl::at_c< Events, 4 >::type M4Event
Definition: synchronizer.h:384
mpl::at_c< Events, 8 >::type M8Event
Definition: synchronizer.h:388
boost::tuple< M0Event, M1Event, M2Event, M3Event, M4Event, M5Event, M6Event, M7Event, M8Event > Tuple
Definition: exact_time.h:90
mpl::at_c< Events, 6 >::type M6Event
Definition: synchronizer.h:386


message_filters
Author(s): Josh Faust, Vijay Pradeep
autogenerated on Sat Feb 22 2020 03:23:02