sorted_deque.h
Go to the documentation of this file.
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 #include <deque>
00036 #include <boost/shared_ptr.hpp>
00037 #include <boost/bind.hpp>
00038 #include <boost/function.hpp>
00039 #include <ros/time.h>
00040 #include <ros/console.h>
00041 
00042 #ifndef SETTLERLIB_SORTED_DEQUE_H_
00043 #define SETTLERLIB_SORTED_DEQUE_H_
00044 
00045 #define DEQUE_DEBUG(fmt, ...) \
00046     ROS_DEBUG_NAMED(logger_.c_str(), fmt,##__VA_ARGS__)
00047 
00048 namespace settlerlib
00049 {
00050 
00057 template <class M>
00058 class SortedDeque : public std::deque<M>
00059 {
00060 public:
00061 
00062   using std::deque<M>::size;
00063   using std::deque<M>::front;
00064   using std::deque<M>::back;
00065   using std::deque<M>::pop_front;
00066   using std::deque<M>::begin;
00067   using std::deque<M>::end;
00068   using std::deque<M>::rbegin;
00069   using std::deque<M>::rend;
00070   using std::deque<M>::insert;
00071   using std::deque<M>::at;
00072   using std::deque<M>::erase;
00073 
00078   SortedDeque(std::string logger = "deque") : std::deque<M>(), logger_(logger)
00079   {
00080     getStamp = &SortedDeque<M>::getStructStamp;
00081     max_size_ = 1;
00082   }
00083 
00089   SortedDeque(boost::function<const ros::Time&(const M&)> getStampFunc,
00090               std::string logger = "deque") : std::deque<M>(), logger_(logger)
00091   {
00092     getStamp = getStampFunc;
00093     max_size_ = 1;
00094   }
00095 
00100   void setMaxSize(unsigned int max_size)
00101   {
00102     max_size_ = max_size ;
00103   }
00104 
00109   void add(const M& msg)
00110   {
00111     DEQUE_DEBUG("Called add()");
00112     DEQUE_DEBUG_STATS("   ");
00113     if (max_size_ != 0)
00114     {
00115       while (size() >= max_size_)                // Keep popping off old data until we have space for a new msg
00116       {
00117         pop_front() ;                            // The front of the deque has the oldest elem, so we can get rid of it
00118         DEQUE_DEBUG("   Popping element");
00119         DEQUE_DEBUG_STATS("   ");
00120       }
00121     }
00122 
00123     typename std::deque<M>::reverse_iterator rev_it = rbegin();
00124 
00125     // Keep walking backwards along deque until we hit the beginning,
00126     //   or until we find a timestamp that's smaller than (or equal to) msg's timestamp
00127     while(rev_it != rend() && getStamp(*rev_it) > getStamp(msg))
00128       rev_it++;
00129 
00130     // Add msg to the cache
00131     insert(rev_it.base(), msg);
00132     DEQUE_DEBUG("   Done inserting");
00133     DEQUE_DEBUG_STATS("   ");
00134   }
00135 
00141   std::vector<M> getInterval(const ros::Time& start, const ros::Time& end)
00142   {
00143     // Find the starting index. (Find the first index after [or at] the start of the interval)
00144     unsigned int start_index = 0 ;
00145     while(start_index < size() &&
00146           getStamp(at(start_index)) < start)
00147     {
00148       start_index++ ;
00149     }
00150 
00151     // Find the ending index. (Find the first index after the end of interval)
00152     unsigned int end_index = start_index ;
00153     while(end_index < size() &&
00154           getStamp(at(end_index)) <= end)
00155     {
00156       end_index++ ;
00157     }
00158 
00159     std::vector<M> interval_elems ;
00160     interval_elems.reserve(end_index - start_index) ;
00161     for (unsigned int i=start_index; i<end_index; i++)
00162     {
00163       interval_elems.push_back(at(i)) ;
00164     }
00165 
00166     return interval_elems ;
00167   }
00168 
00174   std::vector<M> getSurroundingInterval(const ros::Time& start, const ros::Time& end)
00175   {
00176     // Find the starting index. (Find the first index after [or at] the start of the interval)
00177     unsigned int start_index = size()-1;
00178     while(start_index > 0 &&
00179           getStamp(at(start_index)) > start)
00180     {
00181       start_index--;
00182     }
00183     unsigned int end_index = start_index;
00184     while(end_index < size()-1 &&
00185           getStamp(at(end_index)) < end)
00186     {
00187       end_index++;
00188     }
00189 
00190     std::vector<M> interval_elems;
00191     interval_elems.reserve(end_index - start_index + 1) ;
00192     for (unsigned int i=start_index; i<=end_index; i++)
00193     {
00194       interval_elems.push_back(at(i)) ;
00195     }
00196 
00197     return interval_elems;
00198   }
00199 
00206   bool getElemBeforeTime(const ros::Time& time, M& out) const
00207   {
00208     unsigned int i=0 ;
00209     int elem_index = -1 ;
00210     while (i<size() &&
00211            getStamp(at(i)) < time)
00212     {
00213       elem_index = i ;
00214       i++ ;
00215     }
00216 
00217     if (elem_index >= 0)
00218     {
00219       out = at(elem_index);
00220       return true;
00221     }
00222     //out = M();
00223     return false;
00224   }
00225 
00232   bool getElemAfterTime(const ros::Time& time, M& out) const
00233   {
00234     int i=size()-1 ;
00235     int elem_index = -1 ;
00236     while (i>=0 &&
00237            getStamp(at(i)) > time)
00238     {
00239       elem_index = i ;
00240       i-- ;
00241     }
00242 
00243     if (elem_index >= 0)
00244     {
00245       out = at(elem_index);
00246       return true;
00247     }
00248     //out = M();
00249     return false;
00250   }
00251 
00258   bool getClosestElem(const ros::Time& time, M& out)
00259   {
00260     if (size() == 0)
00261       return false;
00262 
00263     typename std::deque<M>::iterator it = begin();
00264     typename std::deque<M>::iterator best = it;
00265 
00266     double best_diff = fabs( (time - getStamp(*best)).toSec());
00267 
00268     while (it != end())
00269     {
00270       double cur_diff = fabs( (time - getStamp(*it)).toSec());
00271       if (cur_diff < best_diff)
00272       {
00273         best_diff = cur_diff;
00274         best = it;
00275       }
00276       ++it;
00277     }
00278     out = *best;
00279     return true;
00280   }
00281 
00282 
00287   void removeAllBeforeTime(const ros::Time& time)
00288   {
00289     DEQUE_DEBUG("Called removeAllBeforeTime()");
00290     DEQUE_DEBUG("   Erasing all elems before time: %u %u", time.sec, time.nsec);
00291     typename std::deque<M>::iterator it = begin();
00292 
00293     while (size() > 0 && getStamp(front()) < time)
00294     {
00295       DEQUE_DEBUG("   Erasing elem at time: %u, %u", getStamp(front()).sec, getStamp(front()).nsec);
00296       pop_front();
00297       DEQUE_DEBUG("   Erased an elem");
00298       DEQUE_DEBUG_STATS("   ");
00299     }
00300     DEQUE_DEBUG("   Done erasing elems");
00301   }
00302 
00303   static const ros::Time& getPtrStamp(const M& m)
00304   {
00305     return m->header.stamp;
00306   }
00307 
00308   static const ros::Time& getStructStamp(const M& m)
00309   {
00310     return m.header.stamp;
00311   }
00312 
00313   static const ros::Time& getHeaderStamp(const M& m)
00314   {
00315     return m.stamp;
00316   }
00317 private:
00318   unsigned int max_size_;
00319   std::string logger_;
00320 
00321 
00322   /*const ros::Time& getStamp(const M& m)
00323   {
00324     return getStructStamp(m);
00325   }*/
00326 
00327   boost::function<const ros::Time&(const M&)> getStamp;
00328 
00329   inline void DEQUE_DEBUG_STATS(const std::string& prefix)
00330   {
00331     DEQUE_DEBUG("%sdeque.size(): %u   max_size: %u", prefix.c_str(), (unsigned int) size(), max_size_);
00332   }
00333 };
00334 
00335 }
00336 
00337 #undef DEQUE_DEBUG
00338 #undef DEQUE_WARN
00339 
00340 #endif


settlerlib
Author(s): Vijay Pradeep
autogenerated on Tue Sep 27 2016 04:06:40