5 #include <gtest/gtest.h> 11 TEST(ApproxTimeSync, ValidId_PostMask)
100 TEST(ApproxTimeSync, ValidId_PreMask)
270 typedef can_msgs::Frame
Msg;
276 msg.header.stamp = stamp;
278 msg.is_extended = is_extended;
279 msg.is_error = is_error;
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),
311 void callback(
const std::vector<can_msgs::Frame::ConstPtr> &msgs)
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);
326 for (
size_t i = 0; i < input_.size(); i++) {
327 sync_.processMsg(boost::make_shared<Msg>(input_[i]));
330 EXPECT_EQ(output_.size(), output_position_);
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),
356 void callback(
const std::vector<can_msgs::Frame::ConstPtr> &msgs)
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);
375 for (
size_t i = 0; i < input_.size(); i++) {
376 sync_.processMsg(boost::make_shared<Msg>(input_[i]));
379 EXPECT_EQ(output_.size(), output_position_);
392 TEST(ApproxTimeSync, ExactMatch) {
397 std::vector<Msg> input;
398 std::vector<TimePair> output;
410 output.push_back(
TimePair(t+s*3, t+s*3));
411 output.push_back(
TimePair(t+s*6, t+s*6));
417 TEST(ApproxTimeSync, PerfectMatch) {
422 std::vector<Msg> input;
423 std::vector<TimePair> output;
435 output.push_back(
TimePair(t+s*3, t+s*4));
441 TEST(ApproxTimeSync, ImperfectMatch) {
446 std::vector<Msg> input;
447 std::vector<TimePair> output;
460 output.push_back(
TimePair(t+s*6, t+s*5));
466 TEST(ApproxTimeSync, Acceleration) {
472 std::vector<Msg> input;
473 std::vector<TimePair> output;
484 output.push_back(
TimePair(t+s*12, t+s*7));
485 output.push_back(
TimePair(t+s*17, t+s*18));
491 TEST(ApproxTimeSync, DroppedMessages) {
498 std::vector<Msg> input;
499 std::vector<TimePair> output;
514 output.push_back(
TimePair(t+s*4, t+s*3));
515 output.push_back(
TimePair(t+s*10, t+s*11));
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));
536 TEST(ApproxTimeSync, LongQueue) {
543 std::vector<Msg> input;
544 std::vector<TimePair> output;
565 output.push_back(
TimePair(t+s*10, t+s*10));
571 TEST(ApproxTimeSync, DoublePublish) {
579 std::vector<Msg> input;
580 std::vector<TimePair> output;
590 output.push_back(
TimePair(t+s*3, t+s*3));
596 TEST(ApproxTimeSync, FourTopics) {
606 std::vector<Msg> input;
607 std::vector<TimeQuad> output;
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));
635 TEST(ApproxTimeSync, EarlyPublish) {
645 std::vector<Msg> input;
646 std::vector<TimeQuad> output;
656 output.push_back(
TimeQuad(t, t+s, t+s*2, t+s*3));
662 TEST(ApproxTimeSync, RateBound) {
668 std::vector<Msg> input;
669 std::vector<TimePair> output;
681 output.push_back(
TimePair(t+s*3, t+s*4));
693 output.push_back(
TimePair(t+s*6, t+s*7));
700 TEST(ApproxTimeSync, RateBoundAll) {
705 std::vector<Msg> input;
706 std::vector<TimePair> output;
718 output.push_back(
TimePair(t+s*3, t+s*4));
729 output.push_back(
TimePair(t+s*6, t+s*7));
736 TEST(ApproxTimeSync, ExtendedIds) {
741 std::vector<Msg> input;
742 std::vector<TimePair> output;
747 input.push_back(
MsgHelper(t+s*0,0,
true ));
748 input.push_back(
MsgHelper(t+s*0,1,
false));
749 input.push_back(
MsgHelper(t+s*3,0,
true ));
750 input.push_back(
MsgHelper(t+s*3,1,
false));
751 input.push_back(
MsgHelper(t+s*6,0,
true ));
752 input.push_back(
MsgHelper(t+s*6,1,
false));
762 output.push_back(
TimePair(t+s*3, t+s*3));
763 output.push_back(
TimePair(t+s*6, t+s*6));
769 TEST(ApproxTimeSync, ErrorFrames) {
774 std::vector<Msg> input;
775 std::vector<TimePair> output;
780 input.push_back(
MsgHelper(t+s*0,0,
false,
true));
781 input.push_back(
MsgHelper(t+s*0,1,
false,
true));
782 input.push_back(
MsgHelper(t+s*3,0,
false,
true));
783 input.push_back(
MsgHelper(t+s*3,1,
false,
true));
784 input.push_back(
MsgHelper(t+s*6,0,
false,
true));
785 input.push_back(
MsgHelper(t+s*6,1,
false,
true));
792 TEST(ApproxTimeSync, RtrFrames) {
797 std::vector<Msg> input;
798 std::vector<TimePair> output;
803 input.push_back(
MsgHelper(t+s*0,0,
false,
false,
true));
804 input.push_back(
MsgHelper(t+s*0,1,
false,
false,
true));
805 input.push_back(
MsgHelper(t+s*3,0,
false,
false,
true));
806 input.push_back(
MsgHelper(t+s*3,1,
false,
false,
true));
807 input.push_back(
MsgHelper(t+s*6,0,
false,
false,
true));
808 input.push_back(
MsgHelper(t+s*6,1,
false,
false,
true));
816 int main(
int argc,
char **argv)
818 testing::InitGoogleTest(&argc, argv);
819 return RUN_ALL_TESTS();
boost::shared_ptr< Msg const > MsgConstPtr
void callback(const std::vector< can_msgs::Frame::ConstPtr > &msgs)
unsigned int output_position_
void callback(const std::vector< can_msgs::Frame::ConstPtr > &msgs)
static bool ValidId(uint32_t id)
std_msgs::Header * header(M &m)
const std::vector< Msg > & input_
Msg MsgHelper(ros::Time stamp, uint32_t id, bool is_extended=false, bool is_error=false, bool is_rtr=false)
const std::vector< Msg > & input_
TimeQuad(ros::Time p, ros::Time q, ros::Time r, ros::Time s)
ApproximateTimeSynchronizerTest(const std::vector< Msg > &input, const std::vector< TimePair > &output, uint32_t queue_size, uint32_t id1, uint32_t id2)
std::pair< ros::Time, ros::Time > TimePair
int main(int argc, char **argv)
static uint32_t BuildId(uint32_t id, bool extended)
TEST(ApproxTimeSync, ValidId_PostMask)
const std::vector< TimePair > & output_
void setInterMessageLowerBound(ros::Duration lower_bound)
boost::shared_ptr< Msg > MsgPtr
const std::vector< TimeQuad > & output_
unsigned int output_position_
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)