sorted_deque.h
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2009, 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 Willow Garage 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 *********************************************************************/
34 
35 #include <deque>
36 #include <boost/shared_ptr.hpp>
37 #include <boost/bind.hpp>
38 #include <boost/function.hpp>
39 #include <ros/time.h>
40 #include <ros/console.h>
41 
42 #ifndef SETTLERLIB_SORTED_DEQUE_H_
43 #define SETTLERLIB_SORTED_DEQUE_H_
44 
45 #define DEQUE_DEBUG(fmt, ...) \
46  ROS_DEBUG_NAMED(logger_.c_str(), fmt,##__VA_ARGS__)
47 
48 namespace settlerlib
49 {
50 
57 template <class M>
58 class SortedDeque : public std::deque<M>
59 {
60 public:
61 
62  using std::deque<M>::size;
63  using std::deque<M>::front;
64  using std::deque<M>::back;
65  using std::deque<M>::pop_front;
66  using std::deque<M>::begin;
67  using std::deque<M>::end;
68  using std::deque<M>::rbegin;
69  using std::deque<M>::rend;
70  using std::deque<M>::insert;
71  using std::deque<M>::at;
72  using std::deque<M>::erase;
73 
78  SortedDeque(std::string logger = "deque") : std::deque<M>(), logger_(logger)
79  {
81  max_size_ = 1;
82  }
83 
89  SortedDeque(boost::function<const ros::Time&(const M&)> getStampFunc,
90  std::string logger = "deque") : std::deque<M>(), logger_(logger)
91  {
92  getStamp = getStampFunc;
93  max_size_ = 1;
94  }
95 
100  void setMaxSize(unsigned int max_size)
101  {
102  max_size_ = max_size ;
103  }
104 
109  void add(const M& msg)
110  {
111  DEQUE_DEBUG("Called add()");
112  DEQUE_DEBUG_STATS(" ");
113  if (max_size_ != 0)
114  {
115  while (size() >= max_size_) // Keep popping off old data until we have space for a new msg
116  {
117  pop_front() ; // The front of the deque has the oldest elem, so we can get rid of it
118  DEQUE_DEBUG(" Popping element");
119  DEQUE_DEBUG_STATS(" ");
120  }
121  }
122 
123  typename std::deque<M>::reverse_iterator rev_it = rbegin();
124 
125  // Keep walking backwards along deque until we hit the beginning,
126  // or until we find a timestamp that's smaller than (or equal to) msg's timestamp
127  while(rev_it != rend() && getStamp(*rev_it) > getStamp(msg))
128  rev_it++;
129 
130  // Add msg to the cache
131  insert(rev_it.base(), msg);
132  DEQUE_DEBUG(" Done inserting");
133  DEQUE_DEBUG_STATS(" ");
134  }
135 
141  std::vector<M> getInterval(const ros::Time& start, const ros::Time& end)
142  {
143  // Find the starting index. (Find the first index after [or at] the start of the interval)
144  unsigned int start_index = 0 ;
145  while(start_index < size() &&
146  getStamp(at(start_index)) < start)
147  {
148  start_index++ ;
149  }
150 
151  // Find the ending index. (Find the first index after the end of interval)
152  unsigned int end_index = start_index ;
153  while(end_index < size() &&
154  getStamp(at(end_index)) <= end)
155  {
156  end_index++ ;
157  }
158 
159  std::vector<M> interval_elems ;
160  interval_elems.reserve(end_index - start_index) ;
161  for (unsigned int i=start_index; i<end_index; i++)
162  {
163  interval_elems.push_back(at(i)) ;
164  }
165 
166  return interval_elems ;
167  }
168 
174  std::vector<M> getSurroundingInterval(const ros::Time& start, const ros::Time& end)
175  {
176  // Find the starting index. (Find the first index after [or at] the start of the interval)
177  unsigned int start_index = size()-1;
178  while(start_index > 0 &&
179  getStamp(at(start_index)) > start)
180  {
181  start_index--;
182  }
183  unsigned int end_index = start_index;
184  while(end_index < size()-1 &&
185  getStamp(at(end_index)) < end)
186  {
187  end_index++;
188  }
189 
190  std::vector<M> interval_elems;
191  interval_elems.reserve(end_index - start_index + 1) ;
192  for (unsigned int i=start_index; i<=end_index; i++)
193  {
194  interval_elems.push_back(at(i)) ;
195  }
196 
197  return interval_elems;
198  }
199 
206  bool getElemBeforeTime(const ros::Time& time, M& out) const
207  {
208  unsigned int i=0 ;
209  int elem_index = -1 ;
210  while (i<size() &&
211  getStamp(at(i)) < time)
212  {
213  elem_index = i ;
214  i++ ;
215  }
216 
217  if (elem_index >= 0)
218  {
219  out = at(elem_index);
220  return true;
221  }
222  //out = M();
223  return false;
224  }
225 
232  bool getElemAfterTime(const ros::Time& time, M& out) const
233  {
234  int i=size()-1 ;
235  int elem_index = -1 ;
236  while (i>=0 &&
237  getStamp(at(i)) > time)
238  {
239  elem_index = i ;
240  i-- ;
241  }
242 
243  if (elem_index >= 0)
244  {
245  out = at(elem_index);
246  return true;
247  }
248  //out = M();
249  return false;
250  }
251 
258  bool getClosestElem(const ros::Time& time, M& out)
259  {
260  if (size() == 0)
261  return false;
262 
263  typename std::deque<M>::iterator it = begin();
264  typename std::deque<M>::iterator best = it;
265 
266  double best_diff = fabs( (time - getStamp(*best)).toSec());
267 
268  while (it != end())
269  {
270  double cur_diff = fabs( (time - getStamp(*it)).toSec());
271  if (cur_diff < best_diff)
272  {
273  best_diff = cur_diff;
274  best = it;
275  }
276  ++it;
277  }
278  out = *best;
279  return true;
280  }
281 
282 
287  void removeAllBeforeTime(const ros::Time& time)
288  {
289  DEQUE_DEBUG("Called removeAllBeforeTime()");
290  DEQUE_DEBUG(" Erasing all elems before time: %u %u", time.sec, time.nsec);
291  typename std::deque<M>::iterator it = begin();
292 
293  while (size() > 0 && getStamp(front()) < time)
294  {
295  DEQUE_DEBUG(" Erasing elem at time: %u, %u", getStamp(front()).sec, getStamp(front()).nsec);
296  pop_front();
297  DEQUE_DEBUG(" Erased an elem");
298  DEQUE_DEBUG_STATS(" ");
299  }
300  DEQUE_DEBUG(" Done erasing elems");
301  }
302 
303  static const ros::Time& getPtrStamp(const M& m)
304  {
305  return m->header.stamp;
306  }
307 
308  static const ros::Time& getStructStamp(const M& m)
309  {
310  return m.header.stamp;
311  }
312 
313  static const ros::Time& getHeaderStamp(const M& m)
314  {
315  return m.stamp;
316  }
317 private:
318  unsigned int max_size_;
319  std::string logger_;
320 
321 
322  /*const ros::Time& getStamp(const M& m)
323  {
324  return getStructStamp(m);
325  }*/
326 
327  boost::function<const ros::Time&(const M&)> getStamp;
328 
329  inline void DEQUE_DEBUG_STATS(const std::string& prefix)
330  {
331  DEQUE_DEBUG("%sdeque.size(): %u max_size: %u", prefix.c_str(), (unsigned int) size(), max_size_);
332  }
333 };
334 
335 }
336 
337 #undef DEQUE_DEBUG
338 #undef DEQUE_WARN
339 
340 #endif
SortedDeque(boost::function< const ros::Time &(const M &)> getStampFunc, std::string logger="deque")
Advanced constructor, allowing user to specific the timestamp getter method.
Definition: sorted_deque.h:89
static const ros::Time & getStructStamp(const M &m)
Definition: sorted_deque.h:308
Adds helper routines to the STL Deque for holding messages with headers.
Definition: sorted_deque.h:58
void setMaxSize(unsigned int max_size)
Set the maximum # of elements this deque can hold. Older elems are popped once the length is exeeded...
Definition: sorted_deque.h:100
static const ros::Time & getHeaderStamp(const M &m)
Definition: sorted_deque.h:313
bool getClosestElem(const ros::Time &time, M &out)
Get the elem that occurs closest to the specified time.
Definition: sorted_deque.h:258
static const ros::Time & getPtrStamp(const M &m)
Definition: sorted_deque.h:303
bool getElemAfterTime(const ros::Time &time, M &out) const
Grab the oldest element that occurs right after the specified time.
Definition: sorted_deque.h:232
bool getElemBeforeTime(const ros::Time &time, M &out) const
Grab the oldest element that occurs right before the specified time.
Definition: sorted_deque.h:206
void DEQUE_DEBUG_STATS(const std::string &prefix)
Definition: sorted_deque.h:329
void add(const M &msg)
Add a new element to the deque, correctly sorted by timestamp.
Definition: sorted_deque.h:109
#define DEQUE_DEBUG(fmt,...)
Definition: sorted_deque.h:45
SortedDeque(std::string logger="deque")
Assumes that &#39;.header.stamp&#39; can be used to get the stamp used for sorting.
Definition: sorted_deque.h:78
std::vector< M > getSurroundingInterval(const ros::Time &start, const ros::Time &end)
Definition: sorted_deque.h:174
boost::function< const ros::Time &(const M &)> getStamp
Definition: sorted_deque.h:327
void removeAllBeforeTime(const ros::Time &time)
Removes all elements that occur before the specified time.
Definition: sorted_deque.h:287
std::vector< M > getInterval(const ros::Time &start, const ros::Time &end)
Extract all the elements that occur in the interval between the start and end times.
Definition: sorted_deque.h:141


settlerlib
Author(s): Vijay Pradeep
autogenerated on Thu Jun 6 2019 19:17:23