35 #ifndef __PCL_SYNCHRONIZER__
36 #define __PCL_SYNCHRONIZER__
48 template <
typename T1,
typename T2>
51 typedef std::pair<unsigned long, T1> T1Stamped;
52 typedef std::pair<unsigned long, T2> T2Stamped;
55 boost::mutex publish_mutex_;
56 std::deque<T1Stamped> queueT1;
57 std::deque<T2Stamped> queueT2;
59 typedef boost::function<void(T1, T2, unsigned long, unsigned long) > CallbackFunction;
61 std::map<int, CallbackFunction> cb_;
65 Synchronizer () : mutex1_ (), mutex2_ (), publish_mutex_ (), queueT1 (), queueT2 (), cb_ (), callback_counter (0) { };
70 boost::unique_lock<boost::mutex> publish_lock (publish_mutex_);
71 cb_[callback_counter] = callback;
72 return callback_counter++;
78 boost::unique_lock<boost::mutex> publish_lock (publish_mutex_);
83 add0 (
const T1& t,
unsigned long time)
86 queueT1.push_back (T1Stamped (time, t));
92 add1 (
const T2& t,
unsigned long time)
95 queueT2.push_back (T2Stamped (time, t));
105 boost::unique_lock<boost::mutex> lock1 (mutex1_);
106 boost::unique_lock<boost::mutex> lock2 (mutex2_);
108 for (
typename std::map<int, CallbackFunction>::iterator cb = cb_.begin (); cb != cb_.end (); ++cb)
110 if (!cb->second.empty ())
112 cb->second.operator()(queueT1.front ().second, queueT2.front ().second, queueT1.front ().first, queueT2.front ().first);
116 queueT1.pop_front ();
117 queueT2.pop_front ();
124 boost::unique_lock<boost::mutex> publish_lock (publish_mutex_);
126 boost::unique_lock<boost::mutex> lock1 (mutex1_);
127 if (queueT1.empty ())
129 T1Stamped t1 = queueT1.front ();
132 boost::unique_lock<boost::mutex> lock2 (mutex2_);
133 if (queueT2.empty ())
135 T2Stamped t2 = queueT2.front ();
138 bool do_publish =
false;
140 if (t1.first <= t2.first)
143 while (queueT1.size () > 1 && queueT1[1].first <= t2.first)
144 queueT1.pop_front ();
146 if (queueT1.size () > 1)
148 if ( (t2.first << 1) > (queueT1[0].first + queueT1[1].first) )
149 queueT1.pop_front ();
158 while (queueT2.size () > 1 && (queueT2[1].first <= t1.first) )
159 queueT2.pop_front ();
161 if (queueT2.size () > 1)
163 if ( (t1.first << 1) > queueT2[0].first + queueT2[1].first )
164 queueT2.pop_front ();
177 #endif // __PCL_SYNCHRONIZER__
void removeCallback(int i)
int addCallback(const CallbackFunction &callback)
void add1(const T2 &t, unsigned long time)
void add0(const T1 &t, unsigned long time)
/brief This template class synchronizes two data streams of different types.