test_trigger_matcher.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2009-2010, 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 
36 #include <gtest/gtest.h>
37 #include <queue>
38 
40 
42 {
43  std::queue< std::pair<double, int> > incoming_;
44 
45 public:
47 
48  QueuedChecker() : tm(1, 10, 5)
49  {
50  tm.setTrigDelay(0.2);
52  tm.setMatchCallback(bound_cb);
53 
54  // Get it into the permanent regime with an empty queue.
55  tm.triggerCallback(0);
56  expectEmpty(0);
57  tm.dataCallback(1, 1);
58  expectEmpty(1);
59  tm.triggerCallback(2);
60  expectHead(0.2, 1);
61  expectEmpty(2);
62  tm.dataCallback(3, 2);
63  expectHead(2.2, 2);
64  expectEmpty(3);
65  }
66 
67  void expectEmpty(double time)
68  {
69  EXPECT_TRUE(incoming_.empty()) << "QueuedChecker queue not empty at time " << time
70  << " contains " << incoming_.front().first << ", " << incoming_.front().second;
71  }
72 
73  void expectHead(double time, int data)
74  {
75  if (incoming_.empty())
76  {
77  ADD_FAILURE() << "QueuedChecker queue empty when checking " << time << ", " << data;
78  return;
79  }
80 
81  std::pair<double, int> &head = incoming_.front();
82 
83  EXPECT_EQ(time, head.first) << "Timestamp mismatch when checking " << time << ", " << data;
84  EXPECT_EQ(data, head.second) << "Data mismatch when checking " << time << ", " << data;
85 
86  incoming_.pop();
87  }
88 
89  void callback(const ros::Time &time, const boost::shared_ptr<int const> &data)
90  {
91  incoming_.push(std::pair<double, int>(time.toSec(), *data));
92  }
93 };
94 
95 TEST(QueuedTriggerMatcher, BasicFunctionality)
96 {
97  QueuedChecker c;
98 
99 // Data gets delayed...
100  c.tm.triggerCallback(4);
101  c.expectEmpty(4);
102  c.tm.triggerCallback(6);
103  c.expectEmpty(6);
104  c.tm.triggerCallback(8);
105  c.expectEmpty(8);
106  c.tm.dataCallback(5, 3);
107  c.expectHead(4.2, 3);
108  c.expectEmpty(5);
109  c.tm.dataCallback(7, 4);
110  c.expectHead(6.2, 4);
111  c.expectEmpty(7);
112  c.tm.dataCallback(9, 5);
113  c.expectHead(8.2, 5);
114  c.expectEmpty(9);
115 
116 // Timestamp gets delayed...
117  c.tm.dataCallback(11, 6);
118  c.expectEmpty(11);
119  c.tm.dataCallback(13, 7);
120  c.expectEmpty(13);
121  c.tm.dataCallback(15, 8);
122  c.expectEmpty(15);
123  c.tm.triggerCallback(10);
124  c.expectHead(10.2, 6);
125  c.expectEmpty(10);
126  c.tm.triggerCallback(12);
127  c.expectHead(12.2, 7);
128  c.expectEmpty(12);
129  c.tm.triggerCallback(14);
130  c.expectHead(14.2, 8);
131  c.expectEmpty(14);
132 }
133 
134 TEST(QueuedTriggerMatcher, TestReset)
135 {
136  QueuedChecker c;
137 
138  c.tm.triggerCallback(4);
139  c.expectEmpty(4);
140  c.tm.reset();
141  c.tm.dataCallback(5, 3);
142  c.expectEmpty(5);
143  c.tm.triggerCallback(6);
144  c.expectEmpty(6);
145  c.tm.dataCallback(7, 4);
146  c.expectEmpty(7);
147  c.tm.triggerCallback(8);
148  c.expectHead(6.2, 4);
149  c.expectEmpty(8);
150  c.tm.dataCallback(9, 5);
151  c.expectHead(8.2, 5);
152  c.expectEmpty(9);
153 
154  c.tm.dataCallback(5, 3);
155  c.expectEmpty(5);
156  c.tm.reset();
157  c.expectEmpty(5.1);
158  c.tm.triggerCallback(4);
159  c.expectEmpty(4);
160  c.tm.triggerCallback(6);
161  c.expectEmpty(6);
162  c.tm.dataCallback(7, 4);
163  c.expectEmpty(7);
164  c.tm.triggerCallback(8);
165  c.expectHead(6.2, 4);
166  c.expectEmpty(8);
167  c.tm.dataCallback(9, 5);
168  c.expectHead(8.2, 5);
169  c.expectEmpty(9);
170 }
171 
172 TEST(QueuedTriggerMatcher, MissingTrigger)
173 {
174  QueuedChecker c;
175 
176  // Miss a trigger at time 4...
177  c.tm.triggerCallback(6);
178  c.expectEmpty(6);
179  c.tm.triggerCallback(8);
180  c.expectEmpty(8);
181  c.tm.dataCallback(5, 3);
182  c.expectEmpty(5);
183  c.tm.dataCallback(7, 4);
184  c.expectHead(6.2, 4);
185  c.expectEmpty(7);
186  c.tm.dataCallback(9, 5);
187  c.expectHead(8.2, 5);
188  c.expectEmpty(9);
189 }
190 
191 TEST(QueuedTriggerMatcher, MissingData)
192 {
193  QueuedChecker c;
194 
195  // Miss data at time 5...
196  c.tm.triggerCallback(4);
197  c.expectEmpty(4);
198  c.tm.triggerCallback(6);
199  c.expectEmpty(6);
200  c.tm.triggerCallback(8);
201  c.expectEmpty(8);
202  c.tm.triggerCallback(10);
203  c.expectEmpty(10);
204  c.tm.triggerCallback(12);
205  c.expectEmpty(12);
206  c.tm.dataCallback(7, 4);
207  c.expectHead(4.2, 4); // Bad
208  c.expectEmpty(7);
209  c.tm.dataCallback(9, 5);
210  c.expectHead(8.2, 5); // Recovered
211  c.expectEmpty(9);
212  c.tm.dataCallback(11, 6);
213  c.expectHead(10.2, 6);
214  c.expectEmpty(11);
215  c.tm.dataCallback(13, 7);
216  c.expectHead(12.2, 7);
217  c.expectEmpty(13);
218 }
219 
220 TEST(QueuedTriggerMatcher, MissingDataZeroLateTolerance)
221 {
222  QueuedChecker c;
223 
225 
226  // Miss data at time 5...
227  c.tm.triggerCallback(4);
228  c.expectEmpty(4);
229  c.tm.triggerCallback(6);
230  c.expectEmpty(6);
231  c.tm.triggerCallback(8);
232  c.expectEmpty(8);
233  c.tm.triggerCallback(10);
234  c.expectEmpty(10);
235  c.tm.triggerCallback(12);
236  c.expectEmpty(12);
237  c.tm.triggerCallback(14);
238  c.expectEmpty(14);
239  c.tm.dataCallback(5, 4);
240  c.expectHead(4.2, 4);
241  c.expectEmpty(5);
242  c.tm.dataCallback(9, 5);
243  c.expectHead(8.2, 5); // Recovered
244  c.expectEmpty(9);
245  c.tm.dataCallback(11, 6);
246  c.expectHead(10.2, 6);
247  c.expectEmpty(11);
248  c.tm.dataCallback(13, 7);
249  c.expectHead(12.2, 7);
250  c.expectEmpty(13);
251 }
252 
253 TEST(QueuedTriggerMatcher, TriggerQueueOverflow)
254 {
255  QueuedChecker c;
256 
257  double trig_time = 4;
258  for (int i = 0; i < 15; i++)
259  {
260  c.tm.triggerCallback(trig_time);
261  c.expectEmpty(trig_time);
262  trig_time += 2;
263  }
264  c.tm.dataCallback(15, 15);
265  c.expectHead(14.2, 15); // Previous triggers were dropped
266  c.expectEmpty(15);
267 }
268 
269 TEST(QueuedTriggerMatcher, DataQueueOverflow)
270 {
271  QueuedChecker c;
272 
273  double data_time = 5;
274  for (int i = 0; i < 10; i++)
275  {
276  c.tm.dataCallback(data_time, data_time);
277  c.expectEmpty(data_time);
278  data_time += 2;
279  }
280  c.tm.triggerCallback(14);
281  c.expectHead(14.2, 15); // Previous triggers were dropped
282  c.expectEmpty(14);
283 }
284 
285 TEST(TriggerMatcher, TimeoutCheck)
286 {
288 
289  // If this does not return because the timestamp
291 }
292 
293 TEST(TriggerMatcher, TriggerFirstCheck)
294 {
296 
297  tm.triggerCallback(1);
298  tm.triggerCallback(2);
299  tm.triggerCallback(3);
300 
301  EXPECT_EQ(ros::Time(1), tm.getTimestampBlocking(ros::Time(1.5), 1)) << "Testing getTimestampBlocking without timeout";
302  EXPECT_EQ(ros::Time(2), tm.getTimestampBlocking(ros::Time(2.5))) << "Testing getTimestampBlocking with timeout";
303 }
304 
305 TEST(TriggerMatcher, TestReset)
306 {
308 
309  tm.triggerCallback(1);
310  tm.triggerCallback(2);
311  tm.reset();
312  tm.triggerCallback(3);
313  tm.triggerCallback(4);
314 
315  EXPECT_EQ(timestamp_tools::TriggerMatcher::DropData, tm.getTimestampBlocking(ros::Time(1.5), 1)) << "Testing getTimestampBlocking without timeout";
316  EXPECT_EQ(ros::Time(3), tm.getTimestampBlocking(ros::Time(3.5))) << "Testing getTimestampBlocking with timeout";
317 }
318 
319 
320 void AsyncGenTrigger(timestamp_tools::TriggerMatcher *tm, double time, int delay)
321 {
322  sleep(delay);
323  tm->triggerCallback(time);
324 }
325 
326 TEST(TriggerMatcher, DataFirstCheck)
327 {
329 
330  tm.triggerCallback(5);
331  boost::function<void(void)> agt = boost::bind(&AsyncGenTrigger, &tm, 7.0, 2);
332  boost::thread trigger_thread(agt);
333 
334  EXPECT_EQ(timestamp_tools::TriggerMatcher::RetryLater, tm.getTimestampBlocking(ros::Time(5.5), 0.5)) << "getTimestampBlocking should have timed out or test computer is VERY slow";
335  EXPECT_EQ(ros::Time(5), tm.getTimestampBlocking(ros::Time(6.0))) << "getTimestampBlocking should have received timestamp";
336 
337  trigger_thread.join();
338 }
339 
340 
341 int main(int argc, char **argv){
342  for (int i = 0; i < argc; i++)
343  printf("%s\n", argv[i]);
344  testing::InitGoogleTest(&argc, argv);
345  return RUN_ALL_TESTS();
346 }
TEST(QueuedTriggerMatcher, BasicFunctionality)
int main(int argc, char **argv)
void expectEmpty(double time)
void callback(const ros::Time &time, const boost::shared_ptr< int const > &data)
void dataCallback(double stamp, const C &data)
std::queue< std::pair< double, int > > incoming_
timestamp_tools::QueuedTriggerMatcher< int > tm
boost::function< void(const ros::Time &, boost::shared_ptr< C const > &)> MatchCallback
void AsyncGenTrigger(timestamp_tools::TriggerMatcher *tm, double time, int delay)
ros::Time getTimestampBlocking(const ros::Time &t)
void expectHead(double time, int data)
void setMatchCallback(MatchCallback &cb)
void setLateDataCountAllowed(unsigned int v)


timestamp_tools
Author(s): Blaise Gassend
autogenerated on Thu Apr 11 2019 02:07:22