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