$search
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2008, 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_CACHE_H_ 00036 #define MESSAGE_FILTERS_CACHE_H_ 00037 00038 #include <deque> 00039 #include "boost/thread.hpp" 00040 #include "boost/shared_ptr.hpp" 00041 #include <boost/signals.hpp> 00042 #include "ros/time.h" 00043 00044 #include "connection.h" 00045 #include "simple_filter.h" 00046 00047 namespace message_filters 00048 { 00049 00065 template<class M> 00066 class Cache : public SimpleFilter<M> 00067 { 00068 public: 00069 typedef boost::shared_ptr<M const> MConstPtr; 00070 typedef ros::MessageEvent<M const> EventType; 00071 00072 template<class F> 00073 Cache(F& f, unsigned int cache_size = 1) 00074 { 00075 setCacheSize(cache_size) ; 00076 connectInput(f) ; 00077 } 00078 00084 Cache(unsigned int cache_size = 1) 00085 { 00086 setCacheSize(cache_size); 00087 } 00088 00089 template<class F> 00090 void connectInput(F& f) 00091 { 00092 incoming_connection_ = f.registerCallback(typename SimpleFilter<M>::EventCallback(boost::bind(&Cache::callback, this, _1))); 00093 } 00094 00095 ~Cache() 00096 { 00097 incoming_connection_.disconnect(); 00098 } 00099 00104 void setCacheSize(unsigned int cache_size) 00105 { 00106 if (cache_size == 0) 00107 { 00108 //ROS_ERROR("Cannot set max_size to 0") ; 00109 return ; 00110 } 00111 00112 cache_size_ = cache_size ; 00113 } 00114 00119 void add(const MConstPtr& msg) 00120 { 00121 add(EventType(msg)); 00122 } 00123 00128 void add(const EventType& evt) 00129 { 00130 namespace mt = ros::message_traits; 00131 00132 //printf(" Cache Size: %u\n", cache_.size()) ; 00133 { 00134 boost::mutex::scoped_lock lock(cache_lock_); 00135 00136 while (cache_.size() >= cache_size_) // Keep popping off old data until we have space for a new msg 00137 cache_.pop_front() ; // The front of the deque has the oldest elem, so we can get rid of it 00138 00139 // No longer naively pushing msgs to back. Want to make sure they're sorted correctly 00140 //cache_.push_back(msg) ; // Add the newest message to the back of the deque 00141 00142 typename std::deque<EventType >::reverse_iterator rev_it = cache_.rbegin(); 00143 00144 // Keep walking backwards along deque until we hit the beginning, 00145 // or until we find a timestamp that's smaller than (or equal to) msg's timestamp 00146 ros::Time evt_stamp = mt::TimeStamp<M>::value(*evt.getMessage()); 00147 while(rev_it != cache_.rend() && mt::TimeStamp<M>::value(*(*rev_it).getMessage()) > evt_stamp) 00148 rev_it++; 00149 00150 // Add msg to the cache 00151 cache_.insert(rev_it.base(), evt); 00152 00153 } 00154 00155 this->signalMessage(evt); 00156 } 00157 00166 std::vector<MConstPtr> getInterval(const ros::Time& start, const ros::Time& end) const 00167 { 00168 namespace mt = ros::message_traits; 00169 00170 boost::mutex::scoped_lock lock(cache_lock_); 00171 00172 // Find the starting index. (Find the first index after [or at] the start of the interval) 00173 unsigned int start_index = 0 ; 00174 while(start_index < cache_.size() && 00175 mt::TimeStamp<M>::value(*cache_[start_index].getMessage()) < start) 00176 { 00177 start_index++ ; 00178 } 00179 00180 // Find the ending index. (Find the first index after the end of interval) 00181 unsigned int end_index = start_index ; 00182 while(end_index < cache_.size() && 00183 mt::TimeStamp<M>::value(*cache_[end_index].getMessage()) <= end) 00184 { 00185 end_index++ ; 00186 } 00187 00188 std::vector<MConstPtr> interval_elems ; 00189 interval_elems.reserve(end_index - start_index) ; 00190 for (unsigned int i=start_index; i<end_index; i++) 00191 { 00192 interval_elems.push_back(cache_[i].getMessage()) ; 00193 } 00194 00195 return interval_elems ; 00196 } 00197 00198 00205 std::vector<MConstPtr> getSurroundingInterval(const ros::Time& start, const ros::Time& end) const 00206 { 00207 namespace mt = ros::message_traits; 00208 00209 boost::mutex::scoped_lock lock(cache_lock_); 00210 // Find the starting index. (Find the first index after [or at] the start of the interval) 00211 unsigned int start_index = cache_.size()-1; 00212 while(start_index > 0 && 00213 mt::TimeStamp<M>::value(*cache_[start_index].getMessage()) > start) 00214 { 00215 start_index--; 00216 } 00217 unsigned int end_index = start_index; 00218 while(end_index < cache_.size()-1 && 00219 mt::TimeStamp<M>::value(*cache_[end_index].getMessage()) < end) 00220 { 00221 end_index++; 00222 } 00223 00224 std::vector<MConstPtr> interval_elems; 00225 interval_elems.reserve(end_index - start_index + 1) ; 00226 for (unsigned int i=start_index; i<=end_index; i++) 00227 { 00228 interval_elems.push_back(cache_[i].getMessage()) ; 00229 } 00230 00231 return interval_elems; 00232 } 00233 00239 MConstPtr getElemBeforeTime(const ros::Time& time) const 00240 { 00241 namespace mt = ros::message_traits; 00242 00243 boost::mutex::scoped_lock lock(cache_lock_); 00244 00245 MConstPtr out ; 00246 00247 unsigned int i=0 ; 00248 int elem_index = -1 ; 00249 while (i<cache_.size() && 00250 mt::TimeStamp<M>::value(*cache_[i].getMessage()) < time) 00251 { 00252 elem_index = i ; 00253 i++ ; 00254 } 00255 00256 if (elem_index >= 0) 00257 out = cache_[elem_index].getMessage() ; 00258 00259 return out ; 00260 } 00261 00267 MConstPtr getElemAfterTime(const ros::Time& time) const 00268 { 00269 namespace mt = ros::message_traits; 00270 00271 boost::mutex::scoped_lock lock(cache_lock_); 00272 00273 MConstPtr out ; 00274 00275 int i=cache_.size()-1 ; 00276 int elem_index = -1 ; 00277 while (i>=0 && 00278 mt::TimeStamp<M>::value(*cache_[i].getMessage()) > time) 00279 { 00280 elem_index = i ; 00281 i-- ; 00282 } 00283 00284 if (elem_index >= 0) 00285 out = cache_[elem_index].getMessage() ; 00286 else 00287 out.reset() ; 00288 00289 return out ; 00290 } 00291 00295 ros::Time getLatestTime() const 00296 { 00297 namespace mt = ros::message_traits; 00298 00299 boost::mutex::scoped_lock lock(cache_lock_); 00300 00301 ros::Time latest_time; 00302 00303 if (cache_.size() > 0) 00304 latest_time = mt::TimeStamp<M>::value(*cache_.back().getMessage()); 00305 00306 return latest_time ; 00307 } 00308 00312 ros::Time getOldestTime() const 00313 { 00314 namespace mt = ros::message_traits; 00315 00316 boost::mutex::scoped_lock lock(cache_lock_); 00317 00318 ros::Time oldest_time; 00319 00320 if (cache_.size() > 0) 00321 oldest_time = mt::TimeStamp<M>::value(*cache_.front().getMessage()); 00322 00323 return oldest_time ; 00324 } 00325 00326 00327 private: 00328 void callback(const EventType& evt) 00329 { 00330 add(evt); 00331 } 00332 00333 mutable boost::mutex cache_lock_ ; 00334 std::deque<EventType> cache_ ; 00335 unsigned int cache_size_ ; 00336 00337 Connection incoming_connection_; 00338 }; 00339 00340 } 00341 00342 00343 #endif /* MESSAGE_FILTERS_CACHE_H_ */