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 {
75  typedef Synchronizer<ExactTime> Sync;
76  typedef PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8> Super;
77  typedef typename Super::Messages Messages;
78  typedef typename Super::Signal Signal;
79  typedef typename Super::Events Events;
80  typedef typename Super::RealTypeCount RealTypeCount;
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  , enable_reset_(false)
96  , last_stamp_(0)
97  {
98  }
99 
100  ExactTime(const ExactTime& e)
101  {
102  *this = e;
103  }
104 
105  ExactTime& operator=(const ExactTime& rhs)
106  {
107  parent_ = rhs.parent_;
108  queue_size_ = rhs.queue_size_;
109  enable_reset_ = rhs.enable_reset_;
110  last_signal_time_ = rhs.last_signal_time_;
111  tuples_ = rhs.tuples_;
112 
113  return *this;
114  }
115 
116  void initParent(Sync* parent)
117  {
118  parent_ = parent;
119  }
120 
121  template<int i>
122  void add(const typename mpl::at_c<Events, i>::type& evt)
123  {
125 
126  namespace mt = ros::message_traits;
127 
128  boost::mutex::scoped_lock lock(mutex_);
129 
131  {
132  ros::Time now = ros::Time::now();
133  if (now < last_stamp_)
134  {
135  ROS_WARN("Detected jump back in time. Clearing the message filters queue");
136  tuples_.clear();
137  }
138  last_stamp_ = now;
139  }
140 
141  Tuple& t = tuples_[mt::TimeStamp<typename mpl::at_c<Messages, i>::type>::value(*evt.getMessage())];
142  boost::get<i>(t) = evt;
143 
144  checkTuple(t);
145  }
146 
147  template<class C>
148  Connection registerDropCallback(const C& callback)
149  {
150  #ifndef _WIN32
151  return drop_signal_.template addCallback(callback);
152  #else
153  return drop_signal_.addCallback(callback);
154  #endif
155  }
156 
157  template<class C>
158  Connection registerDropCallback(C& callback)
159  {
160  #ifndef _WIN32
161  return drop_signal_.template addCallback(callback);
162  #else
163  return drop_signal_.addCallback(callback);
164  #endif
165  }
166 
167  template<class C, typename T>
168  Connection registerDropCallback(const C& callback, T* t)
169  {
170  #ifndef _WIN32
171  return drop_signal_.template addCallback(callback, t);
172  #else
173  return drop_signal_.addCallback(callback, t);
174  #endif
175  }
176 
177  template<class C, typename T>
178  Connection registerDropCallback(C& callback, T* t)
179  {
180  #ifndef _WIN32
181  return drop_signal_.template addCallback(callback, t);
182  #else
183  return drop_signal_.addCallback(callback, t);
184  #endif
185  }
186 
187  void setReset(const bool reset)
188  {
189  enable_reset_ = reset;
190  }
191 
192 private:
193 
194  // assumes mutex_ is already locked
195  void checkTuple(Tuple& t)
196  {
197  namespace mt = ros::message_traits;
198 
199  bool full = true;
200  full = full && (bool)boost::get<0>(t).getMessage();
201  full = full && (bool)boost::get<1>(t).getMessage();
202  full = full && (RealTypeCount::value > 2 ? (bool)boost::get<2>(t).getMessage() : true);
203  full = full && (RealTypeCount::value > 3 ? (bool)boost::get<3>(t).getMessage() : true);
204  full = full && (RealTypeCount::value > 4 ? (bool)boost::get<4>(t).getMessage() : true);
205  full = full && (RealTypeCount::value > 5 ? (bool)boost::get<5>(t).getMessage() : true);
206  full = full && (RealTypeCount::value > 6 ? (bool)boost::get<6>(t).getMessage() : true);
207  full = full && (RealTypeCount::value > 7 ? (bool)boost::get<7>(t).getMessage() : true);
208  full = full && (RealTypeCount::value > 8 ? (bool)boost::get<8>(t).getMessage() : true);
209 
210  if (full)
211  {
212  parent_->signal(boost::get<0>(t), boost::get<1>(t), boost::get<2>(t),
213  boost::get<3>(t), boost::get<4>(t), boost::get<5>(t),
214  boost::get<6>(t), boost::get<7>(t), boost::get<8>(t));
215 
216  last_signal_time_ = mt::TimeStamp<M0>::value(*boost::get<0>(t).getMessage());
217 
219 
220  clearOldTuples();
221  }
222 
223  if (queue_size_ > 0)
224  {
225  while (tuples_.size() > queue_size_)
226  {
227  Tuple& t2 = tuples_.begin()->second;
228  drop_signal_.call(boost::get<0>(t2), boost::get<1>(t2), boost::get<2>(t2),
229  boost::get<3>(t2), boost::get<4>(t2), boost::get<5>(t2),
230  boost::get<6>(t2), boost::get<7>(t2), boost::get<8>(t2));
231  tuples_.erase(tuples_.begin());
232  }
233  }
234  }
235 
236  // assumes mutex_ is already locked
237  void clearOldTuples()
238  {
239  typename M_TimeToTuple::iterator it = tuples_.begin();
240  typename M_TimeToTuple::iterator end = tuples_.end();
241  for (; it != end;)
242  {
243  if (it->first <= last_signal_time_)
244  {
245  typename M_TimeToTuple::iterator old = it;
246  ++it;
247 
248  Tuple& t = old->second;
249  drop_signal_.call(boost::get<0>(t), boost::get<1>(t), boost::get<2>(t),
250  boost::get<3>(t), boost::get<4>(t), boost::get<5>(t),
251  boost::get<6>(t), boost::get<7>(t), boost::get<8>(t));
252  tuples_.erase(old);
253  }
254  else
255  {
256  // the map is sorted by time, so we can ignore anything after this if this one's time is ok
257  break;
258  }
259  }
260  }
261 
262 private:
263  Sync* parent_;
264 
265  uint32_t queue_size_;
266  bool enable_reset_;
267  typedef std::map<ros::Time, Tuple> M_TimeToTuple;
271 
273 
274  boost::mutex mutex_;
275 };
276 
277 } // namespace sync
278 } // namespace message_filters
279 
280 #endif // MESSAGE_FILTERS_SYNC_EXACT_TIME_H
281 
message_filters::Signal9::addCallback
Connection addCallback(const boost::function< void(P0, P1, P2, P3, P4, P5, P6, P7, P8)> &callback)
Definition: signal9.h:203
message_filters::sync_policies::ExactTime::queue_size_
uint32_t queue_size_
Definition: exact_time.h:361
message_filters::sync_policies::ExactTime::last_stamp_
ros::Time last_stamp_
Definition: exact_time.h:366
message_filters::Connection
Encapsulates a connection from one filter to another (or to a user-specified callback)
Definition: connection.h:80
ros::Time::isSimTime
static bool isSimTime()
message_filters::sync_policies::ExactTime::initParent
void initParent(Sync *parent)
Definition: exact_time.h:212
message_filters::sync_policies::ExactTime::M_TimeToTuple
std::map< ros::Time, Tuple > M_TimeToTuple
Definition: exact_time.h:363
message_filters::PolicyBase::M7Event
mpl::at_c< Events, 7 >::type M7Event
Definition: synchronizer.h:419
message_filters::sync_policies::ExactTime::registerDropCallback
Connection registerDropCallback(const C &callback)
Definition: exact_time.h:244
message_filters::sync_policies::ExactTime::add
void add(const typename mpl::at_c< Events, i >::type &evt)
Definition: exact_time.h:218
message_filters::sync_policies::ExactTime::checkTuple
void checkTuple(Tuple &t)
Definition: exact_time.h:291
synchronizer.h
message_filters::sync_policies::ExactTime::RealTypeCount
Super::RealTypeCount RealTypeCount
Definition: exact_time.h:176
message_filters::sync_policies::ExactTime::mutex_
boost::mutex mutex_
Definition: exact_time.h:370
message_filters::sync_policies::ExactTime::parent_
Sync * parent_
Definition: exact_time.h:359
message_filters::PolicyBase::M8Event
mpl::at_c< Events, 8 >::type M8Event
Definition: synchronizer.h:420
message_filters::sync_policies::ExactTime::M3Event
Super::M3Event M3Event
Definition: exact_time.h:180
message_filters::sync_policies::ExactTime::operator=
ExactTime & operator=(const ExactTime &rhs)
Definition: exact_time.h:201
message_filters::PolicyBase::M2Event
mpl::at_c< Events, 2 >::type M2Event
Definition: synchronizer.h:414
message_filters::sync_policies::ExactTime::Tuple
boost::tuple< M0Event, M1Event, M2Event, M3Event, M4Event, M5Event, M6Event, M7Event, M8Event > Tuple
Definition: exact_time.h:186
message_filters::sync_policies::ExactTime::clearOldTuples
void clearOldTuples()
Definition: exact_time.h:333
message_filters::PolicyBase::RealTypeCount
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:411
message_filters::sync_policies::ExactTime::M0Event
Super::M0Event M0Event
Definition: exact_time.h:177
message_filters::PolicyBase::Events
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:410
message_filters::sync_policies::ExactTime::M4Event
Super::M4Event M4Event
Definition: exact_time.h:181
ros::message_traits
message_filters::PolicyBase::M1Event
mpl::at_c< Events, 1 >::type M1Event
Definition: synchronizer.h:413
signal9.h
message_filters::sync_policies::ExactTime::M2Event
Super::M2Event M2Event
Definition: exact_time.h:179
message_filters::PolicyBase::M3Event
mpl::at_c< Events, 3 >::type M3Event
Definition: synchronizer.h:415
message_filters::sync_policies::ExactTime::drop_signal_
Signal drop_signal_
Definition: exact_time.h:368
message_filters::Signal9
Definition: signal9.h:176
message_filters::sync_policies::ExactTime::Super
PolicyBase< M0, M1, M2, M3, M4, M5, M6, M7, M8 > Super
Definition: exact_time.h:172
message_traits.h
message_filters::sync_policies::ExactTime::Messages
Super::Messages Messages
Definition: exact_time.h:173
ROS_WARN
#define ROS_WARN(...)
message_filters::Signal9::call
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:326
message_filters::PolicyBase::M5Event
mpl::at_c< Events, 5 >::type M5Event
Definition: synchronizer.h:417
message_filters::sync_policies::ExactTime::Signal
Super::Signal Signal
Definition: exact_time.h:174
connection.h
message_filters::sync_policies::ExactTime::M8Event
Super::M8Event M8Event
Definition: exact_time.h:185
ros::Time
null_types.h
message_filters::PolicyBase::Messages
mpl::vector< M0, M1, M2, M3, M4, M5, M6, M7, M8 > Messages
Definition: synchronizer.h:406
message_filters::sync_policies::ExactTime::last_signal_time_
ros::Time last_signal_time_
Definition: exact_time.h:365
message_filters::PolicyBase::Signal
Signal9< M0, M1, M2, M3, M4, M5, M6, M7, M8 > Signal
Definition: synchronizer.h:407
message_filters::sync_policies::ExactTime::M1Event
Super::M1Event M1Event
Definition: exact_time.h:178
message_filters::sync_policies::ExactTime::Sync
Synchronizer< ExactTime > Sync
Definition: exact_time.h:171
message_filters::sync_policies::ExactTime::Events
Super::Events Events
Definition: exact_time.h:175
message_filters::sync_policies::ExactTime::setReset
void setReset(const bool reset)
Definition: exact_time.h:283
message_filters::sync_policies::ExactTime::M5Event
Super::M5Event M5Event
Definition: exact_time.h:182
message_filters
Definition: cache.h:47
message_filters::PolicyBase::M0Event
mpl::at_c< Events, 0 >::type M0Event
Definition: synchronizer.h:412
message_filters::sync_policies::ExactTime::ExactTime
ExactTime(uint32_t queue_size)
Definition: exact_time.h:188
assert.h
message_filters::Synchronizer::signal
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:393
ROS_ASSERT
#define ROS_ASSERT(cond)
message_filters::sync_policies::ExactTime::tuples_
M_TimeToTuple tuples_
Definition: exact_time.h:364
message_event.h
message_filters::PolicyBase::M4Event
mpl::at_c< Events, 4 >::type M4Event
Definition: synchronizer.h:416
message_filters::sync_policies::ExactTime::M6Event
Super::M6Event M6Event
Definition: exact_time.h:183
message_filters::sync_policies::ExactTime::enable_reset_
bool enable_reset_
Definition: exact_time.h:362
message_filters::PolicyBase::M6Event
mpl::at_c< Events, 6 >::type M6Event
Definition: synchronizer.h:418
message_filters::sync_policies::ExactTime::M7Event
Super::M7Event M7Event
Definition: exact_time.h:184
ros::Time::now
static Time now()


message_filters
Author(s): Josh Faust, Vijay Pradeep, Dirk Thomas
autogenerated on Sat Oct 17 2020 19:28:51