timer_callbacks.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2NULLNULL8, 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   {
00065     NodeHandle n;
00066     timer_ = n.createWallTimer(expected_period_, &WallTimerHelper::callback, this, oneshot);
00067   }
00068 
00069   void callback(const WallTimerEvent& e)
00070   {
00071     bool first = last_call_.isZero();
00072     WallTime last_call = last_call_;
00073     last_call_ = WallTime::now();
00074     WallTime start = last_call_;
00075 
00076     if (!first)
00077     {
00078       if (fabsf(expected_next_call_.toSec() - start.toSec()) > 0.1f)
00079       {
00080         ROS_ERROR("Call came at wrong time (%f vs. %f)", expected_next_call_.toSec(), start.toSec());
00081         failed_ = true;
00082       }
00083     }
00084 
00085     expected_next_call_ = e.current_expected + expected_period_;
00086 
00087     WallTime end = WallTime::now();
00088     last_duration_ = end - start;
00089 
00090     ++total_calls_;
00091   }
00092 
00093   WallTime last_call_;
00094   WallTime expected_next_call_;
00095   WallDuration expected_period_;
00096   WallDuration last_duration_;
00097 
00098   bool failed_;
00099 
00100   WallTimer timer_;
00101   int32_t total_calls_;
00102 };
00103 
00104 TEST(RoscppTimerCallbacks, singleWallTimeCallback)
00105 {
00106   NodeHandle n;
00107   WallTimerHelper helper1(0.01);
00108 
00109   WallDuration d(0.001f);
00110   for (int32_t i = 0; i < 1000 && n.ok(); ++i)
00111   {
00112     spinOnce();
00113     d.sleep();
00114   }
00115 
00116   if (helper1.failed_)
00117   {
00118     FAIL();
00119   }
00120 
00121   if (helper1.total_calls_ < 99)
00122   {
00123     ROS_ERROR("Total calls: %d (expected at least 100)", helper1.total_calls_);
00124     FAIL();
00125   }
00126 }
00127 
00128 TEST(RoscppTimerCallbacks, multipleWallTimeCallbacks)
00129 {
00130   NodeHandle n;
00131   const int count = 100;
00132   typedef boost::scoped_ptr<WallTimerHelper> HelperPtr;
00133   HelperPtr helpers[count];
00134   for (int i = 0; i < count; ++i)
00135   {
00136     helpers[i].reset(new WallTimerHelper((float)(i + 1) * 0.1f));
00137   }
00138 
00139   WallDuration d(0.01f);
00140   const int spin_count = 1000;
00141   for (int32_t i = 0; i < spin_count && n.ok(); ++i)
00142   {
00143     spinOnce();
00144     d.sleep();
00145   }
00146 
00147   for (int i = 0; i < count; ++i)
00148   {
00149     if (helpers[i]->failed_)
00150     {
00151       ROS_ERROR("Helper %d failed", i);
00152       FAIL();
00153     }
00154 
00155     int32_t expected_count = (spin_count * d.toSec()) / helpers[i]->expected_period_.toSec();
00156     if (helpers[i]->total_calls_ < (expected_count - 1))
00157     {
00158       ROS_ERROR("Helper %d total calls: %d (at least %d expected)", i, helpers[i]->total_calls_, expected_count);
00159       FAIL();
00160     }
00161   }
00162 }
00163 
00164 TEST(RoscppTimerCallbacks, stopWallTimer)
00165 {
00166   NodeHandle n;
00167   WallTimerHelper helper(0.001);
00168 
00169   for (int32_t i = 0; i < 1000 && n.ok(); ++i)
00170   {
00171     WallDuration(0.001).sleep();
00172     spinOnce();
00173   }
00174 
00175   ASSERT_GT(helper.total_calls_, 0);
00176   int32_t last_count = helper.total_calls_;
00177   helper.timer_.stop();
00178 
00179   for (int32_t i = 0; i < 1000 && n.ok(); ++i)
00180   {
00181     WallDuration(0.001).sleep();
00182     spinOnce();
00183   }
00184 
00185   ASSERT_EQ(last_count, helper.total_calls_);
00186 }
00187 
00188 int32_t g_count = 0;
00189 void timerCallback(const ros::WallTimerEvent& evt)
00190 {
00191   ++g_count;
00192 }
00193 
00194 TEST(RoscppTimerCallbacks, stopThenSpin)
00195 {
00196   g_count = 0;
00197   NodeHandle n;
00198   ros::WallTimer timer = n.createWallTimer(ros::WallDuration(0.001), timerCallback);
00199 
00200   WallDuration(0.1).sleep();
00201   timer.stop();
00202 
00203   spinOnce();
00204 
00205   ASSERT_EQ(g_count, 0);
00206 }
00207 
00208 TEST(RoscppTimerCallbacks, oneShotWallTimer)
00209 {
00210   NodeHandle n;
00211   WallTimerHelper helper(0.001, true);
00212 
00213   for (int32_t i = 0; i < 1000 && n.ok(); ++i)
00214   {
00215     WallDuration(0.001).sleep();
00216     spinOnce();
00217   }
00218 
00219   ASSERT_EQ(helper.total_calls_, 1);
00220 }
00221 
00222 class TimerHelper
00223 {
00224 public:
00225   TimerHelper(Duration period, bool oneshot = false)
00226     : failed_(false)
00227     , expected_period_(period)
00228     , total_calls_(0)
00229   {
00230     NodeHandle n;
00231     timer_ = n.createTimer(expected_period_, &TimerHelper::callback, this, oneshot);
00232   }
00233 
00234   TimerHelper(Rate r, bool oneshot = false)
00235     : failed_(false)
00236     , expected_period_(r.expectedCycleTime())
00237     , total_calls_(0)
00238   {
00239     NodeHandle n;
00240     timer_ = n.createTimer(r, &TimerHelper::callback, this, oneshot);
00241   }
00242 
00243   void callback(const TimerEvent& e)
00244   {
00245     ++total_calls_;
00246   }
00247 
00248   bool failed_;
00249 
00250   Duration expected_period_;
00251 
00252   Timer timer_;
00253   int32_t total_calls_;
00254 };
00255 
00256 TEST(RoscppTimerCallbacks, singleROSTimeCallback)
00257 {
00258   NodeHandle n;
00259 
00260   Time now(1, 0);
00261   Time::setNow(now);
00262 
00263   TimerHelper helper(Duration(0, 10000000));
00264 
00265   Duration d(0, 1000000);
00266   for (int32_t i = 0; i < 1000 && n.ok(); ++i)
00267   {
00268     now += d;
00269     Time::setNow(now);
00270 
00271     while (helper.timer_.hasPending())
00272     {
00273       WallDuration(0.001).sleep();
00274       spinOnce();
00275     }
00276   }
00277 
00278   if (helper.failed_)
00279   {
00280     FAIL();
00281   }
00282 
00283   if (helper.total_calls_ != 100)
00284   {
00285     ROS_ERROR("Total calls: %d (expected 100)", helper.total_calls_);
00286     FAIL();
00287   }
00288 }
00289 
00290 TEST(RoscppTimerCallbacks, singleROSTimeCallbackFromRate)
00291 {
00292   NodeHandle n;
00293 
00294   Time now(1, 0);
00295   Time::setNow(now);
00296 
00297   TimerHelper helper(Rate(100));
00298 
00299   Duration d(0, 1000000);
00300   for (int32_t i = 0; i < 1000 && n.ok(); ++i)
00301   {
00302     now += d;
00303     Time::setNow(now);
00304 
00305     while (helper.timer_.hasPending())
00306     {
00307       WallDuration(0.00025).sleep();
00308       spinOnce();
00309     }
00310   }
00311 
00312   if (helper.failed_)
00313   {
00314     FAIL();
00315   }
00316 
00317   if (helper.total_calls_ != 100)
00318   {
00319     ROS_ERROR("Total calls: %d (expected 100)", helper.total_calls_);
00320     FAIL();
00321   }
00322 }
00323 
00324 TEST(RoscppTimerCallbacks, oneshotROSTimer)
00325 {
00326   NodeHandle n;
00327 
00328   Time now(1, 0);
00329   Time::setNow(now);
00330 
00331   TimerHelper helper(Duration(0, 10000000), true);
00332 
00333   Duration d(0, 1000000);
00334   for (int32_t i = 0; i < 1000 && n.ok(); ++i)
00335   {
00336     now += d;
00337     Time::setNow(now);
00338 
00339     while (helper.timer_.hasPending())
00340     {
00341       WallDuration(0.001).sleep();
00342       spinOnce();
00343     }
00344   }
00345 
00346   if (helper.failed_)
00347   {
00348     FAIL();
00349   }
00350 
00351   ASSERT_EQ(helper.total_calls_, 1);
00352 }
00353 
00354 TEST(RoscppTimerCallbacks, singleROSTimeCallbackLargeTimestep)
00355 {
00356   NodeHandle n;
00357 
00358   Time now(1, 0);
00359   Time::setNow(now);
00360 
00361   TimerHelper helper(Duration(0, 10000000));
00362 
00363   Duration d(0, 100000000);
00364   for (int32_t i = 0; i < 100 && n.ok(); ++i)
00365   {
00366     now += d;
00367     Time::setNow(now);
00368 
00369     while (helper.timer_.hasPending())
00370     {
00371       WallDuration(0.001).sleep();
00372       spinOnce();
00373     }
00374   }
00375 
00376   if (helper.failed_)
00377   {
00378     FAIL();
00379   }
00380 
00381   if (helper.total_calls_ != 200)
00382   {
00383     ROS_ERROR("Total calls: %d (expected 200)", helper.total_calls_);
00384     FAIL();
00385   }
00386 }
00387 
00388 TEST(RoscppTimerCallbacks, multipleROSTimeCallbacks)
00389 {
00390   NodeHandle n;
00391 
00392   Time now(1, 0);
00393   Time::setNow(now);
00394 
00395   const int count = 100;
00396   typedef boost::scoped_ptr<TimerHelper> HelperPtr;
00397   HelperPtr helpers[count];
00398   for (int i = 0; i < count; ++i)
00399   {
00400     helpers[i].reset(new TimerHelper(Duration((float)(i + 1) * 0.01f)));
00401   }
00402 
00403   Duration d(0.001f);
00404   const int spin_count = 1000;
00405   for (int32_t i = 0; i < spin_count && n.ok(); ++i)
00406   {
00407     now += d;
00408     Time::setNow(now);
00409 
00410     bool pending = false;
00411 
00412     do
00413     {
00414       pending = false;
00415       for (int i = 0; i < count; ++i)
00416       {
00417         pending |= helpers[i]->timer_.hasPending();
00418       }
00419 
00420       WallDuration(0.001).sleep();
00421       spinOnce();
00422     } while (pending);
00423   }
00424 
00425   for (int i = 0; i < count; ++i)
00426   {
00427     if (helpers[i]->failed_)
00428     {
00429       ROS_ERROR("Helper %d failed", i);
00430       FAIL();
00431     }
00432 
00433     int32_t expected_count = (spin_count * d.toSec()) / helpers[i]->expected_period_.toSec();
00434     if (helpers[i]->total_calls_ < (expected_count - 1) || helpers[i]->total_calls_ > (expected_count + 1))
00435     {
00436       ROS_ERROR("Helper %d total calls: %d (%d expected)", i, helpers[i]->total_calls_, expected_count);
00437       FAIL();
00438     }
00439   }
00440 }
00441 
00442 class Tracked
00443 {
00444 public:
00445   Tracked()
00446   {
00447     g_count = 0;
00448   }
00449 
00450   void callback(const TimerEvent& e)
00451   {
00452     ++g_count;
00453   }
00454 };
00455 
00456 TEST(RoscppTimerCallbacks, trackedObject)
00457 {
00458   NodeHandle n;
00459   Time now(1, 0);
00460   Time::setNow(now);
00461 
00462   boost::shared_ptr<Tracked> tracked(new Tracked);
00463   Timer timer = n.createTimer(Duration(0.001), &Tracked::callback, tracked);
00464 
00465   now += Duration(0.1);
00466   Time::setNow(now);
00467 
00468   while (timer.hasPending())
00469   {
00470     WallDuration(0.001).sleep();
00471     spinOnce();
00472   }
00473 
00474   ASSERT_GT(g_count, 0);
00475   int32_t last_count = g_count;
00476   tracked.reset();
00477 
00478   now += Duration(0.1);
00479   Time::setNow(now);
00480 
00481   while (timer.hasPending())
00482   {
00483     WallDuration(0.001).sleep();
00484     spinOnce();
00485   }
00486 
00487   ASSERT_EQ(last_count, g_count);
00488 }
00489 
00490 TEST(RoscppTimerCallbacks, stopROSTimer)
00491 {
00492   NodeHandle n;
00493   Time now(1, 0);
00494   Time::setNow(now);
00495 
00496   TimerHelper helper(Duration(0.001));
00497 
00498   now += Duration(0.1);
00499   Time::setNow(now);
00500 
00501   while (helper.timer_.hasPending())
00502   {
00503     WallDuration(0.001).sleep();
00504     spinOnce();
00505   }
00506 
00507   ASSERT_GT(helper.total_calls_, 0);
00508   int32_t last_count = helper.total_calls_;
00509   helper.timer_.stop();
00510 
00511   now += Duration(0.1);
00512   Time::setNow(now);
00513 
00514   while (helper.timer_.hasPending())
00515   {
00516     WallDuration(0.001).sleep();
00517     spinOnce();
00518   }
00519 
00520   ASSERT_EQ(last_count, helper.total_calls_);
00521 }
00522 
00523 int main(int argc, char** argv)
00524 {
00525   testing::InitGoogleTest(&argc, argv);
00526   ros::init(argc, argv, g_node_name);
00527 
00528   return RUN_ALL_TESTS();
00529 }
00530 


test_roscpp
Author(s): Morgan Quigley, Josh Faust, Brian Gerkey, Troy Straszheim
autogenerated on Fri Aug 28 2015 12:33:59