msg_cache_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 #include <gtest/gtest.h>
36 
37 #include "ros/time.h"
38 #include <ros/init.h>
39 #include "message_filters/cache.h"
40 
41 using namespace std ;
42 using namespace message_filters ;
43 
44 struct Header
45 {
47 } ;
48 
49 
50 struct Msg
51 {
53  int data ;
54 } ;
56 
57 namespace ros
58 {
59 namespace message_traits
60 {
61 template<>
62 struct TimeStamp<Msg>
63 {
64  static ros::Time value(const Msg& m)
65  {
66  return m.header.stamp;
67  }
68 };
69 }
70 }
71 
72 
73 void fillCacheEasy(Cache<Msg>& cache, unsigned int start, unsigned int end)
74 
75 {
76  for (unsigned int i=start; i < end; i++)
77  {
78  Msg* msg = new Msg ;
79  msg->data = i ;
80  msg->header.stamp.fromSec(i*10) ;
81 
82  boost::shared_ptr<Msg const> msg_ptr(msg) ;
83  cache.add(msg_ptr) ;
84  }
85 }
86 
87 TEST(Cache, easyInterval)
88 {
89  Cache<Msg> cache(10) ;
90  fillCacheEasy(cache, 0, 5) ;
91 
92  vector<boost::shared_ptr<Msg const> > interval_data = cache.getInterval(ros::Time().fromSec(5), ros::Time().fromSec(35)) ;
93 
94  ASSERT_EQ(interval_data.size(), (unsigned int) 3) ;
95  EXPECT_EQ(interval_data[0]->data, 1) ;
96  EXPECT_EQ(interval_data[1]->data, 2) ;
97  EXPECT_EQ(interval_data[2]->data, 3) ;
98 
99  // Look for an interval past the end of the cache
100  interval_data = cache.getInterval(ros::Time().fromSec(55), ros::Time().fromSec(65)) ;
101  EXPECT_EQ(interval_data.size(), (unsigned int) 0) ;
102 
103  // Look for an interval that fell off the back of the cache
104  fillCacheEasy(cache, 5, 20) ;
105  interval_data = cache.getInterval(ros::Time().fromSec(5), ros::Time().fromSec(35)) ;
106  EXPECT_EQ(interval_data.size(), (unsigned int) 0) ;
107 }
108 
109 TEST(Cache, easySurroundingInterval)
110 {
111  Cache<Msg> cache(10);
112  fillCacheEasy(cache, 1, 6);
113 
114  vector<boost::shared_ptr<Msg const> > interval_data;
115  interval_data = cache.getSurroundingInterval(ros::Time(15,0), ros::Time(35,0)) ;
116  ASSERT_EQ(interval_data.size(), (unsigned int) 4);
117  EXPECT_EQ(interval_data[0]->data, 1);
118  EXPECT_EQ(interval_data[1]->data, 2);
119  EXPECT_EQ(interval_data[2]->data, 3);
120  EXPECT_EQ(interval_data[3]->data, 4);
121 
122  interval_data = cache.getSurroundingInterval(ros::Time(0,0), ros::Time(35,0)) ;
123  ASSERT_EQ(interval_data.size(), (unsigned int) 4);
124  EXPECT_EQ(interval_data[0]->data, 1);
125 
126  interval_data = cache.getSurroundingInterval(ros::Time(35,0), ros::Time(35,0)) ;
127  ASSERT_EQ(interval_data.size(), (unsigned int) 2);
128  EXPECT_EQ(interval_data[0]->data, 3);
129  EXPECT_EQ(interval_data[1]->data, 4);
130 
131  interval_data = cache.getSurroundingInterval(ros::Time(55,0), ros::Time(55,0)) ;
132  ASSERT_EQ(interval_data.size(), (unsigned int) 1);
133  EXPECT_EQ(interval_data[0]->data, 5);
134 }
135 
136 
138 {
139  Msg* msg = new Msg ;
140  msg->data = data ;
141  msg->header.stamp.fromSec(time) ;
142 
143  boost::shared_ptr<Msg const> msg_ptr(msg) ;
144  return msg_ptr ;
145 }
146 
147 TEST(Cache, easyUnsorted)
148 {
149  Cache<Msg> cache(10) ;
150 
151  cache.add(buildMsg(10.0, 1)) ;
152  cache.add(buildMsg(30.0, 3)) ;
153  cache.add(buildMsg(70.0, 7)) ;
154  cache.add(buildMsg( 5.0, 0)) ;
155  cache.add(buildMsg(20.0, 2)) ;
156 
157  vector<boost::shared_ptr<Msg const> > interval_data = cache.getInterval(ros::Time().fromSec(3), ros::Time().fromSec(15)) ;
158 
159  ASSERT_EQ(interval_data.size(), (unsigned int) 2) ;
160  EXPECT_EQ(interval_data[0]->data, 0) ;
161  EXPECT_EQ(interval_data[1]->data, 1) ;
162 
163  // Grab all the data
164  interval_data = cache.getInterval(ros::Time().fromSec(0), ros::Time().fromSec(80)) ;
165  ASSERT_EQ(interval_data.size(), (unsigned int) 5) ;
166  EXPECT_EQ(interval_data[0]->data, 0) ;
167  EXPECT_EQ(interval_data[1]->data, 1) ;
168  EXPECT_EQ(interval_data[2]->data, 2) ;
169  EXPECT_EQ(interval_data[3]->data, 3) ;
170  EXPECT_EQ(interval_data[4]->data, 7) ;
171 }
172 
173 
174 TEST(Cache, easyElemBeforeAfter)
175 {
176  Cache<Msg> cache(10) ;
178 
179  fillCacheEasy(cache, 5, 10) ;
180 
181  elem_ptr = cache.getElemAfterTime( ros::Time().fromSec(85.0)) ;
182 
183  ASSERT_FALSE(!elem_ptr) ;
184  EXPECT_EQ(elem_ptr->data, 9) ;
185 
186  elem_ptr = cache.getElemBeforeTime( ros::Time().fromSec(85.0)) ;
187  ASSERT_FALSE(!elem_ptr) ;
188  EXPECT_EQ(elem_ptr->data, 8) ;
189 
190  elem_ptr = cache.getElemBeforeTime( ros::Time().fromSec(45.0)) ;
191  EXPECT_TRUE(!elem_ptr) ;
192 }
193 
195 {
196 public:
198  {
199  event_ = evt;
200  }
201 
203 };
204 
205 TEST(Cache, eventInEventOut)
206 {
207  Cache<Msg> c0(10);
208  Cache<Msg> c1(c0, 10);
209  EventHelper h;
211 
212  ros::MessageEvent<Msg const> evt(boost::make_shared<Msg const>(), ros::Time(4));
213  c0.add(evt);
214 
215  EXPECT_EQ(h.event_.getReceiptTime(), evt.getReceiptTime());
216  EXPECT_EQ(h.event_.getMessage(), evt.getMessage());
217 }
218 
219 int main(int argc, char **argv){
220  testing::InitGoogleTest(&argc, argv);
221  ros::init(argc, argv, "blah");
222  ros::Time::init();
223  return RUN_ALL_TESTS();
224 }
225 
boost::shared_ptr< Msg const > MsgConstPtr
Header header
boost::shared_ptr< M > getMessage() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
MConstPtr getElemBeforeTime(const ros::Time &time) const
Grab the newest element that occurs right before the specified time.
Definition: cache.h:239
ros::MessageEvent< Msg const > event_
MConstPtr getElemAfterTime(const ros::Time &time) const
Grab the oldest element that occurs right after the specified time.
Definition: cache.h:267
static ros::Time value(const Msg &m)
Time & fromSec(double t)
std::vector< MConstPtr > getSurroundingInterval(const ros::Time &start, const ros::Time &end) const
Retrieve the smallest interval of messages that surrounds an interval from start to end...
Definition: cache.h:205
int main(int argc, char **argv)
static void init()
roscpp::Logger Msg
void cb(const ros::MessageEvent< Msg const > &evt)
boost::shared_ptr< Msg const > buildMsg(double time, int data)
std::vector< MConstPtr > getInterval(const ros::Time &start, const ros::Time &end) const
Receive a vector of messages that occur between a start and end time (inclusive). ...
Definition: cache.h:166
ros::Time getReceiptTime() const
Stores a time history of messages.
Definition: cache.h:66
void fillCacheEasy(Cache< Msg > &cache, unsigned int start, unsigned int end)
TEST(Cache, easyInterval)
ros::Time stamp
void add(const MConstPtr &msg)
Add a message to the cache, and pop off any elements that are too old. This method is registered with...
Definition: cache.h:119
Connection registerCallback(const C &callback)
Register a callback to be called when this filter has passed.
Definition: simple_filter.h:73


message_filters
Author(s): Josh Faust, Vijay Pradeep
autogenerated on Wed Oct 16 2019 03:27:59