timestamp_tools: timestamp_tools: test_trigger

00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2009-2010, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 #include <timestamp_tools/trigger_matcher.h>
00036 #include <gtest/gtest.h>
00037 #include <queue>
00038 
00040 
00041 class QueuedChecker
00042 {
00043   std::queue< std::pair<double, int> > incoming_;
00044 
00045 public:  
00046   timestamp_tools::QueuedTriggerMatcher<int> tm;
00047   
00048   QueuedChecker() : tm(1, 10, 5)
00049   {
00050     tm.setTrigDelay(0.2);
00051     timestamp_tools::QueuedTriggerMatcher<int>::MatchCallback bound_cb = boost::bind(&QueuedChecker::callback, this, _1, _2);
00052     tm.setMatchCallback(bound_cb);
00053                   
00054     // Get it into the permanent regime with an empty queue.
00055     tm.triggerCallback(0);
00056     expectEmpty(0);
00057     tm.dataCallback(1, 1);
00058     expectEmpty(1);
00059     tm.triggerCallback(2);
00060     expectHead(0.2, 1);
00061     expectEmpty(2);
00062     tm.dataCallback(3, 2);
00063     expectHead(2.2, 2);
00064     expectEmpty(3);
00065   }
00066 
00067   void expectEmpty(double time)
00068   {
00069     EXPECT_TRUE(incoming_.empty()) << "QueuedChecker queue not empty at time " << time 
00070       << " contains " << incoming_.front().first << ", " << incoming_.front().second;
00071   }
00072 
00073   void expectHead(double time, int data)
00074   {
00075     if (incoming_.empty())
00076     {
00077       ADD_FAILURE() << "QueuedChecker queue empty when checking " << time << ", " << data;
00078       return;
00079     }
00080 
00081     std::pair<double, int> &head = incoming_.front();
00082 
00083     EXPECT_EQ(time, head.first) << "Timestamp mismatch when checking " << time << ", " << data;
00084     EXPECT_EQ(data, head.second) << "Data mismatch when checking " << time << ", " << data;
00085 
00086     incoming_.pop();
00087   }
00088 
00089   void callback(const ros::Time &time, const boost::shared_ptr<int const> &data)
00090   {
00091     incoming_.push(std::pair<double, int>(time.toSec(), *data));
00092   }
00093 };
00094 
00095 TEST(QueuedTriggerMatcher, BasicFunctionality)
00096 {
00097   QueuedChecker c;
00098   
00099 // Data gets delayed...
00100   c.tm.triggerCallback(4);
00101   c.expectEmpty(4);
00102   c.tm.triggerCallback(6);
00103   c.expectEmpty(6);
00104   c.tm.triggerCallback(8);
00105   c.expectEmpty(8);
00106   c.tm.dataCallback(5, 3);
00107   c.expectHead(4.2, 3);
00108   c.expectEmpty(5);
00109   c.tm.dataCallback(7, 4);
00110   c.expectHead(6.2, 4);
00111   c.expectEmpty(7);
00112   c.tm.dataCallback(9, 5);
00113   c.expectHead(8.2, 5);
00114   c.expectEmpty(9);
00115 
00116 // Timestamp gets delayed...
00117   c.tm.dataCallback(11, 6);
00118   c.expectEmpty(11);
00119   c.tm.dataCallback(13, 7);
00120   c.expectEmpty(13);
00121   c.tm.dataCallback(15, 8);
00122   c.expectEmpty(15);
00123   c.tm.triggerCallback(10);
00124   c.expectHead(10.2, 6);
00125   c.expectEmpty(10);
00126   c.tm.triggerCallback(12);
00127   c.expectHead(12.2, 7);
00128   c.expectEmpty(12);
00129   c.tm.triggerCallback(14);
00130   c.expectHead(14.2, 8);
00131   c.expectEmpty(14);
00132 }
00133 
00134 TEST(QueuedTriggerMatcher, TestReset)
00135 {
00136   QueuedChecker c;
00137 
00138   c.tm.triggerCallback(4);
00139   c.expectEmpty(4);
00140   c.tm.reset();
00141   c.tm.dataCallback(5, 3);
00142   c.expectEmpty(5);
00143   c.tm.triggerCallback(6);
00144   c.expectEmpty(6);
00145   c.tm.dataCallback(7, 4);
00146   c.expectEmpty(7);
00147   c.tm.triggerCallback(8);
00148   c.expectHead(6.2, 4);
00149   c.expectEmpty(8);
00150   c.tm.dataCallback(9, 5);
00151   c.expectHead(8.2, 5);
00152   c.expectEmpty(9);
00153 
00154   c.tm.dataCallback(5, 3);
00155   c.expectEmpty(5);
00156   c.tm.reset();
00157   c.expectEmpty(5.1);
00158   c.tm.triggerCallback(4);
00159   c.expectEmpty(4);
00160   c.tm.triggerCallback(6);
00161   c.expectEmpty(6);
00162   c.tm.dataCallback(7, 4);
00163   c.expectEmpty(7);
00164   c.tm.triggerCallback(8);
00165   c.expectHead(6.2, 4);
00166   c.expectEmpty(8);
00167   c.tm.dataCallback(9, 5);
00168   c.expectHead(8.2, 5);
00169   c.expectEmpty(9);
00170 }
00171 
00172 TEST(QueuedTriggerMatcher, MissingTrigger)
00173 {
00174   QueuedChecker c;
00175   
00176   // Miss a trigger at time 4...
00177   c.tm.triggerCallback(6);
00178   c.expectEmpty(6);
00179   c.tm.triggerCallback(8);
00180   c.expectEmpty(8);
00181   c.tm.dataCallback(5, 3);
00182   c.expectEmpty(5);
00183   c.tm.dataCallback(7, 4);
00184   c.expectHead(6.2, 4);
00185   c.expectEmpty(7);
00186   c.tm.dataCallback(9, 5);
00187   c.expectHead(8.2, 5);
00188   c.expectEmpty(9);
00189 }
00190 
00191 TEST(QueuedTriggerMatcher, MissingData)
00192 {
00193   QueuedChecker c;
00194   
00195   // Miss data at time 5...
00196   c.tm.triggerCallback(4);
00197   c.expectEmpty(4);
00198   c.tm.triggerCallback(6);
00199   c.expectEmpty(6);
00200   c.tm.triggerCallback(8);
00201   c.expectEmpty(8);
00202   c.tm.triggerCallback(10);
00203   c.expectEmpty(10);
00204   c.tm.triggerCallback(12);
00205   c.expectEmpty(12);
00206   c.tm.dataCallback(7, 4);
00207   c.expectHead(4.2, 4); // Bad
00208   c.expectEmpty(7);
00209   c.tm.dataCallback(9, 5);
00210   c.expectHead(8.2, 5); // Recovered
00211   c.expectEmpty(9);
00212   c.tm.dataCallback(11, 6);
00213   c.expectHead(10.2, 6); 
00214   c.expectEmpty(11);
00215   c.tm.dataCallback(13, 7);
00216   c.expectHead(12.2, 7); 
00217   c.expectEmpty(13);
00218 }
00219 
00220 TEST(QueuedTriggerMatcher, MissingDataZeroLateTolerance)
00221 {
00222   QueuedChecker c;
00223   
00224   c.tm.setLateDataCountAllowed(0);
00225 
00226   // Miss data at time 5...
00227   c.tm.triggerCallback(4);
00228   c.expectEmpty(4);
00229   c.tm.triggerCallback(6);
00230   c.expectEmpty(6);
00231   c.tm.triggerCallback(8);
00232   c.expectEmpty(8);
00233   c.tm.triggerCallback(10);
00234   c.expectEmpty(10);
00235   c.tm.triggerCallback(12);
00236   c.expectEmpty(12);
00237   c.tm.triggerCallback(14);
00238   c.expectEmpty(14);
00239   c.tm.dataCallback(5, 4);
00240   c.expectHead(4.2, 4); 
00241   c.expectEmpty(5);
00242   c.tm.dataCallback(9, 5);
00243   c.expectHead(8.2, 5); // Recovered
00244   c.expectEmpty(9);
00245   c.tm.dataCallback(11, 6);
00246   c.expectHead(10.2, 6); 
00247   c.expectEmpty(11);
00248   c.tm.dataCallback(13, 7);
00249   c.expectHead(12.2, 7); 
00250   c.expectEmpty(13);
00251 }
00252 
00253 TEST(QueuedTriggerMatcher, TriggerQueueOverflow)
00254 {
00255   QueuedChecker c;
00256 
00257   double trig_time = 4;
00258   for (int i = 0; i < 15; i++)
00259   {
00260     c.tm.triggerCallback(trig_time);
00261     c.expectEmpty(trig_time);
00262     trig_time += 2;
00263   }
00264   c.tm.dataCallback(15, 15);
00265   c.expectHead(14.2, 15); // Previous triggers were dropped
00266   c.expectEmpty(15);
00267 }
00268 
00269 TEST(QueuedTriggerMatcher, DataQueueOverflow)
00270 {
00271   QueuedChecker c;
00272 
00273   double data_time = 5;
00274   for (int i = 0; i < 10; i++)
00275   {
00276     c.tm.dataCallback(data_time, data_time);
00277     c.expectEmpty(data_time);
00278     data_time += 2;
00279   }
00280   c.tm.triggerCallback(14);
00281   c.expectHead(14.2, 15); // Previous triggers were dropped
00282   c.expectEmpty(14);
00283 }
00284 
00285 TEST(TriggerMatcher, TimeoutCheck)
00286 {
00287   timestamp_tools::TriggerMatcher tm(1, 10);
00288 
00289   // If this does not return because the timestamp
00290   EXPECT_EQ(timestamp_tools::TriggerMatcher::RetryLater, tm.getTimestampBlocking(ros::Time(0), 0.5));
00291 }
00292 
00293 TEST(TriggerMatcher, TriggerFirstCheck)
00294 {
00295   timestamp_tools::TriggerMatcher tm(1, 10);
00296 
00297   tm.triggerCallback(1);
00298   tm.triggerCallback(2);
00299   tm.triggerCallback(3);
00300 
00301   EXPECT_EQ(ros::Time(1), tm.getTimestampBlocking(ros::Time(1.5), 1)) << "Testing getTimestampBlocking without timeout";
00302   EXPECT_EQ(ros::Time(2), tm.getTimestampBlocking(ros::Time(2.5))) << "Testing getTimestampBlocking with timeout";
00303 }
00304 
00305 TEST(TriggerMatcher, TestReset)
00306 {
00307   timestamp_tools::TriggerMatcher tm(1, 10);
00308 
00309   tm.triggerCallback(1);
00310   tm.triggerCallback(2);
00311   tm.reset();
00312   tm.triggerCallback(3);
00313   tm.triggerCallback(4);
00314 
00315   EXPECT_EQ(timestamp_tools::TriggerMatcher::DropData, tm.getTimestampBlocking(ros::Time(1.5), 1)) << "Testing getTimestampBlocking without timeout";
00316   EXPECT_EQ(ros::Time(3), tm.getTimestampBlocking(ros::Time(3.5))) << "Testing getTimestampBlocking with timeout";
00317 }
00318 
00319 
00320 void AsyncGenTrigger(timestamp_tools::TriggerMatcher *tm, double time, int delay)
00321 {
00322   sleep(delay);
00323   tm->triggerCallback(time);
00324 }
00325 
00326 TEST(TriggerMatcher, DataFirstCheck)
00327 {
00328   timestamp_tools::TriggerMatcher tm(1, 10);
00329 
00330   tm.triggerCallback(5);
00331   boost::function<void(void)> agt = boost::bind(&AsyncGenTrigger, &tm, 7.0, 2);
00332   boost::thread trigger_thread(agt);
00333 
00334   EXPECT_EQ(timestamp_tools::TriggerMatcher::RetryLater, tm.getTimestampBlocking(ros::Time(5.5), 0.5)) << "getTimestampBlocking should have timed out or test computer is VERY slow";
00335   EXPECT_EQ(ros::Time(5), tm.getTimestampBlocking(ros::Time(6.0))) << "getTimestampBlocking should have received timestamp";
00336 
00337   trigger_thread.join();
00338 }
00339 
00340 
00341 int main(int argc, char **argv){
00342   for (int i = 0; i < argc; i++)
00343     printf("%s\n", argv[i]);
00344   testing::InitGoogleTest(&argc, argv);
00345   return RUN_ALL_TESTS();
00346 }
test_trigger_matcher.cpp
