timer_callbacks.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 /* Author: Josh Faust */
00031 
00032 /*
00033  * Test timer callbacks
00034  */
00035 
00036 #include <string>
00037 #include <sstream>
00038 #include <fstream>
00039 
00040 #include <gtest/gtest.h>
00041 
00042 #include <time.h>
00043 #include <stdlib.h>
00044 
00045 #include "ros/ros.h"
00046 #include "ros/callback_queue.h"
00047 #include <test_roscpp/TestArray.h>
00048 #include <test_roscpp/TestStringString.h>
00049 
00050 #include <boost/scoped_ptr.hpp>
00051 
00052 using namespace ros;
00053 using namespace test_roscpp;
00054 
00055 std::string g_node_name = "test_timer_callbacks";
00056 
00057 class WallTimerHelper
00058 {
00059 public:
00060   WallTimerHelper(float period, bool oneshot = false)
00061   : expected_period_(period)
00062   , failed_(false)
00063   , total_calls_(0)
00064   , testing_period_(false)
00065   , calls_before_testing_period_(0)
00066   {
00067     NodeHandle n;
00068     timer_ = n.createWallTimer(expected_period_, &WallTimerHelper::callback, this, oneshot);
00069   }
00070 
00071   void callback(const WallTimerEvent& e)
00072   {
00073     bool first = last_call_.isZero();
00074     WallTime last_call = last_call_;
00075     last_call_ = WallTime::now();
00076     WallTime start = last_call_;
00077 
00078     if (!first)
00079     {
00080       if (fabsf(expected_next_call_.toSec() - start.toSec()) > 0.1f)
00081       {
00082         ROS_ERROR("Call came at wrong time (%f vs. %f)", expected_next_call_.toSec(), start.toSec());
00083         failed_ = true;
00084       }
00085     }
00086 
00087     if(testing_period_)
00088     {
00089 
00090       // Inside callback, less than current period, reset=false
00091       if(total_calls_ == calls_before_testing_period_)
00092       {
00093         WallDuration p(0.5);
00094         pretendWork(0.15);
00095         setPeriod(p);
00096       }
00097       
00098       // Inside callback, greater than current period, reset=false
00099       else if(total_calls_ == (calls_before_testing_period_+1))
00100       {
00101         WallDuration p(0.25);
00102         pretendWork(0.15);
00103         setPeriod(p);
00104       }
00105       
00106       // Inside callback, less than current period, reset=true
00107       else if(total_calls_ == (calls_before_testing_period_+2))
00108       {
00109         WallDuration p(0.5);
00110         pretendWork(0.15);
00111         setPeriod(p, true);
00112       }
00113       
00114       // Inside callback, greater than current period, reset=true
00115       else if(total_calls_ == (calls_before_testing_period_+3))
00116       {
00117         WallDuration p(0.25);
00118         pretendWork(0.15);
00119         setPeriod(p, true);
00120       }
00121     }
00122     else
00123     { 
00124       expected_next_call_ = e.current_expected + expected_period_;
00125     }
00126 
00127     WallTime end = WallTime::now();
00128     last_duration_ = end - start;
00129 
00130     ++total_calls_;
00131   }
00132 
00133   void setPeriod(const WallDuration p, bool reset=false)
00134   {
00135     if(reset)
00136     {
00137       expected_next_call_ = WallTime::now() + p;
00138     }
00139     else
00140     {
00141       expected_next_call_ = last_call_ + p;
00142     }
00143     
00144     timer_.setPeriod(p, reset);
00145     expected_period_ = p;
00146   }
00147 
00148 
00149   void pretendWork(const float t)
00150   {
00151     ros::Rate r(1. / t);
00152     r.sleep();
00153   }
00154 
00155   WallTime last_call_;
00156   WallTime expected_next_call_;
00157   WallDuration expected_period_;
00158   WallDuration last_duration_;
00159 
00160   bool failed_;
00161 
00162   WallTimer timer_;
00163   int32_t total_calls_;
00164 
00165   bool testing_period_;
00166   int  calls_before_testing_period_;
00167 };
00168 
00169 TEST(RoscppTimerCallbacks, singleWallTimeCallback)
00170 {
00171   NodeHandle n;
00172   WallTimerHelper helper1(0.01);
00173 
00174   WallDuration d(0.001f);
00175   for (int32_t i = 0; i < 1000 && n.ok(); ++i)
00176   {
00177     spinOnce();
00178     d.sleep();
00179   }
00180 
00181   if (helper1.failed_)
00182   {
00183     FAIL();
00184   }
00185 
00186   if (helper1.total_calls_ < 99)
00187   {
00188     ROS_ERROR("Total calls: %d (expected at least 100)", helper1.total_calls_);
00189     FAIL();
00190   }
00191 }
00192 
00193 TEST(RoscppTimerCallbacks, multipleWallTimeCallbacks)
00194 {
00195   NodeHandle n;
00196   const int count = 100;
00197   typedef boost::scoped_ptr<WallTimerHelper> HelperPtr;
00198   HelperPtr helpers[count];
00199   for (int i = 0; i < count; ++i)
00200   {
00201     helpers[i].reset(new WallTimerHelper((float)(i + 1) * 0.1f));
00202   }
00203 
00204   WallDuration d(0.01f);
00205   const int spin_count = 1000;
00206   for (int32_t i = 0; i < spin_count && n.ok(); ++i)
00207   {
00208     spinOnce();
00209     d.sleep();
00210   }
00211 
00212   for (int i = 0; i < count; ++i)
00213   {
00214     if (helpers[i]->failed_)
00215     {
00216       ROS_ERROR("Helper %d failed", i);
00217       FAIL();
00218     }
00219 
00220     int32_t expected_count = (spin_count * d.toSec()) / helpers[i]->expected_period_.toSec();
00221     if (helpers[i]->total_calls_ < (expected_count - 1))
00222     {
00223       ROS_ERROR("Helper %d total calls: %d (at least %d expected)", i, helpers[i]->total_calls_, expected_count);
00224       FAIL();
00225     }
00226   }
00227 }
00228 
00229 TEST(RoscppTimerCallbacks, setPeriod)
00230 {
00231   NodeHandle n;
00232   WallDuration    period(0.5);
00233   WallTimerHelper helper(period.toSec());
00234   Rate            r(100);
00235 
00236   // Let the callback occur once before getting started
00237   while(helper.total_calls_ < 1)
00238   {
00239     spinOnce();
00240     r.sleep();
00241   }
00242 
00243   helper.pretendWork(0.1);
00244   
00245   // outside callback, new period < old period, reset = false
00246   Time          start = Time::now();
00247   Duration      wait(0.5);
00248   WallDuration  p(0.25);
00249   helper.setPeriod(p);
00250   while(helper.total_calls_ < 2)
00251   {
00252     spinOnce();
00253     r.sleep();
00254   }
00255   
00256   helper.pretendWork(0.1);
00257   
00258   // outside callback, new period > old period, reset = false
00259   WallDuration p2(0.5);
00260   start = Time::now();
00261   helper.setPeriod(p);
00262   while(helper.total_calls_ < 3)
00263   {
00264     spinOnce();
00265     r.sleep();
00266   }
00267   
00268   helper.pretendWork(0.1);
00269   
00270   // outside callback, new period < old period, reset = true
00271   WallDuration p3(0.25);
00272   start = Time::now();
00273   helper.setPeriod(p, true);
00274   while(helper.total_calls_ < 4)
00275   {
00276     spinOnce();
00277     r.sleep();
00278   }
00279   
00280   helper.pretendWork(0.1);
00281   
00282   // outside callback, new period > old period, reset = true
00283   WallDuration p4(0.5);
00284   start = Time::now();
00285   helper.setPeriod(p, true);
00286   while(helper.total_calls_ < 5)
00287   {
00288     spinOnce();
00289     r.sleep();
00290   }
00291 
00292   // Test calling setPeriod inside callback
00293   helper.calls_before_testing_period_ = helper.total_calls_;
00294   int total = helper.total_calls_ + 5;
00295   helper.testing_period_ = true;
00296   while(helper.total_calls_ < total)
00297   {
00298     spinOnce();
00299     r.sleep();
00300   }
00301   helper.testing_period_ = false;
00302 
00303 
00304   if(helper.failed_)
00305   {
00306     ROS_ERROR("Helper failed in setPeriod");
00307     FAIL();
00308   }
00309 }
00310 
00311 TEST(RoscppTimerCallbacks, stopWallTimer)
00312 {
00313   NodeHandle n;
00314   WallTimerHelper helper(0.001);
00315 
00316   for (int32_t i = 0; i < 1000 && n.ok(); ++i)
00317   {
00318     WallDuration(0.001).sleep();
00319     spinOnce();
00320   }
00321 
00322   ASSERT_GT(helper.total_calls_, 0);
00323   int32_t last_count = helper.total_calls_;
00324   helper.timer_.stop();
00325 
00326   for (int32_t i = 0; i < 1000 && n.ok(); ++i)
00327   {
00328     WallDuration(0.001).sleep();
00329     spinOnce();
00330   }
00331 
00332   ASSERT_EQ(last_count, helper.total_calls_);
00333 }
00334 
00335 int32_t g_count = 0;
00336 void timerCallback(const ros::WallTimerEvent&)
00337 {
00338   ++g_count;
00339 }
00340 
00341 TEST(RoscppTimerCallbacks, stopThenSpin)
00342 {
00343   g_count = 0;
00344   NodeHandle n;
00345   ros::WallTimer timer = n.createWallTimer(ros::WallDuration(0.001), timerCallback);
00346 
00347   WallDuration(0.1).sleep();
00348   timer.stop();
00349 
00350   spinOnce();
00351 
00352   ASSERT_EQ(g_count, 0);
00353 }
00354 
00355 TEST(RoscppTimerCallbacks, oneShotWallTimer)
00356 {
00357   NodeHandle n;
00358   WallTimerHelper helper(0.001, true);
00359 
00360   for (int32_t i = 0; i < 1000 && n.ok(); ++i)
00361   {
00362     WallDuration(0.001).sleep();
00363     spinOnce();
00364   }
00365 
00366   ASSERT_EQ(helper.total_calls_, 1);
00367 }
00368 
00369 class TimerHelper
00370 {
00371 public:
00372   TimerHelper(Duration period, bool oneshot = false)
00373     : failed_(false)
00374     , expected_period_(period)
00375     , total_calls_(0)
00376   {
00377     NodeHandle n;
00378     timer_ = n.createTimer(expected_period_, &TimerHelper::callback, this, oneshot);
00379   }
00380 
00381   TimerHelper(Rate r, bool oneshot = false)
00382     : failed_(false)
00383     , expected_period_(r.expectedCycleTime())
00384     , total_calls_(0)
00385   {
00386     NodeHandle n;
00387     timer_ = n.createTimer(r, &TimerHelper::callback, this, oneshot);
00388   }
00389 
00390   void callback(const TimerEvent&)
00391   {
00392     ++total_calls_;
00393   }
00394 
00395   bool failed_;
00396 
00397   Duration expected_period_;
00398 
00399   Timer timer_;
00400   int32_t total_calls_;
00401 };
00402 
00403 TEST(RoscppTimerCallbacks, singleROSTimeCallback)
00404 {
00405   NodeHandle n;
00406 
00407   Time now(1, 0);
00408   Time::setNow(now);
00409 
00410   TimerHelper helper(Duration(0, 10000000));
00411 
00412   Duration d(0, 1000000);
00413   for (int32_t i = 0; i < 1000 && n.ok(); ++i)
00414   {
00415     now += d;
00416     Time::setNow(now);
00417 
00418     while (helper.timer_.hasPending())
00419     {
00420       WallDuration(0.001).sleep();
00421       spinOnce();
00422     }
00423   }
00424 
00425   if (helper.failed_)
00426   {
00427     FAIL();
00428   }
00429 
00430   if (helper.total_calls_ != 100)
00431   {
00432     ROS_ERROR("Total calls: %d (expected 100)", helper.total_calls_);
00433     FAIL();
00434   }
00435 }
00436 
00437 TEST(RoscppTimerCallbacks, singleROSTimeCallbackFromRate)
00438 {
00439   NodeHandle n;
00440 
00441   Time now(1, 0);
00442   Time::setNow(now);
00443 
00444   TimerHelper helper(Rate(100));
00445 
00446   Duration d(0, 1000000);
00447   for (int32_t i = 0; i < 1000 && n.ok(); ++i)
00448   {
00449     now += d;
00450     Time::setNow(now);
00451 
00452     while (helper.timer_.hasPending())
00453     {
00454       WallDuration(0.00025).sleep();
00455       spinOnce();
00456     }
00457   }
00458 
00459   if (helper.failed_)
00460   {
00461     FAIL();
00462   }
00463 
00464   if (helper.total_calls_ != 100)
00465   {
00466     ROS_ERROR("Total calls: %d (expected 100)", helper.total_calls_);
00467     FAIL();
00468   }
00469 }
00470 
00471 TEST(RoscppTimerCallbacks, oneshotROSTimer)
00472 {
00473   NodeHandle n;
00474 
00475   Time now(1, 0);
00476   Time::setNow(now);
00477 
00478   TimerHelper helper(Duration(0, 10000000), true);
00479 
00480   Duration d(0, 1000000);
00481   for (int32_t i = 0; i < 1000 && n.ok(); ++i)
00482   {
00483     now += d;
00484     Time::setNow(now);
00485 
00486     while (helper.timer_.hasPending())
00487     {
00488       WallDuration(0.001).sleep();
00489       spinOnce();
00490     }
00491   }
00492 
00493   if (helper.failed_)
00494   {
00495     FAIL();
00496   }
00497 
00498   ASSERT_EQ(helper.total_calls_, 1);
00499 }
00500 
00501 TEST(RoscppTimerCallbacks, singleROSTimeCallbackLargeTimestep)
00502 {
00503   NodeHandle n;
00504 
00505   Time now(1, 0);
00506   Time::setNow(now);
00507 
00508   TimerHelper helper(Duration(0, 10000000));
00509 
00510   Duration d(0, 100000000);
00511   for (int32_t i = 0; i < 100 && n.ok(); ++i)
00512   {
00513     now += d;
00514     Time::setNow(now);
00515 
00516     while (helper.timer_.hasPending())
00517     {
00518       WallDuration(0.001).sleep();
00519       spinOnce();
00520     }
00521   }
00522 
00523   if (helper.failed_)
00524   {
00525     FAIL();
00526   }
00527 
00528   if (helper.total_calls_ != 200)
00529   {
00530     ROS_ERROR("Total calls: %d (expected 200)", helper.total_calls_);
00531     FAIL();
00532   }
00533 }
00534 
00535 TEST(RoscppTimerCallbacks, multipleROSTimeCallbacks)
00536 {
00537   NodeHandle n;
00538 
00539   Time now(1, 0);
00540   Time::setNow(now);
00541 
00542   const int count = 100;
00543   typedef boost::scoped_ptr<TimerHelper> HelperPtr;
00544   HelperPtr helpers[count];
00545   for (int i = 0; i < count; ++i)
00546   {
00547     helpers[i].reset(new TimerHelper(Duration((float)(i + 1) * 0.01f)));
00548   }
00549 
00550   Duration d(0.001f);
00551   const int spin_count = 1000;
00552   for (int32_t i = 0; i < spin_count && n.ok(); ++i)
00553   {
00554     now += d;
00555     Time::setNow(now);
00556 
00557     bool pending = false;
00558 
00559     do
00560     {
00561       pending = false;
00562       for (int i = 0; i < count; ++i)
00563       {
00564         pending |= helpers[i]->timer_.hasPending();
00565       }
00566 
00567       WallDuration(0.001).sleep();
00568       spinOnce();
00569     } while (pending);
00570   }
00571 
00572   for (int i = 0; i < count; ++i)
00573   {
00574     if (helpers[i]->failed_)
00575     {
00576       ROS_ERROR("Helper %d failed", i);
00577       FAIL();
00578     }
00579 
00580     int32_t expected_count = (spin_count * d.toSec()) / helpers[i]->expected_period_.toSec();
00581     if (helpers[i]->total_calls_ < (expected_count - 1) || helpers[i]->total_calls_ > (expected_count + 1))
00582     {
00583       ROS_ERROR("Helper %d total calls: %d (%d expected)", i, helpers[i]->total_calls_, expected_count);
00584       FAIL();
00585     }
00586   }
00587 }
00588 
00589 class Tracked
00590 {
00591 public:
00592   Tracked()
00593   {
00594     g_count = 0;
00595   }
00596 
00597   void callback(const TimerEvent&)
00598   {
00599     ++g_count;
00600   }
00601 };
00602 
00603 TEST(RoscppTimerCallbacks, trackedObject)
00604 {
00605   NodeHandle n;
00606   Time now(1, 0);
00607   Time::setNow(now);
00608 
00609   boost::shared_ptr<Tracked> tracked(boost::make_shared<Tracked>());
00610   Timer timer = n.createTimer(Duration(0.001), &Tracked::callback, tracked);
00611 
00612   now += Duration(0.1);
00613   Time::setNow(now);
00614 
00615   while (timer.hasPending())
00616   {
00617     WallDuration(0.001).sleep();
00618     spinOnce();
00619   }
00620 
00621   ASSERT_GT(g_count, 0);
00622   int32_t last_count = g_count;
00623   tracked.reset();
00624 
00625   now += Duration(0.1);
00626   Time::setNow(now);
00627 
00628   while (timer.hasPending())
00629   {
00630     WallDuration(0.001).sleep();
00631     spinOnce();
00632   }
00633 
00634   ASSERT_EQ(last_count, g_count);
00635 }
00636 
00637 TEST(RoscppTimerCallbacks, stopROSTimer)
00638 {
00639   NodeHandle n;
00640   Time now(1, 0);
00641   Time::setNow(now);
00642 
00643   TimerHelper helper(Duration(0.001));
00644 
00645   now += Duration(0.1);
00646   Time::setNow(now);
00647 
00648   while (helper.timer_.hasPending())
00649   {
00650     WallDuration(0.001).sleep();
00651     spinOnce();
00652   }
00653 
00654   ASSERT_GT(helper.total_calls_, 0);
00655   int32_t last_count = helper.total_calls_;
00656   helper.timer_.stop();
00657 
00658   now += Duration(0.1);
00659   Time::setNow(now);
00660 
00661   while (helper.timer_.hasPending())
00662   {
00663     WallDuration(0.001).sleep();
00664     spinOnce();
00665   }
00666 
00667   ASSERT_EQ(last_count, helper.total_calls_);
00668 }
00669 
00670 int main(int argc, char** argv)
00671 {
00672   testing::InitGoogleTest(&argc, argv);
00673   ros::init(argc, argv, g_node_name);
00674 
00675   return RUN_ALL_TESTS();
00676 }
00677 


test_roscpp
Author(s): Morgan Quigley, Josh Faust, Brian Gerkey, Troy Straszheim
autogenerated on Tue Mar 7 2017 03:45:23