$search
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2009, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 #ifndef MESSAGE_FILTERS_SYNC_EXACT_TIME_H 00036 #define MESSAGE_FILTERS_SYNC_EXACT_TIME_H 00037 00038 #include "message_filters/synchronizer.h" 00039 #include "message_filters/connection.h" 00040 #include "message_filters/null_types.h" 00041 #include "message_filters/signal9.h" 00042 00043 #include <boost/tuple/tuple.hpp> 00044 #include <boost/shared_ptr.hpp> 00045 #include <boost/function.hpp> 00046 #include <boost/thread/mutex.hpp> 00047 #include <boost/signals.hpp> 00048 #include <boost/bind.hpp> 00049 #include <boost/type_traits/is_same.hpp> 00050 #include <boost/noncopyable.hpp> 00051 #include <boost/mpl/or.hpp> 00052 #include <boost/mpl/at.hpp> 00053 #include <boost/mpl/vector.hpp> 00054 00055 #include <ros/message_traits.h> 00056 #include <ros/message_event.h> 00057 00058 #include <deque> 00059 #include <vector> 00060 #include <string> 00061 00062 namespace message_filters 00063 { 00064 namespace sync_policies 00065 { 00066 00067 namespace mpl = boost::mpl; 00068 00069 00070 template<typename M0, typename M1, typename M2 = NullType, typename M3 = NullType, typename M4 = NullType, 00071 typename M5 = NullType, typename M6 = NullType, typename M7 = NullType, typename M8 = NullType> 00072 struct ExactTime : public PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8> 00073 { 00074 typedef Synchronizer<ExactTime> Sync; 00075 typedef PolicyBase<M0, M1, M2, M3, M4, M5, M6, M7, M8> Super; 00076 typedef typename Super::Messages Messages; 00077 typedef typename Super::Signal Signal; 00078 typedef typename Super::Events Events; 00079 typedef typename Super::RealTypeCount RealTypeCount; 00080 typedef typename Super::M0Event M0Event; 00081 typedef typename Super::M1Event M1Event; 00082 typedef typename Super::M2Event M2Event; 00083 typedef typename Super::M3Event M3Event; 00084 typedef typename Super::M4Event M4Event; 00085 typedef typename Super::M5Event M5Event; 00086 typedef typename Super::M6Event M6Event; 00087 typedef typename Super::M7Event M7Event; 00088 typedef typename Super::M8Event M8Event; 00089 typedef boost::tuple<M0Event, M1Event, M2Event, M3Event, M4Event, M5Event, M6Event, M7Event, M8Event> Tuple; 00090 00091 ExactTime(uint32_t queue_size) 00092 : parent_(0) 00093 , queue_size_(queue_size) 00094 { 00095 } 00096 00097 ExactTime(const ExactTime& e) 00098 { 00099 *this = e; 00100 } 00101 00102 ExactTime& operator=(const ExactTime& rhs) 00103 { 00104 parent_ = rhs.parent_; 00105 queue_size_ = rhs.queue_size_; 00106 last_signal_time_ = rhs.last_signal_time_; 00107 tuples_ = rhs.tuples_; 00108 00109 return *this; 00110 } 00111 00112 void initParent(Sync* parent) 00113 { 00114 parent_ = parent; 00115 } 00116 00117 template<int i> 00118 void add(const typename mpl::at_c<Events, i>::type& evt) 00119 { 00120 ROS_ASSERT(parent_); 00121 00122 namespace mt = ros::message_traits; 00123 00124 boost::mutex::scoped_lock lock(mutex_); 00125 00126 Tuple& t = tuples_[mt::TimeStamp<typename mpl::at_c<Messages, i>::type>::value(*evt.getMessage())]; 00127 boost::get<i>(t) = evt; 00128 00129 checkTuple(t); 00130 } 00131 00132 template<class C> 00133 Connection registerDropCallback(const C& callback) 00134 { 00135 return drop_signal_.template addCallback(callback); 00136 } 00137 00138 template<class C> 00139 Connection registerDropCallback(C& callback) 00140 { 00141 return drop_signal_.template addCallback(callback); 00142 } 00143 00144 template<class C, typename T> 00145 Connection registerDropCallback(const C& callback, T* t) 00146 { 00147 return drop_signal_.template addCallback(callback, t); 00148 } 00149 00150 template<class C, typename T> 00151 Connection registerDropCallback(C& callback, T* t) 00152 { 00153 return drop_signal_.template addCallback(callback, t); 00154 } 00155 00156 private: 00157 00158 // assumes mutex_ is already locked 00159 void checkTuple(Tuple& t) 00160 { 00161 namespace mt = ros::message_traits; 00162 00163 bool full = true; 00164 full = full && (bool)boost::get<0>(t).getMessage(); 00165 full = full && (bool)boost::get<1>(t).getMessage(); 00166 full = full && (RealTypeCount::value > 2 ? (bool)boost::get<2>(t).getMessage() : true); 00167 full = full && (RealTypeCount::value > 3 ? (bool)boost::get<3>(t).getMessage() : true); 00168 full = full && (RealTypeCount::value > 4 ? (bool)boost::get<4>(t).getMessage() : true); 00169 full = full && (RealTypeCount::value > 5 ? (bool)boost::get<5>(t).getMessage() : true); 00170 full = full && (RealTypeCount::value > 6 ? (bool)boost::get<6>(t).getMessage() : true); 00171 full = full && (RealTypeCount::value > 7 ? (bool)boost::get<7>(t).getMessage() : true); 00172 full = full && (RealTypeCount::value > 8 ? (bool)boost::get<8>(t).getMessage() : true); 00173 00174 if (full) 00175 { 00176 parent_->signal(boost::get<0>(t), boost::get<1>(t), boost::get<2>(t), 00177 boost::get<3>(t), boost::get<4>(t), boost::get<5>(t), 00178 boost::get<6>(t), boost::get<7>(t), boost::get<8>(t)); 00179 00180 last_signal_time_ = mt::TimeStamp<M0>::value(*boost::get<0>(t).getMessage()); 00181 00182 tuples_.erase(last_signal_time_); 00183 00184 clearOldTuples(); 00185 } 00186 00187 if (queue_size_ > 0) 00188 { 00189 while (tuples_.size() > queue_size_) 00190 { 00191 Tuple& t2 = tuples_.begin()->second; 00192 drop_signal_.call(boost::get<0>(t2), boost::get<1>(t2), boost::get<2>(t2), 00193 boost::get<3>(t2), boost::get<4>(t2), boost::get<5>(t2), 00194 boost::get<6>(t2), boost::get<7>(t2), boost::get<8>(t2)); 00195 tuples_.erase(tuples_.begin()); 00196 } 00197 } 00198 } 00199 00200 // assumes mutex_ is already locked 00201 void clearOldTuples() 00202 { 00203 typename M_TimeToTuple::iterator it = tuples_.begin(); 00204 typename M_TimeToTuple::iterator end = tuples_.end(); 00205 for (; it != end;) 00206 { 00207 if (it->first <= last_signal_time_) 00208 { 00209 typename M_TimeToTuple::iterator old = it; 00210 ++it; 00211 00212 Tuple& t = old->second; 00213 drop_signal_.call(boost::get<0>(t), boost::get<1>(t), boost::get<2>(t), 00214 boost::get<3>(t), boost::get<4>(t), boost::get<5>(t), 00215 boost::get<6>(t), boost::get<7>(t), boost::get<8>(t)); 00216 tuples_.erase(old); 00217 } 00218 else 00219 { 00220 // the map is sorted by time, so we can ignore anything after this if this one's time is ok 00221 break; 00222 } 00223 } 00224 } 00225 00226 private: 00227 Sync* parent_; 00228 00229 uint32_t queue_size_; 00230 typedef std::map<ros::Time, Tuple> M_TimeToTuple; 00231 M_TimeToTuple tuples_; 00232 ros::Time last_signal_time_; 00233 00234 Signal drop_signal_; 00235 00236 boost::mutex mutex_; 00237 }; 00238 00239 } // namespace sync 00240 } // namespace message_filters 00241 00242 #endif // MESSAGE_FILTERS_SYNC_EXACT_TIME_H 00243