36 #include <gtest/gtest.h>
62 namespace message_traits
75 typedef std::pair<ros::Time, ros::Time>
TimePair;
98 const std::vector<TimePair> &output,
99 uint32_t queue_size) :
100 input_(input), output_(output), output_position_(0), sync_(queue_size)
111 ASSERT_LT(output_position_, output_.size());
112 EXPECT_EQ(output_[output_position_].first, p->header.stamp);
113 EXPECT_EQ(output_[output_position_].second, q->header.stamp);
119 for (
unsigned int i = 0; i < input_.size(); i++)
121 if (input_[i].second == 0)
123 MsgPtr p(boost::make_shared<Msg>());
124 p->header.stamp = input_[i].first;
129 MsgPtr q(boost::make_shared<Msg>());
130 q->header.stamp = input_[i].first;
135 EXPECT_EQ(output_.size(), output_position_);
156 const std::vector<TimeQuad> &output,
157 uint32_t queue_size) :
158 input_(input), output_(output), output_position_(0), sync_(queue_size)
171 ASSERT_LT(output_position_, output_.size());
172 EXPECT_EQ(output_[output_position_].time[0], p->header.stamp);
173 EXPECT_EQ(output_[output_position_].time[1], q->header.stamp);
174 EXPECT_EQ(output_[output_position_].time[2], r->header.stamp);
175 EXPECT_EQ(output_[output_position_].time[3],
s->header.stamp);
181 for (
unsigned int i = 0; i < input_.size(); i++)
183 MsgPtr p(boost::make_shared<Msg>());
184 p->header.stamp = input_[i].first;
185 switch (input_[i].second)
202 EXPECT_EQ(output_.size(), output_position_);
218 TEST(ApproxTimeSync, ExactMatch) {
223 std::vector<TimeAndTopic> input;
224 std::vector<TimePair> output;
244 TEST(ApproxTimeSync, PerfectMatch) {
249 std::vector<TimeAndTopic> input;
250 std::vector<TimePair> output;
269 TEST(ApproxTimeSync, ImperfectMatch) {
274 std::vector<TimeAndTopic> input;
275 std::vector<TimePair> output;
295 TEST(ApproxTimeSync, Acceleration) {
301 std::vector<TimeAndTopic> input;
302 std::vector<TimePair> output;
321 TEST(ApproxTimeSync, DroppedMessages) {
328 std::vector<TimeAndTopic> input;
329 std::vector<TimePair> output;
356 std::vector<TimePair> output2;
367 TEST(ApproxTimeSync, LongQueue) {
374 std::vector<TimeAndTopic> input;
375 std::vector<TimePair> output;
403 TEST(ApproxTimeSync, DoublePublish) {
411 std::vector<TimeAndTopic> input;
412 std::vector<TimePair> output;
429 TEST(ApproxTimeSync, FourTopics) {
439 std::vector<TimeAndTopic> input;
440 std::vector<TimeQuad> output;
462 output.push_back(
TimeQuad(t+
s*10, t+
s*9, t+
s*10, t+
s*11));
469 TEST(ApproxTimeSync, EarlyPublish) {
479 std::vector<TimeAndTopic> input;
480 std::vector<TimeQuad> output;
497 TEST(ApproxTimeSync, RateBound) {
503 std::vector<TimeAndTopic> input;
504 std::vector<TimePair> output;
519 sync_test.
sync_.setInterMessageLowerBound(0,
s*1.5);
531 sync_test2.
sync_.setInterMessageLowerBound(0,
s*2);
536 TEST(ApproxTimeSync, RateBoundAll) {
541 std::vector<TimeAndTopic> input;
542 std::vector<TimePair> output;
568 sync_test2.
sync_.setInterMessageLowerBound(
s*2);
573 int main(
int argc,
char **argv) {
574 testing::InitGoogleTest(&argc, argv);
578 return RUN_ALL_TESTS();