00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
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