test.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * C++ unit test for ApproximateTime.h
3  *********************************************************************/
4 
5 #include <gtest/gtest.h>
6 
7 // File under test
8 #include <dataspeed_can_msg_filters/ApproximateTime.h>
9 using namespace dataspeed_can_msg_filters;
10 
11 TEST(ApproxTimeSync, ValidId_PostMask)
12 {
13  // Standard IDs
14  ASSERT_TRUE (ApproximateTime::ValidId(0x000007FFu));
15  ASSERT_TRUE (ApproximateTime::ValidId(0x00000000u));
16  ASSERT_TRUE (ApproximateTime::ValidId(0x00000001u));
17  ASSERT_TRUE (ApproximateTime::ValidId(0x00000002u));
18  ASSERT_TRUE (ApproximateTime::ValidId(0x00000004u));
19  ASSERT_TRUE (ApproximateTime::ValidId(0x00000008u));
20  ASSERT_TRUE (ApproximateTime::ValidId(0x00000010u));
21  ASSERT_TRUE (ApproximateTime::ValidId(0x00000020u));
22  ASSERT_TRUE (ApproximateTime::ValidId(0x00000040u));
23  ASSERT_TRUE (ApproximateTime::ValidId(0x00000080u));
24  ASSERT_TRUE (ApproximateTime::ValidId(0x00000010u));
25  ASSERT_TRUE (ApproximateTime::ValidId(0x00000020u));
26  ASSERT_TRUE (ApproximateTime::ValidId(0x00000040u));
27  ASSERT_TRUE (ApproximateTime::ValidId(0x00000080u));
28  ASSERT_TRUE (ApproximateTime::ValidId(0x00000100u));
29  ASSERT_TRUE (ApproximateTime::ValidId(0x00000200u));
30  ASSERT_TRUE (ApproximateTime::ValidId(0x00000400u));
31  ASSERT_FALSE(ApproximateTime::ValidId(0x00000800u));
32  ASSERT_FALSE(ApproximateTime::ValidId(0x00001000u));
33  ASSERT_FALSE(ApproximateTime::ValidId(0x00002000u));
34  ASSERT_FALSE(ApproximateTime::ValidId(0x00004000u));
35  ASSERT_FALSE(ApproximateTime::ValidId(0x00008000u));
36  ASSERT_FALSE(ApproximateTime::ValidId(0x00010000u));
37  ASSERT_FALSE(ApproximateTime::ValidId(0x00020000u));
38  ASSERT_FALSE(ApproximateTime::ValidId(0x00040000u));
39  ASSERT_FALSE(ApproximateTime::ValidId(0x00080000u));
40  ASSERT_FALSE(ApproximateTime::ValidId(0x00010000u));
41  ASSERT_FALSE(ApproximateTime::ValidId(0x00020000u));
42  ASSERT_FALSE(ApproximateTime::ValidId(0x00040000u));
43  ASSERT_FALSE(ApproximateTime::ValidId(0x00080000u));
44  ASSERT_FALSE(ApproximateTime::ValidId(0x00100000u));
45  ASSERT_FALSE(ApproximateTime::ValidId(0x00200000u));
46  ASSERT_FALSE(ApproximateTime::ValidId(0x00400000u));
47  ASSERT_FALSE(ApproximateTime::ValidId(0x00800000u));
48  ASSERT_FALSE(ApproximateTime::ValidId(0x01000000u));
49  ASSERT_FALSE(ApproximateTime::ValidId(0x02000000u));
50  ASSERT_FALSE(ApproximateTime::ValidId(0x04000000u));
51  ASSERT_FALSE(ApproximateTime::ValidId(0x08000000u));
52  ASSERT_FALSE(ApproximateTime::ValidId(0x10000000u));
53  ASSERT_FALSE(ApproximateTime::ValidId(0x20000000u));
54  ASSERT_FALSE(ApproximateTime::ValidId(0x40000000u));
55 
56  // Extended IDs
57  ASSERT_TRUE (ApproximateTime::ValidId(0x9FFFFFFFu));
58  ASSERT_TRUE (ApproximateTime::ValidId(0x80000000u));
59  ASSERT_TRUE (ApproximateTime::ValidId(0x80000001u));
60  ASSERT_TRUE (ApproximateTime::ValidId(0x80000002u));
61  ASSERT_TRUE (ApproximateTime::ValidId(0x80000004u));
62  ASSERT_TRUE (ApproximateTime::ValidId(0x80000008u));
63  ASSERT_TRUE (ApproximateTime::ValidId(0x80000010u));
64  ASSERT_TRUE (ApproximateTime::ValidId(0x80000020u));
65  ASSERT_TRUE (ApproximateTime::ValidId(0x80000040u));
66  ASSERT_TRUE (ApproximateTime::ValidId(0x80000080u));
67  ASSERT_TRUE (ApproximateTime::ValidId(0x80000010u));
68  ASSERT_TRUE (ApproximateTime::ValidId(0x80000020u));
69  ASSERT_TRUE (ApproximateTime::ValidId(0x80000040u));
70  ASSERT_TRUE (ApproximateTime::ValidId(0x80000080u));
71  ASSERT_TRUE (ApproximateTime::ValidId(0x80000100u));
72  ASSERT_TRUE (ApproximateTime::ValidId(0x80000200u));
73  ASSERT_TRUE (ApproximateTime::ValidId(0x80000400u));
74  ASSERT_TRUE (ApproximateTime::ValidId(0x80000800u));
75  ASSERT_TRUE (ApproximateTime::ValidId(0x80001000u));
76  ASSERT_TRUE (ApproximateTime::ValidId(0x80002000u));
77  ASSERT_TRUE (ApproximateTime::ValidId(0x80004000u));
78  ASSERT_TRUE (ApproximateTime::ValidId(0x80008000u));
79  ASSERT_TRUE (ApproximateTime::ValidId(0x80010000u));
80  ASSERT_TRUE (ApproximateTime::ValidId(0x80020000u));
81  ASSERT_TRUE (ApproximateTime::ValidId(0x80040000u));
82  ASSERT_TRUE (ApproximateTime::ValidId(0x80080000u));
83  ASSERT_TRUE (ApproximateTime::ValidId(0x80010000u));
84  ASSERT_TRUE (ApproximateTime::ValidId(0x80020000u));
85  ASSERT_TRUE (ApproximateTime::ValidId(0x80040000u));
86  ASSERT_TRUE (ApproximateTime::ValidId(0x80080000u));
87  ASSERT_TRUE (ApproximateTime::ValidId(0x80100000u));
88  ASSERT_TRUE (ApproximateTime::ValidId(0x80200000u));
89  ASSERT_TRUE (ApproximateTime::ValidId(0x80400000u));
90  ASSERT_TRUE (ApproximateTime::ValidId(0x80800000u));
91  ASSERT_TRUE (ApproximateTime::ValidId(0x81000000u));
92  ASSERT_TRUE (ApproximateTime::ValidId(0x82000000u));
93  ASSERT_TRUE (ApproximateTime::ValidId(0x84000000u));
94  ASSERT_TRUE (ApproximateTime::ValidId(0x88000000u));
95  ASSERT_TRUE (ApproximateTime::ValidId(0x90000000u));
96  ASSERT_FALSE(ApproximateTime::ValidId(0xA0000000u));
97  ASSERT_FALSE(ApproximateTime::ValidId(0xC0000000u));
98 }
99 
100 TEST(ApproxTimeSync, ValidId_PreMask)
101 {
102  // Standard IDs
103  ASSERT_TRUE (ApproximateTime::ValidId(0x000007FFu, false));
104  ASSERT_TRUE (ApproximateTime::ValidId(0x00000000u, false));
105  ASSERT_TRUE (ApproximateTime::ValidId(0x00000001u, false));
106  ASSERT_TRUE (ApproximateTime::ValidId(0x00000002u, false));
107  ASSERT_TRUE (ApproximateTime::ValidId(0x00000004u, false));
108  ASSERT_TRUE (ApproximateTime::ValidId(0x00000008u, false));
109  ASSERT_TRUE (ApproximateTime::ValidId(0x00000010u, false));
110  ASSERT_TRUE (ApproximateTime::ValidId(0x00000020u, false));
111  ASSERT_TRUE (ApproximateTime::ValidId(0x00000040u, false));
112  ASSERT_TRUE (ApproximateTime::ValidId(0x00000080u, false));
113  ASSERT_TRUE (ApproximateTime::ValidId(0x00000010u, false));
114  ASSERT_TRUE (ApproximateTime::ValidId(0x00000020u, false));
115  ASSERT_TRUE (ApproximateTime::ValidId(0x00000040u, false));
116  ASSERT_TRUE (ApproximateTime::ValidId(0x00000080u, false));
117  ASSERT_TRUE (ApproximateTime::ValidId(0x00000100u, false));
118  ASSERT_TRUE (ApproximateTime::ValidId(0x00000200u, false));
119  ASSERT_TRUE (ApproximateTime::ValidId(0x00000400u, false));
120  ASSERT_FALSE(ApproximateTime::ValidId(0x00000800u, false));
121  ASSERT_FALSE(ApproximateTime::ValidId(0x00001000u, false));
122  ASSERT_FALSE(ApproximateTime::ValidId(0x00002000u, false));
123  ASSERT_FALSE(ApproximateTime::ValidId(0x00004000u, false));
124  ASSERT_FALSE(ApproximateTime::ValidId(0x00008000u, false));
125  ASSERT_FALSE(ApproximateTime::ValidId(0x00010000u, false));
126  ASSERT_FALSE(ApproximateTime::ValidId(0x00020000u, false));
127  ASSERT_FALSE(ApproximateTime::ValidId(0x00040000u, false));
128  ASSERT_FALSE(ApproximateTime::ValidId(0x00080000u, false));
129  ASSERT_FALSE(ApproximateTime::ValidId(0x00010000u, false));
130  ASSERT_FALSE(ApproximateTime::ValidId(0x00020000u, false));
131  ASSERT_FALSE(ApproximateTime::ValidId(0x00040000u, false));
132  ASSERT_FALSE(ApproximateTime::ValidId(0x00080000u, false));
133  ASSERT_FALSE(ApproximateTime::ValidId(0x00100000u, false));
134  ASSERT_FALSE(ApproximateTime::ValidId(0x00200000u, false));
135  ASSERT_FALSE(ApproximateTime::ValidId(0x00400000u, false));
136  ASSERT_FALSE(ApproximateTime::ValidId(0x00800000u, false));
137  ASSERT_FALSE(ApproximateTime::ValidId(0x01000000u, false));
138  ASSERT_FALSE(ApproximateTime::ValidId(0x02000000u, false));
139  ASSERT_FALSE(ApproximateTime::ValidId(0x04000000u, false));
140  ASSERT_FALSE(ApproximateTime::ValidId(0x08000000u, false));
141  ASSERT_FALSE(ApproximateTime::ValidId(0x10000000u, false));
142  ASSERT_FALSE(ApproximateTime::ValidId(0x20000000u, false));
143  ASSERT_FALSE(ApproximateTime::ValidId(0x40000000u, false));
144  ASSERT_FALSE(ApproximateTime::ValidId(0x80000000u, false));
145 
146  // Extended IDs
147  ASSERT_TRUE (ApproximateTime::ValidId(0x1FFFFFFFu, true));
148  ASSERT_TRUE (ApproximateTime::ValidId(0x00000000u, true));
149  ASSERT_TRUE (ApproximateTime::ValidId(0x00000001u, true));
150  ASSERT_TRUE (ApproximateTime::ValidId(0x00000002u, true));
151  ASSERT_TRUE (ApproximateTime::ValidId(0x00000004u, true));
152  ASSERT_TRUE (ApproximateTime::ValidId(0x00000008u, true));
153  ASSERT_TRUE (ApproximateTime::ValidId(0x00000010u, true));
154  ASSERT_TRUE (ApproximateTime::ValidId(0x00000020u, true));
155  ASSERT_TRUE (ApproximateTime::ValidId(0x00000040u, true));
156  ASSERT_TRUE (ApproximateTime::ValidId(0x00000080u, true));
157  ASSERT_TRUE (ApproximateTime::ValidId(0x00000010u, true));
158  ASSERT_TRUE (ApproximateTime::ValidId(0x00000020u, true));
159  ASSERT_TRUE (ApproximateTime::ValidId(0x00000040u, true));
160  ASSERT_TRUE (ApproximateTime::ValidId(0x00000080u, true));
161  ASSERT_TRUE (ApproximateTime::ValidId(0x00000100u, true));
162  ASSERT_TRUE (ApproximateTime::ValidId(0x00000200u, true));
163  ASSERT_TRUE (ApproximateTime::ValidId(0x00000400u, true));
164  ASSERT_TRUE (ApproximateTime::ValidId(0x00000800u, true));
165  ASSERT_TRUE (ApproximateTime::ValidId(0x00001000u, true));
166  ASSERT_TRUE (ApproximateTime::ValidId(0x00002000u, true));
167  ASSERT_TRUE (ApproximateTime::ValidId(0x00004000u, true));
168  ASSERT_TRUE (ApproximateTime::ValidId(0x00008000u, true));
169  ASSERT_TRUE (ApproximateTime::ValidId(0x00010000u, true));
170  ASSERT_TRUE (ApproximateTime::ValidId(0x00020000u, true));
171  ASSERT_TRUE (ApproximateTime::ValidId(0x00040000u, true));
172  ASSERT_TRUE (ApproximateTime::ValidId(0x00080000u, true));
173  ASSERT_TRUE (ApproximateTime::ValidId(0x00010000u, true));
174  ASSERT_TRUE (ApproximateTime::ValidId(0x00020000u, true));
175  ASSERT_TRUE (ApproximateTime::ValidId(0x00040000u, true));
176  ASSERT_TRUE (ApproximateTime::ValidId(0x00080000u, true));
177  ASSERT_TRUE (ApproximateTime::ValidId(0x00100000u, true));
178  ASSERT_TRUE (ApproximateTime::ValidId(0x00200000u, true));
179  ASSERT_TRUE (ApproximateTime::ValidId(0x00400000u, true));
180  ASSERT_TRUE (ApproximateTime::ValidId(0x00800000u, true));
181  ASSERT_TRUE (ApproximateTime::ValidId(0x01000000u, true));
182  ASSERT_TRUE (ApproximateTime::ValidId(0x02000000u, true));
183  ASSERT_TRUE (ApproximateTime::ValidId(0x04000000u, true));
184  ASSERT_TRUE (ApproximateTime::ValidId(0x08000000u, true));
185  ASSERT_TRUE (ApproximateTime::ValidId(0x10000000u, true));
186  ASSERT_FALSE(ApproximateTime::ValidId(0x20000000u, true));
187  ASSERT_FALSE(ApproximateTime::ValidId(0x40000000u, true));
188  ASSERT_FALSE(ApproximateTime::ValidId(0x80000000u, true));
189 }
190 
191 TEST(ApproxTimeSync, BuildId)
192 {
193  // Standard IDs
194  ASSERT_EQ(0x000007FFu, ApproximateTime::BuildId(0x000007FFu, false));
195  ASSERT_EQ(0x000007FFu, ApproximateTime::BuildId(0xFFFFFFFFu, false));
196  ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x00000000u, false));
197  ASSERT_EQ(0x00000001u, ApproximateTime::BuildId(0x00000001u, false));
198  ASSERT_EQ(0x00000002u, ApproximateTime::BuildId(0x00000002u, false));
199  ASSERT_EQ(0x00000004u, ApproximateTime::BuildId(0x00000004u, false));
200  ASSERT_EQ(0x00000008u, ApproximateTime::BuildId(0x00000008u, false));
201  ASSERT_EQ(0x00000010u, ApproximateTime::BuildId(0x00000010u, false));
202  ASSERT_EQ(0x00000020u, ApproximateTime::BuildId(0x00000020u, false));
203  ASSERT_EQ(0x00000040u, ApproximateTime::BuildId(0x00000040u, false));
204  ASSERT_EQ(0x00000080u, ApproximateTime::BuildId(0x00000080u, false));
205  ASSERT_EQ(0x00000100u, ApproximateTime::BuildId(0x00000100u, false));
206  ASSERT_EQ(0x00000200u, ApproximateTime::BuildId(0x00000200u, false));
207  ASSERT_EQ(0x00000400u, ApproximateTime::BuildId(0x00000400u, false));
208  ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x00000800u, false));
209  ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x00001000u, false));
210  ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x00002000u, false));
211  ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x00004000u, false));
212  ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x00008000u, false));
213  ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x00010000u, false));
214  ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x00020000u, false));
215  ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x00040000u, false));
216  ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x00080000u, false));
217  ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x00100000u, false));
218  ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x00200000u, false));
219  ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x00400000u, false));
220  ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x00800000u, false));
221  ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x01000000u, false));
222  ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x02000000u, false));
223  ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x04000000u, false));
224  ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x08000000u, false));
225  ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x10000000u, false));
226  ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x20000000u, false));
227  ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x40000000u, false));
228  ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x80000000u, false));
229 
230  // Extended IDs
231  ASSERT_EQ(0x9FFFFFFFu, ApproximateTime::BuildId(0x1FFFFFFFu, true));
232  ASSERT_EQ(0x9FFFFFFFu, ApproximateTime::BuildId(0xFFFFFFFFu, true));
233  ASSERT_EQ(0x80000000u, ApproximateTime::BuildId(0x00000000u, true));
234  ASSERT_EQ(0x80000001u, ApproximateTime::BuildId(0x00000001u, true));
235  ASSERT_EQ(0x80000002u, ApproximateTime::BuildId(0x00000002u, true));
236  ASSERT_EQ(0x80000004u, ApproximateTime::BuildId(0x00000004u, true));
237  ASSERT_EQ(0x80000008u, ApproximateTime::BuildId(0x00000008u, true));
238  ASSERT_EQ(0x80000010u, ApproximateTime::BuildId(0x00000010u, true));
239  ASSERT_EQ(0x80000020u, ApproximateTime::BuildId(0x00000020u, true));
240  ASSERT_EQ(0x80000040u, ApproximateTime::BuildId(0x00000040u, true));
241  ASSERT_EQ(0x80000080u, ApproximateTime::BuildId(0x00000080u, true));
242  ASSERT_EQ(0x80000100u, ApproximateTime::BuildId(0x00000100u, true));
243  ASSERT_EQ(0x80000200u, ApproximateTime::BuildId(0x00000200u, true));
244  ASSERT_EQ(0x80000400u, ApproximateTime::BuildId(0x00000400u, true));
245  ASSERT_EQ(0x80000800u, ApproximateTime::BuildId(0x00000800u, true));
246  ASSERT_EQ(0x80001000u, ApproximateTime::BuildId(0x00001000u, true));
247  ASSERT_EQ(0x80002000u, ApproximateTime::BuildId(0x00002000u, true));
248  ASSERT_EQ(0x80004000u, ApproximateTime::BuildId(0x00004000u, true));
249  ASSERT_EQ(0x80008000u, ApproximateTime::BuildId(0x00008000u, true));
250  ASSERT_EQ(0x80010000u, ApproximateTime::BuildId(0x00010000u, true));
251  ASSERT_EQ(0x80020000u, ApproximateTime::BuildId(0x00020000u, true));
252  ASSERT_EQ(0x80040000u, ApproximateTime::BuildId(0x00040000u, true));
253  ASSERT_EQ(0x80080000u, ApproximateTime::BuildId(0x00080000u, true));
254  ASSERT_EQ(0x80100000u, ApproximateTime::BuildId(0x00100000u, true));
255  ASSERT_EQ(0x80200000u, ApproximateTime::BuildId(0x00200000u, true));
256  ASSERT_EQ(0x80400000u, ApproximateTime::BuildId(0x00400000u, true));
257  ASSERT_EQ(0x80800000u, ApproximateTime::BuildId(0x00800000u, true));
258  ASSERT_EQ(0x81000000u, ApproximateTime::BuildId(0x01000000u, true));
259  ASSERT_EQ(0x82000000u, ApproximateTime::BuildId(0x02000000u, true));
260  ASSERT_EQ(0x84000000u, ApproximateTime::BuildId(0x04000000u, true));
261  ASSERT_EQ(0x88000000u, ApproximateTime::BuildId(0x08000000u, true));
262  ASSERT_EQ(0x90000000u, ApproximateTime::BuildId(0x10000000u, true));
263  ASSERT_EQ(0x80000000u, ApproximateTime::BuildId(0x20000000u, true));
264  ASSERT_EQ(0x80000000u, ApproximateTime::BuildId(0x40000000u, true));
265  ASSERT_EQ(0x80000000u, ApproximateTime::BuildId(0x80000000u, true));
266 }
267 
268 
269 // Tests ported from https://github.com/ros/ros_comm/blob/2e6ac87958b59455aab0442b205163e9a5f43ff2/utilities/message_filters/test/test_approximate_time_policy.cpp
270 typedef can_msgs::Frame Msg;
271 typedef boost::shared_ptr<Msg> MsgPtr;
272 typedef boost::shared_ptr<Msg const> MsgConstPtr;
273 
274 Msg MsgHelper(ros::Time stamp, uint32_t id, bool is_extended = false, bool is_error = false, bool is_rtr = false) {
275  Msg msg;
276  msg.header.stamp = stamp;
277  msg.id = id;
278  msg.is_extended = is_extended;
279  msg.is_error = is_error;
280  msg.is_rtr = is_rtr;
281  return msg;
282 }
283 
284 typedef std::pair<ros::Time, ros::Time> TimePair;
285 struct TimeQuad
286 {
287  TimeQuad(ros::Time p, ros::Time q, ros::Time r, ros::Time s)
288  {
289  time[0] = p;
290  time[1] = q;
291  time[2] = r;
292  time[3] = s;
293  }
294  ros::Time time[4];
295 };
296 
297 //----------------------------------------------------------
298 // Test Class (for 2 inputs)
299 //----------------------------------------------------------
300 class ApproximateTimeSynchronizerTest
301 {
302 public:
303  ApproximateTimeSynchronizerTest(const std::vector<Msg> &input,
304  const std::vector<TimePair> &output,
305  uint32_t queue_size, uint32_t id1, uint32_t id2) :
306  input_(input), output_(output), output_position_(0),
307  sync_(queue_size, boost::bind(&ApproximateTimeSynchronizerTest::callback, this, _1), id1, id2)
308  {
309  }
310 
311  void callback(const std::vector<can_msgs::Frame::ConstPtr> &msgs)
312  {
313  //printf("Call_back called\n");
314  //printf("Call back: <%f, %f>\n", msgs[0]->header.stamp.toSec(), msgs[1]->header.stamp.toSec());
315  ASSERT_EQ(2u, msgs.size());
316  ASSERT_TRUE(msgs[0]);
317  ASSERT_TRUE(msgs[1]);
318  ASSERT_LT(output_position_, output_.size());
319  EXPECT_EQ(output_[output_position_].first, msgs[0]->header.stamp);
320  EXPECT_EQ(output_[output_position_].second, msgs[1]->header.stamp);
321  ++output_position_;
322  }
323 
324  void run()
325  {
326  for (size_t i = 0; i < input_.size(); i++) {
327  sync_.processMsg(boost::make_shared<Msg>(input_[i]));
328  }
329  //printf("Done running test\n");
330  EXPECT_EQ(output_.size(), output_position_);
331  }
332 
333 private:
334  const std::vector<Msg> &input_;
335  const std::vector<TimePair> &output_;
336  unsigned int output_position_;
337 public:
338  ApproximateTime sync_;
339 };
340 
341 //----------------------------------------------------------
342 // Test Class (for 4 inputs)
343 //----------------------------------------------------------
344 class ApproximateTimeSynchronizerTestQuad
345 {
346 public:
347 
348  ApproximateTimeSynchronizerTestQuad(const std::vector<Msg> &input,
349  const std::vector<TimeQuad> &output,
350  uint32_t queue_size, uint32_t id1, uint32_t id2, uint32_t id3, uint32_t id4) :
351  input_(input), output_(output), output_position_(0),
352  sync_(queue_size, boost::bind(&ApproximateTimeSynchronizerTestQuad::callback, this, _1), id1, id2, id3, id4)
353  {
354  }
355 
356  void callback(const std::vector<can_msgs::Frame::ConstPtr> &msgs)
357  {
358  //printf("Call_back called\n");
359  //printf("Call back: <%f, %f>\n", msgs[0]->header.stamp.toSec(), msgs[1]->header.stamp.toSec());
360  ASSERT_EQ(4u, msgs.size());
361  ASSERT_TRUE(msgs[0]);
362  ASSERT_TRUE(msgs[1]);
363  ASSERT_TRUE(msgs[2]);
364  ASSERT_TRUE(msgs[3]);
365  ASSERT_LT(output_position_, output_.size());
366  EXPECT_EQ(output_[output_position_].time[0], msgs[0]->header.stamp);
367  EXPECT_EQ(output_[output_position_].time[1], msgs[1]->header.stamp);
368  EXPECT_EQ(output_[output_position_].time[2], msgs[2]->header.stamp);
369  EXPECT_EQ(output_[output_position_].time[3], msgs[3]->header.stamp);
370  ++output_position_;
371  }
372 
373  void run()
374  {
375  for (size_t i = 0; i < input_.size(); i++) {
376  sync_.processMsg(boost::make_shared<Msg>(input_[i]));
377  }
378  //printf("Done running test\n");
379  EXPECT_EQ(output_.size(), output_position_);
380  }
381 
382 private:
383  const std::vector<Msg> &input_;
384  const std::vector<TimeQuad> &output_;
385  unsigned int output_position_;
386 public:
387  ApproximateTime sync_;
388 };
389 
390 
391 
392 TEST(ApproxTimeSync, ExactMatch) {
393  // Input A: a..b..c
394  // Input B: A..B..C
395  // Output: a..b..c
396  // A..B..C
397  std::vector<Msg> input;
398  std::vector<TimePair> output;
399 
400  ros::Time t(0, 0);
401  ros::Duration s(1, 0);
402 
403  input.push_back(MsgHelper(t+s*0,0)); // a
404  input.push_back(MsgHelper(t+s*0,1)); // A
405  input.push_back(MsgHelper(t+s*3,0)); // b
406  input.push_back(MsgHelper(t+s*3,1)); // B
407  input.push_back(MsgHelper(t+s*6,0)); // c
408  input.push_back(MsgHelper(t+s*6,1)); // C
409  output.push_back(TimePair(t, t));
410  output.push_back(TimePair(t+s*3, t+s*3));
411  output.push_back(TimePair(t+s*6, t+s*6));
412 
413  ApproximateTimeSynchronizerTest sync_test(input, output, 10, 0, 1);
414  sync_test.run();
415 }
416 
417 TEST(ApproxTimeSync, PerfectMatch) {
418  // Input A: a..b..c.
419  // Input B: .A..B..C
420  // Output: ...a..b.
421  // ...A..B.
422  std::vector<Msg> input;
423  std::vector<TimePair> output;
424 
425  ros::Time t(0, 0);
426  ros::Duration s(1, 0);
427 
428  input.push_back(MsgHelper(t+s*0,0)); // a
429  input.push_back(MsgHelper(t+s*1,1)); // A
430  input.push_back(MsgHelper(t+s*3,0)); // b
431  input.push_back(MsgHelper(t+s*4,1)); // B
432  input.push_back(MsgHelper(t+s*6,0)); // c
433  input.push_back(MsgHelper(t+s*7,1)); // C
434  output.push_back(TimePair(t, t+s));
435  output.push_back(TimePair(t+s*3, t+s*4));
436 
437  ApproximateTimeSynchronizerTest sync_test(input, output, 10, 0, 1);
438  sync_test.run();
439 }
440 
441 TEST(ApproxTimeSync, ImperfectMatch) {
442  // Input A: a.xb..c.
443  // Input B: .A...B.C
444  // Output: ..a...c.
445  // ..A...B.
446  std::vector<Msg> input;
447  std::vector<TimePair> output;
448 
449  ros::Time t(0, 0);
450  ros::Duration s(1, 0);
451 
452  input.push_back(MsgHelper(t+s*0,0)); // a
453  input.push_back(MsgHelper(t+s*1,1)); // A
454  input.push_back(MsgHelper(t+s*2,0)); // x
455  input.push_back(MsgHelper(t+s*3,0)); // b
456  input.push_back(MsgHelper(t+s*5,1)); // B
457  input.push_back(MsgHelper(t+s*6,0)); // c
458  input.push_back(MsgHelper(t+s*7,1)); // C
459  output.push_back(TimePair(t, t+s));
460  output.push_back(TimePair(t+s*6, t+s*5));
461 
462  ApproximateTimeSynchronizerTest sync_test(input, output, 10, 0, 1);
463  sync_test.run();
464 }
465 
466 TEST(ApproxTimeSync, Acceleration) {
467  // Time: 0123456789012345678
468  // Input A: a...........b....c.
469  // Input B: .......A.......B..C
470  // Output: ............b.....c
471  // ............A.....C
472  std::vector<Msg> input;
473  std::vector<TimePair> output;
474 
475  ros::Time t(0, 0);
476  ros::Duration s(1, 0);
477 
478  input.push_back(MsgHelper(t+s*0,0)); // a
479  input.push_back(MsgHelper(t+s*7,1)); // A
480  input.push_back(MsgHelper(t+s*12,0)); // b
481  input.push_back(MsgHelper(t+s*15,1)); // B
482  input.push_back(MsgHelper(t+s*17,0)); // c
483  input.push_back(MsgHelper(t+s*18,1)); // C
484  output.push_back(TimePair(t+s*12, t+s*7));
485  output.push_back(TimePair(t+s*17, t+s*18));
486 
487  ApproximateTimeSynchronizerTest sync_test(input, output, 10, 0, 1);
488  sync_test.run();
489 }
490 
491 TEST(ApproxTimeSync, DroppedMessages) {
492  // Queue size 1 (too small)
493  // Time: 012345678901234
494  // Input A: a...b...c.d..e.
495  // Input B: .A.B...C...D..E
496  // Output: .......b.....d.
497  // .......B.....D.
498  std::vector<Msg> input;
499  std::vector<TimePair> output;
500 
501  ros::Time t(0, 0);
502  ros::Duration s(1, 0);
503 
504  input.push_back(MsgHelper(t+s*0,0)); // a
505  input.push_back(MsgHelper(t+s*1,1)); // A
506  input.push_back(MsgHelper(t+s*3,1)); // B
507  input.push_back(MsgHelper(t+s*4,0)); // b
508  input.push_back(MsgHelper(t+s*7,1)); // C
509  input.push_back(MsgHelper(t+s*8,0)); // c
510  input.push_back(MsgHelper(t+s*10,0)); // d
511  input.push_back(MsgHelper(t+s*11,1)); // D
512  input.push_back(MsgHelper(t+s*13,0)); // e
513  input.push_back(MsgHelper(t+s*14,1)); // E
514  output.push_back(TimePair(t+s*4, t+s*3));
515  output.push_back(TimePair(t+s*10, t+s*11));
516 
517  ApproximateTimeSynchronizerTest sync_test(input, output, 1, 0, 1);
518  sync_test.run();
519 
520  // Queue size 2 (just enough)
521  // Time: 012345678901234
522  // Input A: a...b...c.d..e.
523  // Input B: .A.B...C...D..E
524  // Output: ....a..b...c.d.
525  // ....A..B...C.D.
526  std::vector<TimePair> output2;
527  output2.push_back(TimePair(t, t+s));
528  output2.push_back(TimePair(t+s*4, t+s*3));
529  output2.push_back(TimePair(t+s*8, t+s*7));
530  output2.push_back(TimePair(t+s*10, t+s*11));
531 
532  ApproximateTimeSynchronizerTest sync_test2(input, output2, 2, 0, 1);
533  sync_test2.run();
534 }
535 
536 TEST(ApproxTimeSync, LongQueue) {
537  // Queue size 5
538  // Time: 012345678901234
539  // Input A: abcdefghiklmnp.
540  // Input B: ...j......o....
541  // Output: ..........l....
542  // ..........o....
543  std::vector<Msg> input;
544  std::vector<TimePair> output;
545 
546  ros::Time t(0, 0);
547  ros::Duration s(1, 0);
548 
549  input.push_back(MsgHelper(t+s*0,0)); // a
550  input.push_back(MsgHelper(t+s*1,0)); // b
551  input.push_back(MsgHelper(t+s*2,0)); // c
552  input.push_back(MsgHelper(t+s*3,0)); // d
553  input.push_back(MsgHelper(t+s*4,0)); // e
554  input.push_back(MsgHelper(t+s*5,0)); // f
555  input.push_back(MsgHelper(t+s*6,0)); // g
556  input.push_back(MsgHelper(t+s*7,0)); // h
557  input.push_back(MsgHelper(t+s*8,0)); // i
558  input.push_back(MsgHelper(t+s*3,1)); // j
559  input.push_back(MsgHelper(t+s*9,0)); // k
560  input.push_back(MsgHelper(t+s*10,0)); // l
561  input.push_back(MsgHelper(t+s*11,0)); // m
562  input.push_back(MsgHelper(t+s*12,0)); // n
563  input.push_back(MsgHelper(t+s*10,1)); // o
564  input.push_back(MsgHelper(t+s*13,0)); // l
565  output.push_back(TimePair(t+s*10, t+s*10));
566 
567  ApproximateTimeSynchronizerTest sync_test(input, output, 5, 0, 1);
568  sync_test.run();
569 }
570 
571 TEST(ApproxTimeSync, DoublePublish) {
572  // Input A: a..b
573  // Input B: .A.B
574  // Output: ...b
575  // ...B
576  // +
577  // a
578  // A
579  std::vector<Msg> input;
580  std::vector<TimePair> output;
581 
582  ros::Time t(0, 0);
583  ros::Duration s(1, 0);
584 
585  input.push_back(MsgHelper(t+s*0,0)); // a
586  input.push_back(MsgHelper(t+s*1,1)); // A
587  input.push_back(MsgHelper(t+s*3,1)); // B
588  input.push_back(MsgHelper(t+s*3,0)); // b
589  output.push_back(TimePair(t, t+s));
590  output.push_back(TimePair(t+s*3, t+s*3));
591 
592  ApproximateTimeSynchronizerTest sync_test(input, output, 10, 0, 1);
593  sync_test.run();
594 }
595 
596 TEST(ApproxTimeSync, FourTopics) {
597  // Time: 012345678901234
598  // Input A: a....e..i.m..n.
599  // Input B: .b....g..j....o
600  // Input C: ..c...h...k....
601  // Input D: ...d.f.....l...
602  // Output: ......a....e..m
603  // ......b....g..j
604  // ......c....h..k
605  // ......d....f..l
606  std::vector<Msg> input;
607  std::vector<TimeQuad> output;
608 
609  ros::Time t(0, 0);
610  ros::Duration s(1, 0);
611 
612  input.push_back(MsgHelper(t+s*0,0)); // a
613  input.push_back(MsgHelper(t+s*1,1)); // b
614  input.push_back(MsgHelper(t+s*2,2)); // c
615  input.push_back(MsgHelper(t+s*3,3)); // d
616  input.push_back(MsgHelper(t+s*5,0)); // e
617  input.push_back(MsgHelper(t+s*5,3)); // f
618  input.push_back(MsgHelper(t+s*6,1)); // g
619  input.push_back(MsgHelper(t+s*6,2)); // h
620  input.push_back(MsgHelper(t+s*8,0)); // i
621  input.push_back(MsgHelper(t+s*9,1)); // j
622  input.push_back(MsgHelper(t+s*10,2)); // k
623  input.push_back(MsgHelper(t+s*11,3)); // l
624  input.push_back(MsgHelper(t+s*10,0)); // m
625  input.push_back(MsgHelper(t+s*13,0)); // n
626  input.push_back(MsgHelper(t+s*14,1)); // o
627  output.push_back(TimeQuad(t, t+s, t+s*2, t+s*3));
628  output.push_back(TimeQuad(t+s*5, t+s*6, t+s*6, t+s*5));
629  output.push_back(TimeQuad(t+s*10, t+s*9, t+s*10, t+s*11));
630 
631  ApproximateTimeSynchronizerTestQuad sync_test(input, output, 10, 0, 1, 2, 3);
632  sync_test.run();
633 }
634 
635 TEST(ApproxTimeSync, EarlyPublish) {
636  // Time: 012345678901234
637  // Input A: a......e
638  // Input B: .b......
639  // Input C: ..c.....
640  // Input D: ...d....
641  // Output: .......a
642  // .......b
643  // .......c
644  // .......d
645  std::vector<Msg> input;
646  std::vector<TimeQuad> output;
647 
648  ros::Time t(0, 0);
649  ros::Duration s(1, 0);
650 
651  input.push_back(MsgHelper(t+s*0,0)); // a
652  input.push_back(MsgHelper(t+s*1,1)); // b
653  input.push_back(MsgHelper(t+s*2,2)); // c
654  input.push_back(MsgHelper(t+s*3,3)); // d
655  input.push_back(MsgHelper(t+s*7,0)); // e
656  output.push_back(TimeQuad(t, t+s, t+s*2, t+s*3));
657 
658  ApproximateTimeSynchronizerTestQuad sync_test(input, output, 10, 0, 1, 2, 3);
659  sync_test.run();
660 }
661 
662 TEST(ApproxTimeSync, RateBound) {
663  // Rate bound A: 1.5
664  // Input A: a..b..c.
665  // Input B: .A..B..C
666  // Output: .a..b...
667  // .A..B...
668  std::vector<Msg> input;
669  std::vector<TimePair> output;
670 
671  ros::Time t(0, 0);
672  ros::Duration s(1, 0);
673 
674  input.push_back(MsgHelper(t+s*0,0)); // a
675  input.push_back(MsgHelper(t+s*1,1)); // A
676  input.push_back(MsgHelper(t+s*3,0)); // b
677  input.push_back(MsgHelper(t+s*4,1)); // B
678  input.push_back(MsgHelper(t+s*6,0)); // c
679  input.push_back(MsgHelper(t+s*7,1)); // C
680  output.push_back(TimePair(t, t+s));
681  output.push_back(TimePair(t+s*3, t+s*4));
682 
683  ApproximateTimeSynchronizerTest sync_test(input, output, 10, 0, 1);
684  sync_test.sync_.setInterMessageLowerBound(0, s*1.5);
685  sync_test.run();
686 
687  // Rate bound A: 2
688  // Input A: a..b..c.
689  // Input B: .A..B..C
690  // Output: .a..b..c
691  // .A..B..C
692 
693  output.push_back(TimePair(t+s*6, t+s*7));
694 
695  ApproximateTimeSynchronizerTest sync_test2(input, output, 10, 0, 1);
696  sync_test2.sync_.setInterMessageLowerBound(0, s*2);
697  sync_test2.run();
698 }
699 
700 TEST(ApproxTimeSync, RateBoundAll) {
701  // Input A: a..b..c.
702  // Input B: .A..B..C
703  // Output: .a..b...
704  // .A..B...
705  std::vector<Msg> input;
706  std::vector<TimePair> output;
707 
708  ros::Time t(0, 0);
709  ros::Duration s(1, 0);
710 
711  input.push_back(MsgHelper(t+s*0,0)); // a
712  input.push_back(MsgHelper(t+s*1,1)); // A
713  input.push_back(MsgHelper(t+s*3,0)); // b
714  input.push_back(MsgHelper(t+s*4,1)); // B
715  input.push_back(MsgHelper(t+s*6,0)); // c
716  input.push_back(MsgHelper(t+s*7,1)); // C
717  output.push_back(TimePair(t, t+s));
718  output.push_back(TimePair(t+s*3, t+s*4));
719 
720  ApproximateTimeSynchronizerTest sync_test(input, output, 10, 0, 1);
721  sync_test.run();
722 
723  // Rate bound: 2
724  // Input A: a..b..c.
725  // Input B: .A..B..C
726  // Output: .a..b..c
727  // .A..B..C
728 
729  output.push_back(TimePair(t+s*6, t+s*7));
730 
731  ApproximateTimeSynchronizerTest sync_test2(input, output, 10, 0, 1);
732  sync_test2.sync_.setInterMessageLowerBound(s*2.0);
733  sync_test2.run();
734 }
735 
736 TEST(ApproxTimeSync, ExtendedIds) {
737  // Input A: a..b..c
738  // Input B: A..B..C
739  // Output: a..b..c
740  // A..B..C
741  std::vector<Msg> input;
742  std::vector<TimePair> output;
743 
744  ros::Time t(0, 0);
745  ros::Duration s(1, 0);
746 
747  input.push_back(MsgHelper(t+s*0,0,true )); // a
748  input.push_back(MsgHelper(t+s*0,1,false)); // A
749  input.push_back(MsgHelper(t+s*3,0,true )); // b
750  input.push_back(MsgHelper(t+s*3,1,false)); // B
751  input.push_back(MsgHelper(t+s*6,0,true )); // c
752  input.push_back(MsgHelper(t+s*6,1,false)); // C
753  output.clear();
754 
755  ApproximateTimeSynchronizerTest sync_test1(input, output, 10, 0x00000000, 0x00000001);
756  sync_test1.run();
757 
758  ApproximateTimeSynchronizerTest sync_test2(input, output, 10, 0x00000000, 0x80000001);
759  sync_test2.run();
760 
761  output.push_back(TimePair(t, t));
762  output.push_back(TimePair(t+s*3, t+s*3));
763  output.push_back(TimePair(t+s*6, t+s*6));
764 
765  ApproximateTimeSynchronizerTest sync_test3(input, output, 10, 0x80000000, 0x00000001);
766  sync_test3.run();
767 }
768 
769 TEST(ApproxTimeSync, ErrorFrames) {
770  // Input A: a..b..c
771  // Input B: A..B..C
772  // Output: a..b..c
773  // A..B..C
774  std::vector<Msg> input;
775  std::vector<TimePair> output;
776 
777  ros::Time t(0, 0);
778  ros::Duration s(1, 0);
779 
780  input.push_back(MsgHelper(t+s*0,0,false,true)); // a
781  input.push_back(MsgHelper(t+s*0,1,false,true)); // A
782  input.push_back(MsgHelper(t+s*3,0,false,true)); // b
783  input.push_back(MsgHelper(t+s*3,1,false,true)); // B
784  input.push_back(MsgHelper(t+s*6,0,false,true)); // c
785  input.push_back(MsgHelper(t+s*6,1,false,true)); // C
786  output.clear();
787 
788  ApproximateTimeSynchronizerTest sync_test(input, output, 10, 0, 1);
789  sync_test.run();
790 }
791 
792 TEST(ApproxTimeSync, RtrFrames) {
793  // Input A: a..b..c
794  // Input B: A..B..C
795  // Output: a..b..c
796  // A..B..C
797  std::vector<Msg> input;
798  std::vector<TimePair> output;
799 
800  ros::Time t(0, 0);
801  ros::Duration s(1, 0);
802 
803  input.push_back(MsgHelper(t+s*0,0,false,false,true)); // a
804  input.push_back(MsgHelper(t+s*0,1,false,false,true)); // A
805  input.push_back(MsgHelper(t+s*3,0,false,false,true)); // b
806  input.push_back(MsgHelper(t+s*3,1,false,false,true)); // B
807  input.push_back(MsgHelper(t+s*6,0,false,false,true)); // c
808  input.push_back(MsgHelper(t+s*6,1,false,false,true)); // C
809  output.clear();
810 
811  ApproximateTimeSynchronizerTest sync_test(input, output, 10, 0, 1);
812  sync_test.run();
813 }
814 
815 
816 int main(int argc, char **argv)
817 {
818  testing::InitGoogleTest(&argc, argv);
819  return RUN_ALL_TESTS();
820 }
821 
MsgConstPtr
boost::shared_ptr< Msg const > MsgConstPtr
Definition: test.cpp:272
ApproximateTimeSynchronizerTest::callback
void callback(const std::vector< can_msgs::Frame::ConstPtr > &msgs)
Definition: test.cpp:311
s
XmlRpcServer s
dataspeed_can_msg_filters
Definition: ApproximateTime.h:42
ApproximateTimeSynchronizerTestQuad::callback
void callback(const std::vector< can_msgs::Frame::ConstPtr > &msgs)
Definition: test.cpp:356
dataspeed_can_msg_filters::ApproximateTime::ValidId
static bool ValidId(uint32_t id)
Definition: ApproximateTime.h:73
header
std_msgs::Header * header(M &m)
ApproximateTimeSynchronizerTestQuad::input_
const std::vector< Msg > & input_
Definition: test.cpp:383
MsgHelper
Msg MsgHelper(ros::Time stamp, uint32_t id, bool is_extended=false, bool is_error=false, bool is_rtr=false)
Definition: test.cpp:274
boost::shared_ptr
ApproximateTimeSynchronizerTest::input_
const std::vector< Msg > & input_
Definition: test.cpp:334
dataspeed_can_msg_filters::ApproximateTime
Definition: ApproximateTime.h:67
TimeQuad::TimeQuad
TimeQuad(ros::Time p, ros::Time q, ros::Time r, ros::Time s)
Definition: test.cpp:287
ApproximateTimeSynchronizerTest::ApproximateTimeSynchronizerTest
ApproximateTimeSynchronizerTest(const std::vector< Msg > &input, const std::vector< TimePair > &output, uint32_t queue_size, uint32_t id1, uint32_t id2)
Definition: test.cpp:303
TimePair
std::pair< ros::Time, ros::Time > TimePair
Definition: test.cpp:284
main
int main(int argc, char **argv)
Definition: test.cpp:816
dataspeed_can_msg_filters::ApproximateTime::BuildId
static uint32_t BuildId(uint32_t id, bool extended)
Definition: ApproximateTime.h:91
TEST
TEST(ApproxTimeSync, ValidId_PostMask)
Definition: test.cpp:11
ApproximateTimeSynchronizerTest::output_
const std::vector< TimePair > & output_
Definition: test.cpp:335
dataspeed_can_msg_filters::ApproximateTime::setInterMessageLowerBound
void setInterMessageLowerBound(ros::Duration lower_bound)
Definition: ApproximateTime.h:415
MsgPtr
boost::shared_ptr< Msg > MsgPtr
Definition: test.cpp:271
Msg
can_msgs::Frame Msg
Definition: test.cpp:270
ApproximateTimeSynchronizerTestQuad::output_
const std::vector< TimeQuad > & output_
Definition: test.cpp:384
ApproximateTimeSynchronizerTest::output_position_
unsigned int output_position_
Definition: test.cpp:336
ApproximateTimeSynchronizerTestQuad::ApproximateTimeSynchronizerTestQuad
ApproximateTimeSynchronizerTestQuad(const std::vector< Msg > &input, const std::vector< TimeQuad > &output, uint32_t queue_size, uint32_t id1, uint32_t id2, uint32_t id3, uint32_t id4)
Definition: test.cpp:348

dataspeed_can_msg_filters
Author(s): Kevin Hallenbeck
autogenerated on Thu Jul 9 2020 03:41:58