sorted_deque_unittest.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, 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 
36 #include <gtest/gtest.h>
37 
38 #include "settlerlib/sorted_deque.h"
39 
40 using namespace std;
41 using namespace settlerlib;
42 
43 struct Header
44 {
45  ros::Time stamp ;
46 } ;
47 
48 
49 struct Msg
50 {
51  Header header ;
52  int data ;
53 } ;
54 
55 void fillEasy(SortedDeque<Msg>& sd, unsigned int start, unsigned int end)
56 {
57  for (unsigned int i=start; i < end; i++)
58  {
59  Msg msg ;
60  msg.data = i ;
61  msg.header.stamp.fromSec(i*10) ;
62 
63  sd.add(msg) ;
64  }
65 }
66 
67 TEST(SortedDeque, easyInterval)
68 {
69  SortedDeque<Msg> sd;
70  sd.setMaxSize(10);
71 
72  fillEasy(sd, 0, 5);
73 
74  vector<Msg> interval_data = sd.getInterval(ros::Time().fromSec(5), ros::Time().fromSec(35)) ;
75 
76  EXPECT_EQ(interval_data.size(), (unsigned int) 3) ;
77  EXPECT_EQ(interval_data[0].data, 1) ;
78  EXPECT_EQ(interval_data[1].data, 2) ;
79  EXPECT_EQ(interval_data[2].data, 3) ;
80 
81  // Look for an interval past the end of the cache
82  interval_data = sd.getInterval(ros::Time().fromSec(55), ros::Time().fromSec(65)) ;
83  EXPECT_EQ(interval_data.size(), (unsigned int) 0) ;
84 
85  // Look for an interval that fell off the back of the cache
86  fillEasy(sd, 5, 20) ;
87  interval_data = sd.getInterval(ros::Time().fromSec(5), ros::Time().fromSec(35)) ;
88  EXPECT_EQ(interval_data.size(), (unsigned int) 0) ;
89 }
90 
91 TEST(SortedDeque, easySurroundingInterval)
92 {
93  SortedDeque<Msg> cache;
94  cache.setMaxSize(10);
95  fillEasy(cache, 1, 6);
96 
97  vector<Msg> interval_data;
98  interval_data = cache.getSurroundingInterval(ros::Time(15,0), ros::Time(35,0)) ;
99  EXPECT_EQ(interval_data.size(), (unsigned int) 4);
100  EXPECT_EQ(interval_data[0].data, 1);
101  EXPECT_EQ(interval_data[1].data, 2);
102  EXPECT_EQ(interval_data[2].data, 3);
103  EXPECT_EQ(interval_data[3].data, 4);
104 
105  interval_data = cache.getSurroundingInterval(ros::Time(0,0), ros::Time(35,0)) ;
106  EXPECT_EQ(interval_data.size(), (unsigned int) 4);
107  EXPECT_EQ(interval_data[0].data, 1);
108 
109  interval_data = cache.getSurroundingInterval(ros::Time(35,0), ros::Time(35,0)) ;
110  EXPECT_EQ(interval_data.size(), (unsigned int) 2);
111  EXPECT_EQ(interval_data[0].data, 3);
112  EXPECT_EQ(interval_data[1].data, 4);
113 
114  interval_data = cache.getSurroundingInterval(ros::Time(55,0), ros::Time(55,0)) ;
115  EXPECT_EQ(interval_data.size(), (unsigned int) 1);
116  EXPECT_EQ(interval_data[0].data, 5);
117 }
118 
119 Msg buildMsg(double time, int data)
120 {
121  Msg msg;
122  msg.data = data;
123  msg.header.stamp.fromSec(time);
124  return msg;
125 }
126 
127 TEST(SortedDeque, easyUnsorted)
128 {
129  SortedDeque<Msg> cache;
130  cache.setMaxSize(10);
131 
132  cache.add(buildMsg(10.0, 1)) ;
133  cache.add(buildMsg(30.0, 3)) ;
134  cache.add(buildMsg(70.0, 7)) ;
135  cache.add(buildMsg( 5.0, 0)) ;
136  cache.add(buildMsg(20.0, 2)) ;
137 
138  vector<Msg> interval_data = cache.getInterval(ros::Time().fromSec(3), ros::Time().fromSec(15)) ;
139 
140  EXPECT_EQ(interval_data.size(), (unsigned int) 2) ;
141  EXPECT_EQ(interval_data[0].data, 0) ;
142  EXPECT_EQ(interval_data[1].data, 1) ;
143 
144  // Grab all the data
145  interval_data = cache.getInterval(ros::Time().fromSec(0), ros::Time().fromSec(80)) ;
146  EXPECT_EQ(interval_data.size(), (unsigned int) 5) ;
147  EXPECT_EQ(interval_data[0].data, 0) ;
148  EXPECT_EQ(interval_data[1].data, 1) ;
149  EXPECT_EQ(interval_data[2].data, 2) ;
150  EXPECT_EQ(interval_data[3].data, 3) ;
151  EXPECT_EQ(interval_data[4].data, 7) ;
152 }
153 
154 TEST(SortedDeque, easyElemBeforeAfter)
155 {
156  SortedDeque<Msg> cache;
157  cache.setMaxSize(10);
158  Msg elem;
159 
160  fillEasy(cache, 5, 10) ;
161 
162  bool result;
163  result = cache.getElemAfterTime( ros::Time().fromSec(85.0), elem);
164 
165  ASSERT_TRUE(result);
166  EXPECT_EQ(elem.data, 9);
167 
168  result = cache.getElemBeforeTime( ros::Time().fromSec(85.0), elem);
169  ASSERT_TRUE(result) ;
170  EXPECT_EQ(elem.data, 8) ;
171 
172  result = cache.getElemBeforeTime( ros::Time().fromSec(45.0), elem);
173  EXPECT_FALSE(result) ;
174 }
175 
176 TEST(SortedDeque, easyRemoval)
177 {
178  SortedDeque<Msg> sd;
179  sd.setMaxSize(20);
180 
181  fillEasy(sd, 1, 10);
182 
183  vector<Msg> interval_data;
184  interval_data = sd.getInterval(ros::Time().fromSec(5), ros::Time().fromSec(105)) ;
185  EXPECT_EQ(interval_data.size(), (unsigned int) 9) ;
186 
187  sd.removeAllBeforeTime(ros::Time().fromSec(35));
188  interval_data = sd.getInterval(ros::Time().fromSec(5), ros::Time().fromSec(105)) ;
189  EXPECT_EQ(interval_data.size(), (unsigned int) 6);
190  EXPECT_EQ(interval_data[0].data, 4) ;
191  EXPECT_EQ(interval_data[1].data, 5) ;
192  EXPECT_EQ(interval_data[2].data, 6) ;
193  EXPECT_EQ(interval_data[3].data, 7) ;
194  EXPECT_EQ(interval_data[4].data, 8) ;
195  EXPECT_EQ(interval_data[5].data, 9) ;
196 }
197 
198 TEST(SortedDeque, easyClosest)
199 {
200  SortedDeque<Msg> sd;
201  sd.setMaxSize(20);
202 
203  fillEasy(sd, 1, 10);
204 
205  bool found;
206  Msg elem;
207 
208  found = sd.getClosestElem(ros::Time().fromSec(5), elem) ;
209  EXPECT_TRUE(found) ;
210  EXPECT_EQ(elem.data, 1) ;
211 
212  found = sd.getClosestElem(ros::Time().fromSec(20), elem) ;
213  EXPECT_TRUE(found) ;
214  EXPECT_EQ(elem.data, 2) ;
215 }
216 
217 
218 
219 TEST(SortedDeque, easyPointer)
220 {
221  SortedDeque<boost::shared_ptr<Msg> > sd(SortedDeque<boost::shared_ptr<Msg> >::getPtrStamp);
222  sd.setMaxSize(20);
223 
224  boost::shared_ptr<Msg> msg_ptr(new Msg);
225  msg_ptr->header.stamp = ros::Time(10,0);
226  sd.add(msg_ptr);
227 
228  msg_ptr.reset(new Msg);
229  msg_ptr->header.stamp = ros::Time(20,0);
230  sd.add(msg_ptr);
231 
232  msg_ptr.reset(new Msg);
233  msg_ptr->header.stamp = ros::Time(30,0);
234  sd.add(msg_ptr);
235 
236 
237  boost::shared_ptr<Msg> found_elem;
238  bool found;
239  found = sd.getClosestElem(ros::Time().fromSec(22), found_elem);
240  ASSERT_TRUE(found);
241  EXPECT_EQ(found_elem->header.stamp, ros::Time(20,0));
242 }
243 
244 
245 
246 int main(int argc, char **argv){
247  testing::InitGoogleTest(&argc, argv);
248  return RUN_ALL_TESTS();
249 }
settlerlib::SortedDeque
Adds helper routines to the STL Deque for holding messages with headers.
Definition: sorted_deque.h:58
settlerlib::SortedDeque::setMaxSize
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
TimeBase< Time, Duration >::fromSec
Time & fromSec(double t)
boost::shared_ptr
settlerlib::SortedDeque::getClosestElem
bool getClosestElem(const ros::Time &time, M &out)
Get the elem that occurs closest to the specified time.
Definition: sorted_deque.h:258
settlerlib::SortedDeque::getElemAfterTime
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
settlerlib::SortedDeque::getElemBeforeTime
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
settlerlib::SortedDeque::add
void add(const M &msg)
Add a new element to the deque, correctly sorted by timestamp.
Definition: sorted_deque.h:109
TEST
TEST(SortedDeque, easyInterval)
Definition: sorted_deque_unittest.cpp:67
buildMsg
Msg buildMsg(double time, int data)
Definition: sorted_deque_unittest.cpp:119
sorted_deque.h
main
int main(int argc, char **argv)
Definition: sorted_deque_unittest.cpp:246
settlerlib::SortedDeque::getSurroundingInterval
std::vector< M > getSurroundingInterval(const ros::Time &start, const ros::Time &end)
Definition: sorted_deque.h:174
fillEasy
void fillEasy(SortedDeque< Msg > &sd, unsigned int start, unsigned int end)
Definition: sorted_deque_unittest.cpp:55
settlerlib::SortedDeque::removeAllBeforeTime
void removeAllBeforeTime(const ros::Time &time)
Removes all elements that occur before the specified time.
Definition: sorted_deque.h:287
settlerlib::SortedDeque::getInterval
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 Fri Apr 2 2021 02:12:59