test.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * C++ unit test for ApproximateTime.h
00003  *********************************************************************/
00004 
00005 #include <gtest/gtest.h>
00006 
00007 // File under test
00008 #include <dataspeed_can_msg_filters/ApproximateTime.h>
00009 using namespace dataspeed_can_msg_filters;
00010 
00011 TEST(ApproxTimeSync, ValidId_PostMask)
00012 {
00013   // Standard IDs
00014   ASSERT_TRUE (ApproximateTime::ValidId(0x000007FFu));
00015   ASSERT_TRUE (ApproximateTime::ValidId(0x00000000u));
00016   ASSERT_TRUE (ApproximateTime::ValidId(0x00000001u));
00017   ASSERT_TRUE (ApproximateTime::ValidId(0x00000002u));
00018   ASSERT_TRUE (ApproximateTime::ValidId(0x00000004u));
00019   ASSERT_TRUE (ApproximateTime::ValidId(0x00000008u));
00020   ASSERT_TRUE (ApproximateTime::ValidId(0x00000010u));
00021   ASSERT_TRUE (ApproximateTime::ValidId(0x00000020u));
00022   ASSERT_TRUE (ApproximateTime::ValidId(0x00000040u));
00023   ASSERT_TRUE (ApproximateTime::ValidId(0x00000080u));
00024   ASSERT_TRUE (ApproximateTime::ValidId(0x00000010u));
00025   ASSERT_TRUE (ApproximateTime::ValidId(0x00000020u));
00026   ASSERT_TRUE (ApproximateTime::ValidId(0x00000040u));
00027   ASSERT_TRUE (ApproximateTime::ValidId(0x00000080u));
00028   ASSERT_TRUE (ApproximateTime::ValidId(0x00000100u));
00029   ASSERT_TRUE (ApproximateTime::ValidId(0x00000200u));
00030   ASSERT_TRUE (ApproximateTime::ValidId(0x00000400u));
00031   ASSERT_FALSE(ApproximateTime::ValidId(0x00000800u));
00032   ASSERT_FALSE(ApproximateTime::ValidId(0x00001000u));
00033   ASSERT_FALSE(ApproximateTime::ValidId(0x00002000u));
00034   ASSERT_FALSE(ApproximateTime::ValidId(0x00004000u));
00035   ASSERT_FALSE(ApproximateTime::ValidId(0x00008000u));
00036   ASSERT_FALSE(ApproximateTime::ValidId(0x00010000u));
00037   ASSERT_FALSE(ApproximateTime::ValidId(0x00020000u));
00038   ASSERT_FALSE(ApproximateTime::ValidId(0x00040000u));
00039   ASSERT_FALSE(ApproximateTime::ValidId(0x00080000u));
00040   ASSERT_FALSE(ApproximateTime::ValidId(0x00010000u));
00041   ASSERT_FALSE(ApproximateTime::ValidId(0x00020000u));
00042   ASSERT_FALSE(ApproximateTime::ValidId(0x00040000u));
00043   ASSERT_FALSE(ApproximateTime::ValidId(0x00080000u));
00044   ASSERT_FALSE(ApproximateTime::ValidId(0x00100000u));
00045   ASSERT_FALSE(ApproximateTime::ValidId(0x00200000u));
00046   ASSERT_FALSE(ApproximateTime::ValidId(0x00400000u));
00047   ASSERT_FALSE(ApproximateTime::ValidId(0x00800000u));
00048   ASSERT_FALSE(ApproximateTime::ValidId(0x01000000u));
00049   ASSERT_FALSE(ApproximateTime::ValidId(0x02000000u));
00050   ASSERT_FALSE(ApproximateTime::ValidId(0x04000000u));
00051   ASSERT_FALSE(ApproximateTime::ValidId(0x08000000u));
00052   ASSERT_FALSE(ApproximateTime::ValidId(0x10000000u));
00053   ASSERT_FALSE(ApproximateTime::ValidId(0x20000000u));
00054   ASSERT_FALSE(ApproximateTime::ValidId(0x40000000u));
00055 
00056   // Extended IDs
00057   ASSERT_TRUE (ApproximateTime::ValidId(0x9FFFFFFFu));
00058   ASSERT_TRUE (ApproximateTime::ValidId(0x80000000u));
00059   ASSERT_TRUE (ApproximateTime::ValidId(0x80000001u));
00060   ASSERT_TRUE (ApproximateTime::ValidId(0x80000002u));
00061   ASSERT_TRUE (ApproximateTime::ValidId(0x80000004u));
00062   ASSERT_TRUE (ApproximateTime::ValidId(0x80000008u));
00063   ASSERT_TRUE (ApproximateTime::ValidId(0x80000010u));
00064   ASSERT_TRUE (ApproximateTime::ValidId(0x80000020u));
00065   ASSERT_TRUE (ApproximateTime::ValidId(0x80000040u));
00066   ASSERT_TRUE (ApproximateTime::ValidId(0x80000080u));
00067   ASSERT_TRUE (ApproximateTime::ValidId(0x80000010u));
00068   ASSERT_TRUE (ApproximateTime::ValidId(0x80000020u));
00069   ASSERT_TRUE (ApproximateTime::ValidId(0x80000040u));
00070   ASSERT_TRUE (ApproximateTime::ValidId(0x80000080u));
00071   ASSERT_TRUE (ApproximateTime::ValidId(0x80000100u));
00072   ASSERT_TRUE (ApproximateTime::ValidId(0x80000200u));
00073   ASSERT_TRUE (ApproximateTime::ValidId(0x80000400u));
00074   ASSERT_TRUE (ApproximateTime::ValidId(0x80000800u));
00075   ASSERT_TRUE (ApproximateTime::ValidId(0x80001000u));
00076   ASSERT_TRUE (ApproximateTime::ValidId(0x80002000u));
00077   ASSERT_TRUE (ApproximateTime::ValidId(0x80004000u));
00078   ASSERT_TRUE (ApproximateTime::ValidId(0x80008000u));
00079   ASSERT_TRUE (ApproximateTime::ValidId(0x80010000u));
00080   ASSERT_TRUE (ApproximateTime::ValidId(0x80020000u));
00081   ASSERT_TRUE (ApproximateTime::ValidId(0x80040000u));
00082   ASSERT_TRUE (ApproximateTime::ValidId(0x80080000u));
00083   ASSERT_TRUE (ApproximateTime::ValidId(0x80010000u));
00084   ASSERT_TRUE (ApproximateTime::ValidId(0x80020000u));
00085   ASSERT_TRUE (ApproximateTime::ValidId(0x80040000u));
00086   ASSERT_TRUE (ApproximateTime::ValidId(0x80080000u));
00087   ASSERT_TRUE (ApproximateTime::ValidId(0x80100000u));
00088   ASSERT_TRUE (ApproximateTime::ValidId(0x80200000u));
00089   ASSERT_TRUE (ApproximateTime::ValidId(0x80400000u));
00090   ASSERT_TRUE (ApproximateTime::ValidId(0x80800000u));
00091   ASSERT_TRUE (ApproximateTime::ValidId(0x81000000u));
00092   ASSERT_TRUE (ApproximateTime::ValidId(0x82000000u));
00093   ASSERT_TRUE (ApproximateTime::ValidId(0x84000000u));
00094   ASSERT_TRUE (ApproximateTime::ValidId(0x88000000u));
00095   ASSERT_TRUE (ApproximateTime::ValidId(0x90000000u));
00096   ASSERT_FALSE(ApproximateTime::ValidId(0xA0000000u));
00097   ASSERT_FALSE(ApproximateTime::ValidId(0xC0000000u));
00098 }
00099 
00100 TEST(ApproxTimeSync, ValidId_PreMask)
00101 {
00102   // Standard IDs
00103   ASSERT_TRUE (ApproximateTime::ValidId(0x000007FFu, false));
00104   ASSERT_TRUE (ApproximateTime::ValidId(0x00000000u, false));
00105   ASSERT_TRUE (ApproximateTime::ValidId(0x00000001u, false));
00106   ASSERT_TRUE (ApproximateTime::ValidId(0x00000002u, false));
00107   ASSERT_TRUE (ApproximateTime::ValidId(0x00000004u, false));
00108   ASSERT_TRUE (ApproximateTime::ValidId(0x00000008u, false));
00109   ASSERT_TRUE (ApproximateTime::ValidId(0x00000010u, false));
00110   ASSERT_TRUE (ApproximateTime::ValidId(0x00000020u, false));
00111   ASSERT_TRUE (ApproximateTime::ValidId(0x00000040u, false));
00112   ASSERT_TRUE (ApproximateTime::ValidId(0x00000080u, false));
00113   ASSERT_TRUE (ApproximateTime::ValidId(0x00000010u, false));
00114   ASSERT_TRUE (ApproximateTime::ValidId(0x00000020u, false));
00115   ASSERT_TRUE (ApproximateTime::ValidId(0x00000040u, false));
00116   ASSERT_TRUE (ApproximateTime::ValidId(0x00000080u, false));
00117   ASSERT_TRUE (ApproximateTime::ValidId(0x00000100u, false));
00118   ASSERT_TRUE (ApproximateTime::ValidId(0x00000200u, false));
00119   ASSERT_TRUE (ApproximateTime::ValidId(0x00000400u, false));
00120   ASSERT_FALSE(ApproximateTime::ValidId(0x00000800u, false));
00121   ASSERT_FALSE(ApproximateTime::ValidId(0x00001000u, false));
00122   ASSERT_FALSE(ApproximateTime::ValidId(0x00002000u, false));
00123   ASSERT_FALSE(ApproximateTime::ValidId(0x00004000u, false));
00124   ASSERT_FALSE(ApproximateTime::ValidId(0x00008000u, false));
00125   ASSERT_FALSE(ApproximateTime::ValidId(0x00010000u, false));
00126   ASSERT_FALSE(ApproximateTime::ValidId(0x00020000u, false));
00127   ASSERT_FALSE(ApproximateTime::ValidId(0x00040000u, false));
00128   ASSERT_FALSE(ApproximateTime::ValidId(0x00080000u, false));
00129   ASSERT_FALSE(ApproximateTime::ValidId(0x00010000u, false));
00130   ASSERT_FALSE(ApproximateTime::ValidId(0x00020000u, false));
00131   ASSERT_FALSE(ApproximateTime::ValidId(0x00040000u, false));
00132   ASSERT_FALSE(ApproximateTime::ValidId(0x00080000u, false));
00133   ASSERT_FALSE(ApproximateTime::ValidId(0x00100000u, false));
00134   ASSERT_FALSE(ApproximateTime::ValidId(0x00200000u, false));
00135   ASSERT_FALSE(ApproximateTime::ValidId(0x00400000u, false));
00136   ASSERT_FALSE(ApproximateTime::ValidId(0x00800000u, false));
00137   ASSERT_FALSE(ApproximateTime::ValidId(0x01000000u, false));
00138   ASSERT_FALSE(ApproximateTime::ValidId(0x02000000u, false));
00139   ASSERT_FALSE(ApproximateTime::ValidId(0x04000000u, false));
00140   ASSERT_FALSE(ApproximateTime::ValidId(0x08000000u, false));
00141   ASSERT_FALSE(ApproximateTime::ValidId(0x10000000u, false));
00142   ASSERT_FALSE(ApproximateTime::ValidId(0x20000000u, false));
00143   ASSERT_FALSE(ApproximateTime::ValidId(0x40000000u, false));
00144   ASSERT_FALSE(ApproximateTime::ValidId(0x80000000u, false));
00145 
00146   // Extended IDs
00147   ASSERT_TRUE (ApproximateTime::ValidId(0x1FFFFFFFu, true));
00148   ASSERT_TRUE (ApproximateTime::ValidId(0x00000000u, true));
00149   ASSERT_TRUE (ApproximateTime::ValidId(0x00000001u, true));
00150   ASSERT_TRUE (ApproximateTime::ValidId(0x00000002u, true));
00151   ASSERT_TRUE (ApproximateTime::ValidId(0x00000004u, true));
00152   ASSERT_TRUE (ApproximateTime::ValidId(0x00000008u, true));
00153   ASSERT_TRUE (ApproximateTime::ValidId(0x00000010u, true));
00154   ASSERT_TRUE (ApproximateTime::ValidId(0x00000020u, true));
00155   ASSERT_TRUE (ApproximateTime::ValidId(0x00000040u, true));
00156   ASSERT_TRUE (ApproximateTime::ValidId(0x00000080u, true));
00157   ASSERT_TRUE (ApproximateTime::ValidId(0x00000010u, true));
00158   ASSERT_TRUE (ApproximateTime::ValidId(0x00000020u, true));
00159   ASSERT_TRUE (ApproximateTime::ValidId(0x00000040u, true));
00160   ASSERT_TRUE (ApproximateTime::ValidId(0x00000080u, true));
00161   ASSERT_TRUE (ApproximateTime::ValidId(0x00000100u, true));
00162   ASSERT_TRUE (ApproximateTime::ValidId(0x00000200u, true));
00163   ASSERT_TRUE (ApproximateTime::ValidId(0x00000400u, true));
00164   ASSERT_TRUE (ApproximateTime::ValidId(0x00000800u, true));
00165   ASSERT_TRUE (ApproximateTime::ValidId(0x00001000u, true));
00166   ASSERT_TRUE (ApproximateTime::ValidId(0x00002000u, true));
00167   ASSERT_TRUE (ApproximateTime::ValidId(0x00004000u, true));
00168   ASSERT_TRUE (ApproximateTime::ValidId(0x00008000u, true));
00169   ASSERT_TRUE (ApproximateTime::ValidId(0x00010000u, true));
00170   ASSERT_TRUE (ApproximateTime::ValidId(0x00020000u, true));
00171   ASSERT_TRUE (ApproximateTime::ValidId(0x00040000u, true));
00172   ASSERT_TRUE (ApproximateTime::ValidId(0x00080000u, true));
00173   ASSERT_TRUE (ApproximateTime::ValidId(0x00010000u, true));
00174   ASSERT_TRUE (ApproximateTime::ValidId(0x00020000u, true));
00175   ASSERT_TRUE (ApproximateTime::ValidId(0x00040000u, true));
00176   ASSERT_TRUE (ApproximateTime::ValidId(0x00080000u, true));
00177   ASSERT_TRUE (ApproximateTime::ValidId(0x00100000u, true));
00178   ASSERT_TRUE (ApproximateTime::ValidId(0x00200000u, true));
00179   ASSERT_TRUE (ApproximateTime::ValidId(0x00400000u, true));
00180   ASSERT_TRUE (ApproximateTime::ValidId(0x00800000u, true));
00181   ASSERT_TRUE (ApproximateTime::ValidId(0x01000000u, true));
00182   ASSERT_TRUE (ApproximateTime::ValidId(0x02000000u, true));
00183   ASSERT_TRUE (ApproximateTime::ValidId(0x04000000u, true));
00184   ASSERT_TRUE (ApproximateTime::ValidId(0x08000000u, true));
00185   ASSERT_TRUE (ApproximateTime::ValidId(0x10000000u, true));
00186   ASSERT_FALSE(ApproximateTime::ValidId(0x20000000u, true));
00187   ASSERT_FALSE(ApproximateTime::ValidId(0x40000000u, true));
00188   ASSERT_FALSE(ApproximateTime::ValidId(0x80000000u, true));
00189 }
00190 
00191 TEST(ApproxTimeSync, BuildId)
00192 {
00193   // Standard IDs
00194   ASSERT_EQ(0x000007FFu, ApproximateTime::BuildId(0x000007FFu, false));
00195   ASSERT_EQ(0x000007FFu, ApproximateTime::BuildId(0xFFFFFFFFu, false));
00196   ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x00000000u, false));
00197   ASSERT_EQ(0x00000001u, ApproximateTime::BuildId(0x00000001u, false));
00198   ASSERT_EQ(0x00000002u, ApproximateTime::BuildId(0x00000002u, false));
00199   ASSERT_EQ(0x00000004u, ApproximateTime::BuildId(0x00000004u, false));
00200   ASSERT_EQ(0x00000008u, ApproximateTime::BuildId(0x00000008u, false));
00201   ASSERT_EQ(0x00000010u, ApproximateTime::BuildId(0x00000010u, false));
00202   ASSERT_EQ(0x00000020u, ApproximateTime::BuildId(0x00000020u, false));
00203   ASSERT_EQ(0x00000040u, ApproximateTime::BuildId(0x00000040u, false));
00204   ASSERT_EQ(0x00000080u, ApproximateTime::BuildId(0x00000080u, false));
00205   ASSERT_EQ(0x00000100u, ApproximateTime::BuildId(0x00000100u, false));
00206   ASSERT_EQ(0x00000200u, ApproximateTime::BuildId(0x00000200u, false));
00207   ASSERT_EQ(0x00000400u, ApproximateTime::BuildId(0x00000400u, false));
00208   ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x00000800u, false));
00209   ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x00001000u, false));
00210   ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x00002000u, false));
00211   ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x00004000u, false));
00212   ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x00008000u, false));
00213   ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x00010000u, false));
00214   ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x00020000u, false));
00215   ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x00040000u, false));
00216   ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x00080000u, false));
00217   ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x00100000u, false));
00218   ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x00200000u, false));
00219   ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x00400000u, false));
00220   ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x00800000u, false));
00221   ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x01000000u, false));
00222   ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x02000000u, false));
00223   ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x04000000u, false));
00224   ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x08000000u, false));
00225   ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x10000000u, false));
00226   ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x20000000u, false));
00227   ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x40000000u, false));
00228   ASSERT_EQ(0x00000000u, ApproximateTime::BuildId(0x80000000u, false));
00229 
00230   // Extended IDs
00231   ASSERT_EQ(0x9FFFFFFFu, ApproximateTime::BuildId(0x1FFFFFFFu, true));
00232   ASSERT_EQ(0x9FFFFFFFu, ApproximateTime::BuildId(0xFFFFFFFFu, true));
00233   ASSERT_EQ(0x80000000u, ApproximateTime::BuildId(0x00000000u, true));
00234   ASSERT_EQ(0x80000001u, ApproximateTime::BuildId(0x00000001u, true));
00235   ASSERT_EQ(0x80000002u, ApproximateTime::BuildId(0x00000002u, true));
00236   ASSERT_EQ(0x80000004u, ApproximateTime::BuildId(0x00000004u, true));
00237   ASSERT_EQ(0x80000008u, ApproximateTime::BuildId(0x00000008u, true));
00238   ASSERT_EQ(0x80000010u, ApproximateTime::BuildId(0x00000010u, true));
00239   ASSERT_EQ(0x80000020u, ApproximateTime::BuildId(0x00000020u, true));
00240   ASSERT_EQ(0x80000040u, ApproximateTime::BuildId(0x00000040u, true));
00241   ASSERT_EQ(0x80000080u, ApproximateTime::BuildId(0x00000080u, true));
00242   ASSERT_EQ(0x80000100u, ApproximateTime::BuildId(0x00000100u, true));
00243   ASSERT_EQ(0x80000200u, ApproximateTime::BuildId(0x00000200u, true));
00244   ASSERT_EQ(0x80000400u, ApproximateTime::BuildId(0x00000400u, true));
00245   ASSERT_EQ(0x80000800u, ApproximateTime::BuildId(0x00000800u, true));
00246   ASSERT_EQ(0x80001000u, ApproximateTime::BuildId(0x00001000u, true));
00247   ASSERT_EQ(0x80002000u, ApproximateTime::BuildId(0x00002000u, true));
00248   ASSERT_EQ(0x80004000u, ApproximateTime::BuildId(0x00004000u, true));
00249   ASSERT_EQ(0x80008000u, ApproximateTime::BuildId(0x00008000u, true));
00250   ASSERT_EQ(0x80010000u, ApproximateTime::BuildId(0x00010000u, true));
00251   ASSERT_EQ(0x80020000u, ApproximateTime::BuildId(0x00020000u, true));
00252   ASSERT_EQ(0x80040000u, ApproximateTime::BuildId(0x00040000u, true));
00253   ASSERT_EQ(0x80080000u, ApproximateTime::BuildId(0x00080000u, true));
00254   ASSERT_EQ(0x80100000u, ApproximateTime::BuildId(0x00100000u, true));
00255   ASSERT_EQ(0x80200000u, ApproximateTime::BuildId(0x00200000u, true));
00256   ASSERT_EQ(0x80400000u, ApproximateTime::BuildId(0x00400000u, true));
00257   ASSERT_EQ(0x80800000u, ApproximateTime::BuildId(0x00800000u, true));
00258   ASSERT_EQ(0x81000000u, ApproximateTime::BuildId(0x01000000u, true));
00259   ASSERT_EQ(0x82000000u, ApproximateTime::BuildId(0x02000000u, true));
00260   ASSERT_EQ(0x84000000u, ApproximateTime::BuildId(0x04000000u, true));
00261   ASSERT_EQ(0x88000000u, ApproximateTime::BuildId(0x08000000u, true));
00262   ASSERT_EQ(0x90000000u, ApproximateTime::BuildId(0x10000000u, true));
00263   ASSERT_EQ(0x80000000u, ApproximateTime::BuildId(0x20000000u, true));
00264   ASSERT_EQ(0x80000000u, ApproximateTime::BuildId(0x40000000u, true));
00265   ASSERT_EQ(0x80000000u, ApproximateTime::BuildId(0x80000000u, true));
00266 }
00267 
00268 
00269 // Tests ported from https://github.com/ros/ros_comm/blob/2e6ac87958b59455aab0442b205163e9a5f43ff2/utilities/message_filters/test/test_approximate_time_policy.cpp
00270 typedef can_msgs::Frame Msg;
00271 typedef boost::shared_ptr<Msg> MsgPtr;
00272 typedef boost::shared_ptr<Msg const> MsgConstPtr;
00273 
00274 Msg MsgHelper(ros::Time stamp, uint32_t id, bool is_extended = false, bool is_error = false, bool is_rtr = false) {
00275   Msg msg;
00276   msg.header.stamp = stamp;
00277   msg.id = id;
00278   msg.is_extended = is_extended;
00279   msg.is_error = is_error;
00280   msg.is_rtr = is_rtr;
00281   return msg;
00282 }
00283 
00284 typedef std::pair<ros::Time, ros::Time> TimePair;
00285 struct TimeQuad
00286 {
00287   TimeQuad(ros::Time p, ros::Time q, ros::Time r, ros::Time s)
00288   {
00289     time[0] = p;
00290     time[1] = q;
00291     time[2] = r;
00292     time[3] = s;
00293   }
00294   ros::Time time[4];
00295 };
00296 
00297 //----------------------------------------------------------
00298 //                Test Class (for 2 inputs)
00299 //----------------------------------------------------------
00300 class ApproximateTimeSynchronizerTest
00301 {
00302 public:
00303   ApproximateTimeSynchronizerTest(const std::vector<Msg> &input,
00304           const std::vector<TimePair> &output,
00305           uint32_t queue_size, uint32_t id1, uint32_t id2) :
00306     input_(input), output_(output), output_position_(0),
00307     sync_(queue_size, boost::bind(&ApproximateTimeSynchronizerTest::callback, this, _1), id1, id2)
00308   {
00309   }
00310 
00311   void callback(const std::vector<can_msgs::Frame::ConstPtr> &msgs)
00312   {
00313     //printf("Call_back called\n");
00314     //printf("Call back: <%f, %f>\n", msgs[0]->header.stamp.toSec(), msgs[1]->header.stamp.toSec());
00315     ASSERT_EQ(2u, msgs.size());
00316     ASSERT_TRUE(msgs[0]);
00317     ASSERT_TRUE(msgs[1]);
00318     ASSERT_LT(output_position_, output_.size());
00319     EXPECT_EQ(output_[output_position_].first,  msgs[0]->header.stamp);
00320     EXPECT_EQ(output_[output_position_].second, msgs[1]->header.stamp);
00321     ++output_position_;
00322   }
00323 
00324   void run()
00325   {
00326     for (size_t i = 0; i < input_.size(); i++) {
00327       sync_.processMsg(boost::make_shared<Msg>(input_[i]));
00328     }
00329     //printf("Done running test\n");
00330     EXPECT_EQ(output_.size(), output_position_);
00331   }
00332 
00333 private:
00334   const std::vector<Msg> &input_;
00335   const std::vector<TimePair> &output_;
00336   unsigned int output_position_;
00337 public:
00338   ApproximateTime sync_;
00339 };
00340 
00341 //----------------------------------------------------------
00342 //                Test Class (for 4 inputs)
00343 //----------------------------------------------------------
00344 class ApproximateTimeSynchronizerTestQuad
00345 {
00346 public:
00347 
00348   ApproximateTimeSynchronizerTestQuad(const std::vector<Msg> &input,
00349               const std::vector<TimeQuad> &output,
00350               uint32_t queue_size, uint32_t id1, uint32_t id2, uint32_t id3, uint32_t id4) :
00351     input_(input), output_(output), output_position_(0),
00352     sync_(queue_size, boost::bind(&ApproximateTimeSynchronizerTestQuad::callback, this, _1), id1, id2, id3, id4)
00353   {
00354   }
00355 
00356   void callback(const std::vector<can_msgs::Frame::ConstPtr> &msgs)
00357   {
00358     //printf("Call_back called\n");
00359     //printf("Call back: <%f, %f>\n", msgs[0]->header.stamp.toSec(), msgs[1]->header.stamp.toSec());
00360     ASSERT_EQ(4u, msgs.size());
00361     ASSERT_TRUE(msgs[0]);
00362     ASSERT_TRUE(msgs[1]);
00363     ASSERT_TRUE(msgs[2]);
00364     ASSERT_TRUE(msgs[3]);
00365     ASSERT_LT(output_position_, output_.size());
00366     EXPECT_EQ(output_[output_position_].time[0], msgs[0]->header.stamp);
00367     EXPECT_EQ(output_[output_position_].time[1], msgs[1]->header.stamp);
00368     EXPECT_EQ(output_[output_position_].time[2], msgs[2]->header.stamp);
00369     EXPECT_EQ(output_[output_position_].time[3], msgs[3]->header.stamp);
00370     ++output_position_;
00371   }
00372 
00373   void run()
00374   {
00375     for (size_t i = 0; i < input_.size(); i++) {
00376       sync_.processMsg(boost::make_shared<Msg>(input_[i]));
00377     }
00378     //printf("Done running test\n");
00379     EXPECT_EQ(output_.size(), output_position_);
00380   }
00381 
00382 private:
00383   const std::vector<Msg> &input_;
00384   const std::vector<TimeQuad> &output_;
00385   unsigned int output_position_;
00386 public:
00387   ApproximateTime sync_;
00388 };
00389 
00390 
00391 
00392 TEST(ApproxTimeSync, ExactMatch) {
00393   // Input A:  a..b..c
00394   // Input B:  A..B..C
00395   // Output:   a..b..c
00396   //           A..B..C
00397   std::vector<Msg> input;
00398   std::vector<TimePair> output;
00399 
00400   ros::Time t(0, 0);
00401   ros::Duration s(1, 0);
00402 
00403   input.push_back(MsgHelper(t+s*0,0)); // a
00404   input.push_back(MsgHelper(t+s*0,1)); // A
00405   input.push_back(MsgHelper(t+s*3,0)); // b
00406   input.push_back(MsgHelper(t+s*3,1)); // B
00407   input.push_back(MsgHelper(t+s*6,0)); // c
00408   input.push_back(MsgHelper(t+s*6,1)); // C
00409   output.push_back(TimePair(t, t));
00410   output.push_back(TimePair(t+s*3, t+s*3));
00411   output.push_back(TimePair(t+s*6, t+s*6));
00412 
00413   ApproximateTimeSynchronizerTest sync_test(input, output, 10, 0, 1);
00414   sync_test.run();
00415 }
00416 
00417 TEST(ApproxTimeSync, PerfectMatch) {
00418   // Input A:  a..b..c.
00419   // Input B:  .A..B..C
00420   // Output:   ...a..b.
00421   //           ...A..B.
00422   std::vector<Msg> input;
00423   std::vector<TimePair> output;
00424 
00425   ros::Time t(0, 0);
00426   ros::Duration s(1, 0);
00427 
00428   input.push_back(MsgHelper(t+s*0,0)); // a
00429   input.push_back(MsgHelper(t+s*1,1)); // A
00430   input.push_back(MsgHelper(t+s*3,0)); // b
00431   input.push_back(MsgHelper(t+s*4,1)); // B
00432   input.push_back(MsgHelper(t+s*6,0)); // c
00433   input.push_back(MsgHelper(t+s*7,1)); // C
00434   output.push_back(TimePair(t, t+s));
00435   output.push_back(TimePair(t+s*3, t+s*4));
00436 
00437   ApproximateTimeSynchronizerTest sync_test(input, output, 10, 0, 1);
00438   sync_test.run();
00439 }
00440 
00441 TEST(ApproxTimeSync, ImperfectMatch) {
00442   // Input A:  a.xb..c.
00443   // Input B:  .A...B.C
00444   // Output:   ..a...c.
00445   //           ..A...B.
00446   std::vector<Msg> input;
00447   std::vector<TimePair> output;
00448 
00449   ros::Time t(0, 0);
00450   ros::Duration s(1, 0);
00451 
00452   input.push_back(MsgHelper(t+s*0,0)); // a
00453   input.push_back(MsgHelper(t+s*1,1)); // A
00454   input.push_back(MsgHelper(t+s*2,0)); // x
00455   input.push_back(MsgHelper(t+s*3,0)); // b
00456   input.push_back(MsgHelper(t+s*5,1)); // B
00457   input.push_back(MsgHelper(t+s*6,0)); // c
00458   input.push_back(MsgHelper(t+s*7,1)); // C
00459   output.push_back(TimePair(t, t+s));
00460   output.push_back(TimePair(t+s*6, t+s*5));
00461 
00462   ApproximateTimeSynchronizerTest sync_test(input, output, 10, 0, 1);
00463   sync_test.run();
00464 }
00465 
00466 TEST(ApproxTimeSync, Acceleration) {
00467   // Time:     0123456789012345678
00468   // Input A:  a...........b....c.
00469   // Input B:  .......A.......B..C
00470   // Output:   ............b.....c
00471   //           ............A.....C
00472   std::vector<Msg> input;
00473   std::vector<TimePair> output;
00474 
00475   ros::Time t(0, 0);
00476   ros::Duration s(1, 0);
00477 
00478   input.push_back(MsgHelper(t+s*0,0));  // a
00479   input.push_back(MsgHelper(t+s*7,1));  // A
00480   input.push_back(MsgHelper(t+s*12,0)); // b
00481   input.push_back(MsgHelper(t+s*15,1)); // B
00482   input.push_back(MsgHelper(t+s*17,0)); // c
00483   input.push_back(MsgHelper(t+s*18,1)); // C
00484   output.push_back(TimePair(t+s*12, t+s*7));
00485   output.push_back(TimePair(t+s*17, t+s*18));
00486 
00487   ApproximateTimeSynchronizerTest sync_test(input, output, 10, 0, 1);
00488   sync_test.run();
00489 }
00490 
00491 TEST(ApproxTimeSync, DroppedMessages) {
00492   // Queue size 1 (too small)
00493   // Time:     012345678901234
00494   // Input A:  a...b...c.d..e.
00495   // Input B:  .A.B...C...D..E
00496   // Output:   .......b.....d.
00497   //           .......B.....D.
00498   std::vector<Msg> input;
00499   std::vector<TimePair> output;
00500 
00501   ros::Time t(0, 0);
00502   ros::Duration s(1, 0);
00503 
00504   input.push_back(MsgHelper(t+s*0,0));  // a
00505   input.push_back(MsgHelper(t+s*1,1));  // A
00506   input.push_back(MsgHelper(t+s*3,1));  // B
00507   input.push_back(MsgHelper(t+s*4,0));  // b
00508   input.push_back(MsgHelper(t+s*7,1));  // C
00509   input.push_back(MsgHelper(t+s*8,0));  // c
00510   input.push_back(MsgHelper(t+s*10,0)); // d
00511   input.push_back(MsgHelper(t+s*11,1)); // D
00512   input.push_back(MsgHelper(t+s*13,0)); // e
00513   input.push_back(MsgHelper(t+s*14,1)); // E
00514   output.push_back(TimePair(t+s*4, t+s*3));
00515   output.push_back(TimePair(t+s*10, t+s*11));
00516 
00517   ApproximateTimeSynchronizerTest sync_test(input, output, 1, 0, 1);
00518   sync_test.run();
00519 
00520   // Queue size 2 (just enough)
00521   // Time:     012345678901234
00522   // Input A:  a...b...c.d..e.
00523   // Input B:  .A.B...C...D..E
00524   // Output:   ....a..b...c.d.
00525   //           ....A..B...C.D.
00526   std::vector<TimePair> output2;
00527   output2.push_back(TimePair(t, t+s));
00528   output2.push_back(TimePair(t+s*4, t+s*3));
00529   output2.push_back(TimePair(t+s*8, t+s*7));
00530   output2.push_back(TimePair(t+s*10, t+s*11));
00531 
00532   ApproximateTimeSynchronizerTest sync_test2(input, output2, 2, 0, 1);
00533   sync_test2.run();
00534 }
00535 
00536 TEST(ApproxTimeSync, LongQueue) {
00537   // Queue size 5
00538   // Time:     012345678901234
00539   // Input A:  abcdefghiklmnp.
00540   // Input B:  ...j......o....
00541   // Output:   ..........l....
00542   //           ..........o....
00543   std::vector<Msg> input;
00544   std::vector<TimePair> output;
00545 
00546   ros::Time t(0, 0);
00547   ros::Duration s(1, 0);
00548 
00549   input.push_back(MsgHelper(t+s*0,0));  // a
00550   input.push_back(MsgHelper(t+s*1,0));  // b
00551   input.push_back(MsgHelper(t+s*2,0));  // c
00552   input.push_back(MsgHelper(t+s*3,0));  // d
00553   input.push_back(MsgHelper(t+s*4,0));  // e
00554   input.push_back(MsgHelper(t+s*5,0));  // f
00555   input.push_back(MsgHelper(t+s*6,0));  // g
00556   input.push_back(MsgHelper(t+s*7,0));  // h
00557   input.push_back(MsgHelper(t+s*8,0));  // i
00558   input.push_back(MsgHelper(t+s*3,1));  // j
00559   input.push_back(MsgHelper(t+s*9,0));  // k
00560   input.push_back(MsgHelper(t+s*10,0)); // l
00561   input.push_back(MsgHelper(t+s*11,0)); // m
00562   input.push_back(MsgHelper(t+s*12,0)); // n
00563   input.push_back(MsgHelper(t+s*10,1)); // o
00564   input.push_back(MsgHelper(t+s*13,0)); // l
00565   output.push_back(TimePair(t+s*10, t+s*10));
00566 
00567   ApproximateTimeSynchronizerTest sync_test(input, output, 5, 0, 1);
00568   sync_test.run();
00569 }
00570 
00571 TEST(ApproxTimeSync, DoublePublish) {
00572   // Input A:  a..b
00573   // Input B:  .A.B
00574   // Output:   ...b
00575   //           ...B
00576   //              +
00577   //              a
00578   //              A
00579   std::vector<Msg> input;
00580   std::vector<TimePair> output;
00581 
00582   ros::Time t(0, 0);
00583   ros::Duration s(1, 0);
00584 
00585   input.push_back(MsgHelper(t+s*0,0)); // a
00586   input.push_back(MsgHelper(t+s*1,1)); // A
00587   input.push_back(MsgHelper(t+s*3,1)); // B
00588   input.push_back(MsgHelper(t+s*3,0)); // b
00589   output.push_back(TimePair(t, t+s));
00590   output.push_back(TimePair(t+s*3, t+s*3));
00591 
00592   ApproximateTimeSynchronizerTest sync_test(input, output, 10, 0, 1);
00593   sync_test.run();
00594 }
00595 
00596 TEST(ApproxTimeSync, FourTopics) {
00597   // Time:     012345678901234
00598   // Input A:  a....e..i.m..n.
00599   // Input B:  .b....g..j....o
00600   // Input C:  ..c...h...k....
00601   // Input D:  ...d.f.....l...
00602   // Output:   ......a....e..m
00603   //           ......b....g..j
00604   //           ......c....h..k
00605   //           ......d....f..l
00606   std::vector<Msg> input;
00607   std::vector<TimeQuad> output;
00608 
00609   ros::Time t(0, 0);
00610   ros::Duration s(1, 0);
00611 
00612   input.push_back(MsgHelper(t+s*0,0));  // a
00613   input.push_back(MsgHelper(t+s*1,1));  // b
00614   input.push_back(MsgHelper(t+s*2,2));  // c
00615   input.push_back(MsgHelper(t+s*3,3));  // d
00616   input.push_back(MsgHelper(t+s*5,0));  // e
00617   input.push_back(MsgHelper(t+s*5,3));  // f
00618   input.push_back(MsgHelper(t+s*6,1));  // g
00619   input.push_back(MsgHelper(t+s*6,2));  // h
00620   input.push_back(MsgHelper(t+s*8,0));  // i
00621   input.push_back(MsgHelper(t+s*9,1));  // j
00622   input.push_back(MsgHelper(t+s*10,2)); // k
00623   input.push_back(MsgHelper(t+s*11,3)); // l
00624   input.push_back(MsgHelper(t+s*10,0)); // m
00625   input.push_back(MsgHelper(t+s*13,0)); // n
00626   input.push_back(MsgHelper(t+s*14,1)); // o
00627   output.push_back(TimeQuad(t, t+s, t+s*2, t+s*3));
00628   output.push_back(TimeQuad(t+s*5, t+s*6, t+s*6, t+s*5));
00629   output.push_back(TimeQuad(t+s*10, t+s*9, t+s*10, t+s*11));
00630 
00631   ApproximateTimeSynchronizerTestQuad sync_test(input, output, 10, 0, 1, 2, 3);
00632   sync_test.run();
00633 }
00634 
00635 TEST(ApproxTimeSync, EarlyPublish) {
00636   // Time:     012345678901234
00637   // Input A:  a......e
00638   // Input B:  .b......
00639   // Input C:  ..c.....
00640   // Input D:  ...d....
00641   // Output:   .......a
00642   //           .......b
00643   //           .......c
00644   //           .......d
00645   std::vector<Msg> input;
00646   std::vector<TimeQuad> output;
00647 
00648   ros::Time t(0, 0);
00649   ros::Duration s(1, 0);
00650 
00651   input.push_back(MsgHelper(t+s*0,0)); // a
00652   input.push_back(MsgHelper(t+s*1,1)); // b
00653   input.push_back(MsgHelper(t+s*2,2)); // c
00654   input.push_back(MsgHelper(t+s*3,3)); // d
00655   input.push_back(MsgHelper(t+s*7,0)); // e
00656   output.push_back(TimeQuad(t, t+s, t+s*2, t+s*3));
00657 
00658   ApproximateTimeSynchronizerTestQuad sync_test(input, output, 10, 0, 1, 2, 3);
00659   sync_test.run();
00660 }
00661 
00662 TEST(ApproxTimeSync, RateBound) {
00663   // Rate bound A: 1.5
00664   // Input A:  a..b..c.
00665   // Input B:  .A..B..C
00666   // Output:   .a..b...
00667   //           .A..B...
00668   std::vector<Msg> input;
00669   std::vector<TimePair> output;
00670 
00671   ros::Time t(0, 0);
00672   ros::Duration s(1, 0);
00673 
00674   input.push_back(MsgHelper(t+s*0,0)); // a
00675   input.push_back(MsgHelper(t+s*1,1)); // A
00676   input.push_back(MsgHelper(t+s*3,0)); // b
00677   input.push_back(MsgHelper(t+s*4,1)); // B
00678   input.push_back(MsgHelper(t+s*6,0)); // c
00679   input.push_back(MsgHelper(t+s*7,1)); // C
00680   output.push_back(TimePair(t, t+s));
00681   output.push_back(TimePair(t+s*3, t+s*4));
00682 
00683   ApproximateTimeSynchronizerTest sync_test(input, output, 10, 0, 1);
00684   sync_test.sync_.setInterMessageLowerBound(0, s*1.5);
00685   sync_test.run();
00686 
00687   // Rate bound A: 2
00688   // Input A:  a..b..c.
00689   // Input B:  .A..B..C
00690   // Output:   .a..b..c
00691   //           .A..B..C
00692 
00693   output.push_back(TimePair(t+s*6, t+s*7));
00694 
00695   ApproximateTimeSynchronizerTest sync_test2(input, output, 10, 0, 1);
00696   sync_test2.sync_.setInterMessageLowerBound(0, s*2);
00697   sync_test2.run();
00698 }
00699 
00700 TEST(ApproxTimeSync, ExtendedIds) {
00701   // Input A:  a..b..c
00702   // Input B:  A..B..C
00703   // Output:   a..b..c
00704   //           A..B..C
00705   std::vector<Msg> input;
00706   std::vector<TimePair> output;
00707 
00708   ros::Time t(0, 0);
00709   ros::Duration s(1, 0);
00710 
00711   input.push_back(MsgHelper(t+s*0,0,true )); // a
00712   input.push_back(MsgHelper(t+s*0,1,false)); // A
00713   input.push_back(MsgHelper(t+s*3,0,true )); // b
00714   input.push_back(MsgHelper(t+s*3,1,false)); // B
00715   input.push_back(MsgHelper(t+s*6,0,true )); // c
00716   input.push_back(MsgHelper(t+s*6,1,false)); // C
00717   output.clear();
00718 
00719   ApproximateTimeSynchronizerTest sync_test1(input, output, 10, 0x00000000, 0x00000001);
00720   sync_test1.run();
00721 
00722   ApproximateTimeSynchronizerTest sync_test2(input, output, 10, 0x00000000, 0x80000001);
00723   sync_test2.run();
00724 
00725   output.push_back(TimePair(t, t));
00726   output.push_back(TimePair(t+s*3, t+s*3));
00727   output.push_back(TimePair(t+s*6, t+s*6));
00728 
00729   ApproximateTimeSynchronizerTest sync_test3(input, output, 10, 0x80000000, 0x00000001);
00730   sync_test3.run();
00731 }
00732 
00733 TEST(ApproxTimeSync, ErrorFrames) {
00734   // Input A:  a..b..c
00735   // Input B:  A..B..C
00736   // Output:   a..b..c
00737   //           A..B..C
00738   std::vector<Msg> input;
00739   std::vector<TimePair> output;
00740 
00741   ros::Time t(0, 0);
00742   ros::Duration s(1, 0);
00743 
00744   input.push_back(MsgHelper(t+s*0,0,false,true)); // a
00745   input.push_back(MsgHelper(t+s*0,1,false,true)); // A
00746   input.push_back(MsgHelper(t+s*3,0,false,true)); // b
00747   input.push_back(MsgHelper(t+s*3,1,false,true)); // B
00748   input.push_back(MsgHelper(t+s*6,0,false,true)); // c
00749   input.push_back(MsgHelper(t+s*6,1,false,true)); // C
00750   output.clear();
00751 
00752   ApproximateTimeSynchronizerTest sync_test(input, output, 10, 0, 1);
00753   sync_test.run();
00754 }
00755 
00756 TEST(ApproxTimeSync, RtrFrames) {
00757   // Input A:  a..b..c
00758   // Input B:  A..B..C
00759   // Output:   a..b..c
00760   //           A..B..C
00761   std::vector<Msg> input;
00762   std::vector<TimePair> output;
00763 
00764   ros::Time t(0, 0);
00765   ros::Duration s(1, 0);
00766 
00767   input.push_back(MsgHelper(t+s*0,0,false,false,true)); // a
00768   input.push_back(MsgHelper(t+s*0,1,false,false,true)); // A
00769   input.push_back(MsgHelper(t+s*3,0,false,false,true)); // b
00770   input.push_back(MsgHelper(t+s*3,1,false,false,true)); // B
00771   input.push_back(MsgHelper(t+s*6,0,false,false,true)); // c
00772   input.push_back(MsgHelper(t+s*6,1,false,false,true)); // C
00773   output.clear();
00774 
00775   ApproximateTimeSynchronizerTest sync_test(input, output, 10, 0, 1);
00776   sync_test.run();
00777 }
00778 
00779 
00780 int main(int argc, char **argv)
00781 {
00782   testing::InitGoogleTest(&argc, argv);
00783   return RUN_ALL_TESTS();
00784 }
00785

dataspeed_can_msg_filters
Author(s): Kevin Hallenbeck
autogenerated on Thu Jun 6 2019 21:16:37