$search
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(MsgConstPtr(new Msg), 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