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;
236 output.push_back(
TimePair(t+s*3, t+s*3));
237 output.push_back(
TimePair(t+s*6, t+s*6));
244 TEST(ApproxTimeSync, PerfectMatch) {
249 std::vector<TimeAndTopic> input;
250 std::vector<TimePair> output;
262 output.push_back(
TimePair(t+s*3, t+s*4));
269 TEST(ApproxTimeSync, ImperfectMatch) {
274 std::vector<TimeAndTopic> input;
275 std::vector<TimePair> output;
288 output.push_back(
TimePair(t+s*6, t+s*5));
295 TEST(ApproxTimeSync, Acceleration) {
301 std::vector<TimeAndTopic> input;
302 std::vector<TimePair> output;
313 output.push_back(
TimePair(t+s*12, t+s*7));
314 output.push_back(
TimePair(t+s*17, t+s*18));
321 TEST(ApproxTimeSync, DroppedMessages) {
328 std::vector<TimeAndTopic> input;
329 std::vector<TimePair> output;
344 output.push_back(
TimePair(t+s*4, t+s*3));
345 output.push_back(
TimePair(t+s*10, t+s*11));
356 std::vector<TimePair> output2;
357 output2.push_back(
TimePair(t, t+s));
358 output2.push_back(
TimePair(t+s*4, t+s*3));
359 output2.push_back(
TimePair(t+s*8, t+s*7));
360 output2.push_back(
TimePair(t+s*10, t+s*11));
367 TEST(ApproxTimeSync, LongQueue) {
374 std::vector<TimeAndTopic> input;
375 std::vector<TimePair> output;
396 output.push_back(
TimePair(t+s*10, t+s*10));
403 TEST(ApproxTimeSync, DoublePublish) {
411 std::vector<TimeAndTopic> input;
412 std::vector<TimePair> output;
422 output.push_back(
TimePair(t+s*3, t+s*3));
429 TEST(ApproxTimeSync, FourTopics) {
439 std::vector<TimeAndTopic> input;
440 std::vector<TimeQuad> output;
460 output.push_back(
TimeQuad(t, t+s, t+s*2, t+s*3));
461 output.push_back(
TimeQuad(t+s*5, t+s*6, t+s*6, t+s*5));
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;
490 output.push_back(
TimeQuad(t, t+s, t+s*2, t+s*3));
497 TEST(ApproxTimeSync, RateBound) {
503 std::vector<TimeAndTopic> input;
504 std::vector<TimePair> output;
516 output.push_back(
TimePair(t+s*3, t+s*4));
519 sync_test.
sync_.setInterMessageLowerBound(0, s*1.5);
528 output.push_back(
TimePair(t+s*6, t+s*7));
531 sync_test2.
sync_.setInterMessageLowerBound(0, s*2);
536 TEST(ApproxTimeSync, RateBoundAll) {
541 std::vector<TimeAndTopic> input;
542 std::vector<TimePair> output;
554 output.push_back(
TimePair(t+s*3, t+s*4));
565 output.push_back(
TimePair(t+s*6, t+s*7));
568 sync_test2.
sync_.setInterMessageLowerBound(s*2);
573 int main(
int argc,
char **argv) {
574 testing::InitGoogleTest(&argc, argv);
578 return RUN_ALL_TESTS();
std::pair< ros::Time, unsigned int > TimeAndTopic
TEST(ApproxTimeSync, ExactMatch)
void callback(const MsgConstPtr &p, const MsgConstPtr &q)
boost::shared_ptr< Msg const > MsgConstPtr
Synchronizer< ApproximateTime< Msg, Msg > > Sync2
unsigned int output_position_
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
const std::vector< TimeAndTopic > & input_
ApproximateTimeSynchronizerTestQuad(const std::vector< TimeAndTopic > &input, const std::vector< TimeQuad > &output, uint32_t queue_size)
static ros::Time value(const Msg &m)
const std::vector< TimeAndTopic > & input_
TimeQuad(ros::Time p, ros::Time q, ros::Time r, ros::Time s)
boost::shared_ptr< Msg > MsgPtr
const std::vector< TimePair > & output_
Synchronizer< ApproximateTime< Msg, Msg, Msg, Msg > > Sync4
int main(int argc, char **argv)
std::pair< ros::Time, ros::Time > TimePair
void callback(const MsgConstPtr &p, const MsgConstPtr &q, const MsgConstPtr &r, const MsgConstPtr &s)
ApproximateTimeSynchronizerTest(const std::vector< TimeAndTopic > &input, const std::vector< TimePair > &output, uint32_t queue_size)
const std::vector< TimeQuad > & output_
unsigned int output_position_