msg_cache_unittest.cpp
Go to the documentation of this 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 #include <gtest/gtest.h>
00036 
00037 #include "ros/time.h"
00038 #include <ros/init.h>
00039 #include "message_filters/cache.h"
00040 
00041 using namespace std ;
00042 using namespace message_filters ;
00043 
00044 struct Header
00045 {
00046   ros::Time stamp ;
00047 } ;
00048 
00049 
00050 struct Msg
00051 {
00052   Header header ;
00053   int data ;
00054 } ;
00055 typedef boost::shared_ptr<Msg const> MsgConstPtr;
00056 
00057 namespace ros
00058 {
00059 namespace message_traits
00060 {
00061 template<>
00062 struct TimeStamp<Msg>
00063 {
00064   static ros::Time value(const Msg& m)
00065   {
00066     return m.header.stamp;
00067   }
00068 };
00069 }
00070 }
00071 
00072 
00073 void fillCacheEasy(Cache<Msg>& cache, unsigned int start, unsigned int end)
00074 
00075 {
00076   for (unsigned int i=start; i < end; i++)
00077   {
00078     Msg* msg = new Msg ;
00079     msg->data = i ;
00080     msg->header.stamp.fromSec(i*10) ;
00081 
00082     boost::shared_ptr<Msg const> msg_ptr(msg) ;
00083     cache.add(msg_ptr) ;
00084   }
00085 }
00086 
00087 TEST(Cache, easyInterval)
00088 {
00089   Cache<Msg> cache(10) ;
00090   fillCacheEasy(cache, 0, 5) ;
00091 
00092   vector<boost::shared_ptr<Msg const> > interval_data = cache.getInterval(ros::Time().fromSec(5), ros::Time().fromSec(35)) ;
00093 
00094   ASSERT_EQ(interval_data.size(), (unsigned int) 3) ;
00095   EXPECT_EQ(interval_data[0]->data, 1) ;
00096   EXPECT_EQ(interval_data[1]->data, 2) ;
00097   EXPECT_EQ(interval_data[2]->data, 3) ;
00098 
00099   // Look for an interval past the end of the cache
00100   interval_data = cache.getInterval(ros::Time().fromSec(55), ros::Time().fromSec(65)) ;
00101   EXPECT_EQ(interval_data.size(), (unsigned int) 0) ;
00102 
00103   // Look for an interval that fell off the back of the cache
00104   fillCacheEasy(cache, 5, 20) ;
00105   interval_data = cache.getInterval(ros::Time().fromSec(5), ros::Time().fromSec(35)) ;
00106   EXPECT_EQ(interval_data.size(), (unsigned int) 0) ;
00107 }
00108 
00109 TEST(Cache, easySurroundingInterval)
00110 {
00111   Cache<Msg> cache(10);
00112   fillCacheEasy(cache, 1, 6);
00113 
00114   vector<boost::shared_ptr<Msg const> > interval_data;
00115   interval_data = cache.getSurroundingInterval(ros::Time(15,0), ros::Time(35,0)) ;
00116   ASSERT_EQ(interval_data.size(), (unsigned int) 4);
00117   EXPECT_EQ(interval_data[0]->data, 1);
00118   EXPECT_EQ(interval_data[1]->data, 2);
00119   EXPECT_EQ(interval_data[2]->data, 3);
00120   EXPECT_EQ(interval_data[3]->data, 4);
00121 
00122   interval_data = cache.getSurroundingInterval(ros::Time(0,0), ros::Time(35,0)) ;
00123   ASSERT_EQ(interval_data.size(), (unsigned int) 4);
00124   EXPECT_EQ(interval_data[0]->data, 1);
00125 
00126   interval_data = cache.getSurroundingInterval(ros::Time(35,0), ros::Time(35,0)) ;
00127   ASSERT_EQ(interval_data.size(), (unsigned int) 2);
00128   EXPECT_EQ(interval_data[0]->data, 3);
00129   EXPECT_EQ(interval_data[1]->data, 4);
00130 
00131   interval_data = cache.getSurroundingInterval(ros::Time(55,0), ros::Time(55,0)) ;
00132   ASSERT_EQ(interval_data.size(), (unsigned int) 1);
00133   EXPECT_EQ(interval_data[0]->data, 5);
00134 }
00135 
00136 
00137 boost::shared_ptr<Msg const> buildMsg(double time, int data)
00138 {
00139   Msg* msg = new Msg ;
00140   msg->data = data ;
00141   msg->header.stamp.fromSec(time) ;
00142 
00143   boost::shared_ptr<Msg const> msg_ptr(msg) ;
00144   return msg_ptr ;
00145 }
00146 
00147 TEST(Cache, easyUnsorted)
00148 {
00149   Cache<Msg> cache(10) ;
00150 
00151   cache.add(buildMsg(10.0, 1)) ;
00152   cache.add(buildMsg(30.0, 3)) ;
00153   cache.add(buildMsg(70.0, 7)) ;
00154   cache.add(buildMsg( 5.0, 0)) ;
00155   cache.add(buildMsg(20.0, 2)) ;
00156 
00157   vector<boost::shared_ptr<Msg const> > interval_data = cache.getInterval(ros::Time().fromSec(3), ros::Time().fromSec(15)) ;
00158 
00159   ASSERT_EQ(interval_data.size(), (unsigned int) 2) ;
00160   EXPECT_EQ(interval_data[0]->data, 0) ;
00161   EXPECT_EQ(interval_data[1]->data, 1) ;
00162 
00163   // Grab all the data
00164   interval_data = cache.getInterval(ros::Time().fromSec(0), ros::Time().fromSec(80)) ;
00165   ASSERT_EQ(interval_data.size(), (unsigned int) 5) ;
00166   EXPECT_EQ(interval_data[0]->data, 0) ;
00167   EXPECT_EQ(interval_data[1]->data, 1) ;
00168   EXPECT_EQ(interval_data[2]->data, 2) ;
00169   EXPECT_EQ(interval_data[3]->data, 3) ;
00170   EXPECT_EQ(interval_data[4]->data, 7) ;
00171 }
00172 
00173 
00174 TEST(Cache, easyElemBeforeAfter)
00175 {
00176   Cache<Msg> cache(10) ;
00177   boost::shared_ptr<Msg const> elem_ptr ;
00178 
00179   fillCacheEasy(cache, 5, 10) ;
00180 
00181   elem_ptr = cache.getElemAfterTime( ros::Time().fromSec(85.0)) ;
00182 
00183   ASSERT_FALSE(!elem_ptr) ;
00184   EXPECT_EQ(elem_ptr->data, 9) ;
00185 
00186   elem_ptr = cache.getElemBeforeTime( ros::Time().fromSec(85.0)) ;
00187   ASSERT_FALSE(!elem_ptr) ;
00188   EXPECT_EQ(elem_ptr->data, 8) ;
00189 
00190   elem_ptr = cache.getElemBeforeTime( ros::Time().fromSec(45.0)) ;
00191   EXPECT_TRUE(!elem_ptr) ;
00192 }
00193 
00194 struct EventHelper
00195 {
00196 public:
00197   void cb(const ros::MessageEvent<Msg const>& evt)
00198   {
00199     event_ = evt;
00200   }
00201 
00202   ros::MessageEvent<Msg const> event_;
00203 };
00204 
00205 TEST(Cache, eventInEventOut)
00206 {
00207   Cache<Msg> c0(10);
00208   Cache<Msg> c1(c0, 10);
00209   EventHelper h;
00210   c1.registerCallback(&EventHelper::cb, &h);
00211 
00212   ros::MessageEvent<Msg const> evt(boost::make_shared<Msg const>(), ros::Time(4));
00213   c0.add(evt);
00214 
00215   EXPECT_EQ(h.event_.getReceiptTime(), evt.getReceiptTime());
00216   EXPECT_EQ(h.event_.getMessage(), evt.getMessage());
00217 }
00218 
00219 int main(int argc, char **argv){
00220   testing::InitGoogleTest(&argc, argv);
00221   ros::init(argc, argv, "blah");
00222   ros::Time::init();
00223   return RUN_ALL_TESTS();
00224 }
00225 


message_filters
Author(s): Josh Faust, Vijay Pradeep
autogenerated on Tue Mar 7 2017 04:01:23