settlerlib: settlerlib: sorted_deque_unittest.cpp Source File

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 
00036 #include <gtest/gtest.h>
00037 
00038 #include "settlerlib/sorted_deque.h"
00039 
00040 using namespace std;
00041 using namespace settlerlib;
00042 
00043 struct Header
00044 {
00045   ros::Time stamp ;
00046 } ;
00047 
00048 
00049 struct Msg
00050 {
00051   Header header ;
00052   int data ;
00053 } ;
00054 
00055 void fillEasy(SortedDeque<Msg>& sd, unsigned int start, unsigned int end)
00056 {
00057   for (unsigned int i=start; i < end; i++)
00058   {
00059     Msg msg ;
00060     msg.data = i ;
00061     msg.header.stamp.fromSec(i*10) ;
00062 
00063     sd.add(msg) ;
00064   }
00065 }
00066 
00067 TEST(SortedDeque, easyInterval)
00068 {
00069   SortedDeque<Msg> sd;
00070   sd.setMaxSize(10);
00071 
00072   fillEasy(sd, 0, 5);
00073 
00074   vector<Msg> interval_data = sd.getInterval(ros::Time().fromSec(5), ros::Time().fromSec(35)) ;
00075 
00076   EXPECT_EQ(interval_data.size(), (unsigned int) 3) ;
00077   EXPECT_EQ(interval_data[0].data, 1) ;
00078   EXPECT_EQ(interval_data[1].data, 2) ;
00079   EXPECT_EQ(interval_data[2].data, 3) ;
00080 
00081   // Look for an interval past the end of the cache
00082   interval_data = sd.getInterval(ros::Time().fromSec(55), ros::Time().fromSec(65)) ;
00083   EXPECT_EQ(interval_data.size(), (unsigned int) 0) ;
00084 
00085   // Look for an interval that fell off the back of the cache
00086   fillEasy(sd, 5, 20) ;
00087   interval_data = sd.getInterval(ros::Time().fromSec(5), ros::Time().fromSec(35)) ;
00088   EXPECT_EQ(interval_data.size(), (unsigned int) 0) ;
00089 }
00090 
00091 TEST(SortedDeque, easySurroundingInterval)
00092 {
00093   SortedDeque<Msg> cache;
00094   cache.setMaxSize(10);
00095   fillEasy(cache, 1, 6);
00096 
00097   vector<Msg> interval_data;
00098   interval_data = cache.getSurroundingInterval(ros::Time(15,0), ros::Time(35,0)) ;
00099   EXPECT_EQ(interval_data.size(), (unsigned int) 4);
00100   EXPECT_EQ(interval_data[0].data, 1);
00101   EXPECT_EQ(interval_data[1].data, 2);
00102   EXPECT_EQ(interval_data[2].data, 3);
00103   EXPECT_EQ(interval_data[3].data, 4);
00104 
00105   interval_data = cache.getSurroundingInterval(ros::Time(0,0), ros::Time(35,0)) ;
00106   EXPECT_EQ(interval_data.size(), (unsigned int) 4);
00107   EXPECT_EQ(interval_data[0].data, 1);
00108 
00109   interval_data = cache.getSurroundingInterval(ros::Time(35,0), ros::Time(35,0)) ;
00110   EXPECT_EQ(interval_data.size(), (unsigned int) 2);
00111   EXPECT_EQ(interval_data[0].data, 3);
00112   EXPECT_EQ(interval_data[1].data, 4);
00113 
00114   interval_data = cache.getSurroundingInterval(ros::Time(55,0), ros::Time(55,0)) ;
00115   EXPECT_EQ(interval_data.size(), (unsigned int) 1);
00116   EXPECT_EQ(interval_data[0].data, 5);
00117 }
00118 
00119 Msg buildMsg(double time, int data)
00120 {
00121   Msg msg;
00122   msg.data = data;
00123   msg.header.stamp.fromSec(time);
00124   return msg;
00125 }
00126 
00127 TEST(SortedDeque, easyUnsorted)
00128 {
00129   SortedDeque<Msg> cache;
00130   cache.setMaxSize(10);
00131 
00132   cache.add(buildMsg(10.0, 1)) ;
00133   cache.add(buildMsg(30.0, 3)) ;
00134   cache.add(buildMsg(70.0, 7)) ;
00135   cache.add(buildMsg( 5.0, 0)) ;
00136   cache.add(buildMsg(20.0, 2)) ;
00137 
00138   vector<Msg> interval_data = cache.getInterval(ros::Time().fromSec(3), ros::Time().fromSec(15)) ;
00139 
00140   EXPECT_EQ(interval_data.size(), (unsigned int) 2) ;
00141   EXPECT_EQ(interval_data[0].data, 0) ;
00142   EXPECT_EQ(interval_data[1].data, 1) ;
00143 
00144   // Grab all the data
00145   interval_data = cache.getInterval(ros::Time().fromSec(0), ros::Time().fromSec(80)) ;
00146   EXPECT_EQ(interval_data.size(), (unsigned int) 5) ;
00147   EXPECT_EQ(interval_data[0].data, 0) ;
00148   EXPECT_EQ(interval_data[1].data, 1) ;
00149   EXPECT_EQ(interval_data[2].data, 2) ;
00150   EXPECT_EQ(interval_data[3].data, 3) ;
00151   EXPECT_EQ(interval_data[4].data, 7) ;
00152 }
00153 
00154 TEST(SortedDeque, easyElemBeforeAfter)
00155 {
00156   SortedDeque<Msg> cache;
00157   cache.setMaxSize(10);
00158   Msg elem;
00159 
00160   fillEasy(cache, 5, 10) ;
00161 
00162   bool result;
00163   result = cache.getElemAfterTime( ros::Time().fromSec(85.0), elem);
00164 
00165   ASSERT_TRUE(result);
00166   EXPECT_EQ(elem.data, 9);
00167 
00168   result = cache.getElemBeforeTime( ros::Time().fromSec(85.0), elem);
00169   ASSERT_TRUE(result) ;
00170   EXPECT_EQ(elem.data, 8) ;
00171 
00172   result = cache.getElemBeforeTime( ros::Time().fromSec(45.0), elem);
00173   EXPECT_FALSE(result) ;
00174 }
00175 
00176 TEST(SortedDeque, easyRemoval)
00177 {
00178   SortedDeque<Msg> sd;
00179   sd.setMaxSize(20);
00180 
00181   fillEasy(sd, 1, 10);
00182 
00183   vector<Msg> interval_data;
00184   interval_data = sd.getInterval(ros::Time().fromSec(5), ros::Time().fromSec(105)) ;
00185   EXPECT_EQ(interval_data.size(), (unsigned int) 9) ;
00186 
00187   sd.removeAllBeforeTime(ros::Time().fromSec(35));
00188   interval_data = sd.getInterval(ros::Time().fromSec(5), ros::Time().fromSec(105)) ;
00189   EXPECT_EQ(interval_data.size(), (unsigned int) 6);
00190   EXPECT_EQ(interval_data[0].data, 4) ;
00191   EXPECT_EQ(interval_data[1].data, 5) ;
00192   EXPECT_EQ(interval_data[2].data, 6) ;
00193   EXPECT_EQ(interval_data[3].data, 7) ;
00194   EXPECT_EQ(interval_data[4].data, 8) ;
00195   EXPECT_EQ(interval_data[5].data, 9) ;
00196 }
00197 
00198 TEST(SortedDeque, easyClosest)
00199 {
00200   SortedDeque<Msg> sd;
00201   sd.setMaxSize(20);
00202 
00203   fillEasy(sd, 1, 10);
00204 
00205   bool found;
00206   Msg elem;
00207 
00208   found = sd.getClosestElem(ros::Time().fromSec(5), elem) ;
00209   EXPECT_TRUE(found) ;
00210   EXPECT_EQ(elem.data, 1) ;
00211 
00212   found = sd.getClosestElem(ros::Time().fromSec(20), elem) ;
00213   EXPECT_TRUE(found) ;
00214   EXPECT_EQ(elem.data, 2) ;
00215 }
00216 
00217 
00218 
00219 TEST(SortedDeque, easyPointer)
00220 {
00221   SortedDeque<boost::shared_ptr<Msg> > sd(SortedDeque<boost::shared_ptr<Msg> >::getPtrStamp);
00222   sd.setMaxSize(20);
00223 
00224   boost::shared_ptr<Msg> msg_ptr(new Msg);
00225   msg_ptr->header.stamp = ros::Time(10,0);
00226   sd.add(msg_ptr);
00227 
00228   msg_ptr.reset(new Msg);
00229   msg_ptr->header.stamp = ros::Time(20,0);
00230   sd.add(msg_ptr);
00231 
00232   msg_ptr.reset(new Msg);
00233   msg_ptr->header.stamp = ros::Time(30,0);
00234   sd.add(msg_ptr);
00235 
00236 
00237   boost::shared_ptr<Msg> found_elem;
00238   bool found;
00239   found = sd.getClosestElem(ros::Time().fromSec(22), found_elem);
00240   ASSERT_TRUE(found);
00241   EXPECT_EQ(found_elem->header.stamp, ros::Time(20,0));
00242 }
00243 
00244 
00245 
00246 int main(int argc, char **argv){
00247   testing::InitGoogleTest(&argc, argv);
00248   return RUN_ALL_TESTS();
00249 }