Point Cloud Library (PCL)  1.8.0
synchronizer.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, 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 copyright holder(s) 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  * Author: Nico Blodow (blodow@in.tum.de), Suat Gedikli (gedikli@willowgarage.com)
34  */
35 #ifndef __PCL_SYNCHRONIZER__
36 #define __PCL_SYNCHRONIZER__
37 
38 namespace pcl
39 {
40  /** /brief This template class synchronizes two data streams of different types.
41  * The data can be added using add0 and add1 methods which expects also a timestamp of type unsigned long.
42  * If two matching data objects are found, registered callback functions are invoked with the objects and the time stamps.
43  * The only assumption of the timestamp is, that they are in the same unit, linear and strictly monotonic increasing.
44  * If filtering is desired, e.g. thresholding of time differences, the user can do that in the callback method.
45  * This class is thread safe.
46  * /ingroup common
47  */
48  template <typename T1, typename T2>
50  {
51  typedef std::pair<unsigned long, T1> T1Stamped;
52  typedef std::pair<unsigned long, T2> T2Stamped;
53  boost::mutex mutex1_;
54  boost::mutex mutex2_;
55  boost::mutex publish_mutex_;
56  std::deque<T1Stamped> queueT1;
57  std::deque<T2Stamped> queueT2;
58 
59  typedef boost::function<void(T1, T2, unsigned long, unsigned long) > CallbackFunction;
60 
61  std::map<int, CallbackFunction> cb_;
62  int callback_counter;
63  public:
64 
65  Synchronizer () : mutex1_ (), mutex2_ (), publish_mutex_ (), queueT1 (), queueT2 (), cb_ (), callback_counter (0) { };
66 
67  int
68  addCallback (const CallbackFunction& callback)
69  {
70  boost::unique_lock<boost::mutex> publish_lock (publish_mutex_);
71  cb_[callback_counter] = callback;
72  return callback_counter++;
73  }
74 
75  void
77  {
78  boost::unique_lock<boost::mutex> publish_lock (publish_mutex_);
79  cb_.erase (i);
80  }
81 
82  void
83  add0 (const T1& t, unsigned long time)
84  {
85  mutex1_.lock ();
86  queueT1.push_back (T1Stamped (time, t));
87  mutex1_.unlock ();
88  publish ();
89  }
90 
91  void
92  add1 (const T2& t, unsigned long time)
93  {
94  mutex2_.lock ();
95  queueT2.push_back (T2Stamped (time, t));
96  mutex2_.unlock ();
97  publish ();
98  }
99 
100  private:
101 
102  void
103  publishData ()
104  {
105  boost::unique_lock<boost::mutex> lock1 (mutex1_);
106  boost::unique_lock<boost::mutex> lock2 (mutex2_);
107 
108  for (typename std::map<int, CallbackFunction>::iterator cb = cb_.begin (); cb != cb_.end (); ++cb)
109  {
110  if (!cb->second.empty ())
111  {
112  cb->second.operator()(queueT1.front ().second, queueT2.front ().second, queueT1.front ().first, queueT2.front ().first);
113  }
114  }
115 
116  queueT1.pop_front ();
117  queueT2.pop_front ();
118  }
119 
120  void
121  publish ()
122  {
123  // only one publish call at once allowed
124  boost::unique_lock<boost::mutex> publish_lock (publish_mutex_);
125 
126  boost::unique_lock<boost::mutex> lock1 (mutex1_);
127  if (queueT1.empty ())
128  return;
129  T1Stamped t1 = queueT1.front ();
130  lock1.unlock ();
131 
132  boost::unique_lock<boost::mutex> lock2 (mutex2_);
133  if (queueT2.empty ())
134  return;
135  T2Stamped t2 = queueT2.front ();
136  lock2.unlock ();
137 
138  bool do_publish = false;
139 
140  if (t1.first <= t2.first)
141  { // iterate over queue1
142  lock1.lock ();
143  while (queueT1.size () > 1 && queueT1[1].first <= t2.first)
144  queueT1.pop_front ();
145 
146  if (queueT1.size () > 1)
147  { // we have at least 2 measurements; first in past and second in future -> find out closer one!
148  if ( (t2.first << 1) > (queueT1[0].first + queueT1[1].first) )
149  queueT1.pop_front ();
150 
151  do_publish = true;
152  }
153  lock1.unlock ();
154  }
155  else
156  { // iterate over queue2
157  lock2.lock ();
158  while (queueT2.size () > 1 && (queueT2[1].first <= t1.first) )
159  queueT2.pop_front ();
160 
161  if (queueT2.size () > 1)
162  { // we have at least 2 measurements; first in past and second in future -> find out closer one!
163  if ( (t1.first << 1) > queueT2[0].first + queueT2[1].first )
164  queueT2.pop_front ();
165 
166  do_publish = true;
167  }
168  lock2.unlock ();
169  }
170 
171  if (do_publish)
172  publishData ();
173  }
174  } ;
175 } // namespace
176 
177 #endif // __PCL_SYNCHRONIZER__
178 
void add1(const T2 &t, unsigned long time)
Definition: synchronizer.h:92
/brief This template class synchronizes two data streams of different types.
Definition: synchronizer.h:49
void removeCallback(int i)
Definition: synchronizer.h:76
void add0(const T1 &t, unsigned long time)
Definition: synchronizer.h:83
int addCallback(const CallbackFunction &callback)
Definition: synchronizer.h:68