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 <gtest/gtest.h>
00037 #include <ros/callback_queue.h>
00038 #include <ros/console.h>
00039 #include <ros/timer.h>
00040
00041 #include <boost/shared_ptr.hpp>
00042 #include <boost/bind.hpp>
00043 #include <boost/thread.hpp>
00044 #include <boost/function.hpp>
00045
00046 using namespace ros;
00047
00048 class CountingCallback : public CallbackInterface
00049 {
00050 public:
00051 CountingCallback()
00052 : count(0)
00053 {}
00054
00055 virtual CallResult call()
00056 {
00057 boost::mutex::scoped_lock lock(mutex);
00058 ++count;
00059
00060 return Success;
00061 }
00062
00063 boost::mutex mutex;
00064 size_t count;
00065 };
00066 typedef boost::shared_ptr<CountingCallback> CountingCallbackPtr;
00067
00068 TEST(CallbackQueue, singleCallback)
00069 {
00070 CountingCallbackPtr cb(new CountingCallback);
00071 CallbackQueue queue;
00072 queue.addCallback(cb, 1);
00073 queue.callOne();
00074
00075 EXPECT_EQ(cb->count, 1U);
00076
00077 queue.addCallback(cb, 1);
00078 queue.callAvailable();
00079
00080 EXPECT_EQ(cb->count, 2U);
00081
00082 queue.callOne();
00083 EXPECT_EQ(cb->count, 2U);
00084
00085 queue.callAvailable();
00086 EXPECT_EQ(cb->count, 2U);
00087 }
00088
00089 TEST(CallbackQueue, multipleCallbacksCallAvailable)
00090 {
00091 CountingCallbackPtr cb(new CountingCallback);
00092 CallbackQueue queue;
00093 for (uint32_t i = 0; i < 1000; ++i)
00094 {
00095 queue.addCallback(cb, 1);
00096 }
00097
00098 queue.callAvailable();
00099
00100 EXPECT_EQ(cb->count, 1000U);
00101 }
00102
00103 TEST(CallbackQueue, multipleCallbacksCallOne)
00104 {
00105 CountingCallbackPtr cb(new CountingCallback);
00106 CallbackQueue queue;
00107 for (uint32_t i = 0; i < 1000; ++i)
00108 {
00109 queue.addCallback(cb, 1);
00110 }
00111
00112 for (uint32_t i = 0; i < 1000; ++i)
00113 {
00114 queue.callOne();
00115 EXPECT_EQ(cb->count, i + 1);
00116 }
00117 }
00118
00119 TEST(CallbackQueue, remove)
00120 {
00121 CountingCallbackPtr cb1(new CountingCallback);
00122 CountingCallbackPtr cb2(new CountingCallback);
00123 CallbackQueue queue;
00124 queue.addCallback(cb1, 1);
00125 queue.addCallback(cb2, 2);
00126 queue.removeByID(1);
00127 queue.callAvailable();
00128
00129 EXPECT_EQ(cb1->count, 0U);
00130 EXPECT_EQ(cb2->count, 1U);
00131 }
00132
00133 class SelfRemovingCallback : public CallbackInterface
00134 {
00135 public:
00136 SelfRemovingCallback(CallbackQueue* queue, uint64_t id)
00137 : count(0)
00138 , queue(queue)
00139 , id(id)
00140 {}
00141
00142 virtual CallResult call()
00143 {
00144 ++count;
00145
00146 queue->removeByID(id);
00147
00148 return Success;
00149 }
00150
00151 size_t count;
00152
00153 CallbackQueue* queue;
00154 uint64_t id;
00155 };
00156 typedef boost::shared_ptr<SelfRemovingCallback> SelfRemovingCallbackPtr;
00157
00158 TEST(CallbackQueue, removeSelf)
00159 {
00160 CallbackQueue queue;
00161 SelfRemovingCallbackPtr cb1(new SelfRemovingCallback(&queue, 1));
00162 CountingCallbackPtr cb2(new CountingCallback());
00163 queue.addCallback(cb1, 1);
00164 queue.addCallback(cb2, 1);
00165 queue.addCallback(cb2, 1);
00166
00167 queue.callOne();
00168
00169 queue.addCallback(cb2, 1);
00170
00171 queue.callAvailable();
00172
00173 EXPECT_EQ(cb1->count, 1U);
00174 EXPECT_EQ(cb2->count, 1U);
00175 }
00176
00177 class RecursiveCallback : public CallbackInterface
00178 {
00179 public:
00180 RecursiveCallback(CallbackQueue* queue, bool use_available)
00181 : count(0)
00182 , queue(queue)
00183 , use_available(use_available)
00184 {}
00185
00186 virtual CallResult call()
00187 {
00188 ++count;
00189
00190 if (count < 3)
00191 {
00192 if (use_available)
00193 {
00194 queue->callAvailable();
00195 }
00196 else
00197 {
00198 queue->callOne();
00199 }
00200 }
00201
00202 return Success;
00203 }
00204
00205 size_t count;
00206
00207 CallbackQueue* queue;
00208 bool use_available;
00209 };
00210 typedef boost::shared_ptr<RecursiveCallback> RecursiveCallbackPtr;
00211
00212 TEST(CallbackQueue, recursive1)
00213 {
00214 CallbackQueue queue;
00215 RecursiveCallbackPtr cb(new RecursiveCallback(&queue, true));
00216 queue.addCallback(cb, 1);
00217 queue.addCallback(cb, 1);
00218 queue.addCallback(cb, 1);
00219 queue.callAvailable();
00220
00221 EXPECT_EQ(cb->count, 3U);
00222 }
00223
00224 TEST(CallbackQueue, recursive2)
00225 {
00226 CallbackQueue queue;
00227 RecursiveCallbackPtr cb(new RecursiveCallback(&queue, false));
00228 queue.addCallback(cb, 1);
00229 queue.addCallback(cb, 1);
00230 queue.addCallback(cb, 1);
00231 queue.callOne();
00232
00233 EXPECT_EQ(cb->count, 3U);
00234 }
00235
00236 TEST(CallbackQueue, recursive3)
00237 {
00238 CallbackQueue queue;
00239 RecursiveCallbackPtr cb(new RecursiveCallback(&queue, false));
00240 queue.addCallback(cb, 1);
00241 queue.addCallback(cb, 1);
00242 queue.addCallback(cb, 1);
00243 queue.callAvailable();
00244
00245 EXPECT_EQ(cb->count, 3U);
00246 }
00247
00248 TEST(CallbackQueue, recursive4)
00249 {
00250 CallbackQueue queue;
00251 RecursiveCallbackPtr cb(new RecursiveCallback(&queue, true));
00252 queue.addCallback(cb, 1);
00253 queue.addCallback(cb, 1);
00254 queue.addCallback(cb, 1);
00255 queue.callOne();
00256
00257 EXPECT_EQ(cb->count, 3U);
00258 }
00259
00260 void callAvailableThread(CallbackQueue* queue, bool& done)
00261 {
00262 while (!done)
00263 {
00264 queue->callAvailable(ros::WallDuration(0.1));
00265 }
00266 }
00267
00268 size_t runThreadedTest(const CountingCallbackPtr& cb, const boost::function<void(CallbackQueue*, bool&)>& threadFunc)
00269 {
00270 CallbackQueue queue;
00271 boost::thread_group tg;
00272 bool done = false;
00273
00274 for (uint32_t i = 0; i < 10; ++i)
00275 {
00276 tg.create_thread(boost::bind(threadFunc, &queue, boost::ref(done)));
00277 }
00278
00279 ros::WallTime start = ros::WallTime::now();
00280 size_t i = 0;
00281 while (ros::WallTime::now() - start < ros::WallDuration(5))
00282 {
00283 queue.addCallback(cb);
00284 ++i;
00285 }
00286
00287 while (!queue.isEmpty())
00288 {
00289 ros::WallDuration(0.01).sleep();
00290 }
00291
00292 done = true;
00293 tg.join_all();
00294
00295 return i;
00296 }
00297
00298 TEST(CallbackQueue, threadedCallAvailable)
00299 {
00300 CountingCallbackPtr cb(new CountingCallback);
00301 size_t i = runThreadedTest(cb, callAvailableThread);
00302 ROS_INFO_STREAM(i);
00303 EXPECT_EQ(cb->count, i);
00304 }
00305
00306 void callOneThread(CallbackQueue* queue, bool& done)
00307 {
00308 while (!done)
00309 {
00310 queue->callOne(ros::WallDuration(0.1));
00311 }
00312 }
00313
00314 TEST(CallbackQueue, threadedCallOne)
00315 {
00316 CountingCallbackPtr cb(new CountingCallback);
00317 size_t i = runThreadedTest(cb, callOneThread);
00318 ROS_INFO_STREAM(i);
00319 EXPECT_EQ(cb->count, i);
00320 }
00321
00322
00323
00324 namespace ros
00325 {
00326 class NodeHandle
00327 {
00328 public:
00329 static Timer createTimer(const TimerOptions& ops)
00330 {
00331 return Timer(ops);
00332 }
00333 };
00334 }
00335
00336 void dummyTimer(const ros::TimerEvent&)
00337 {
00338 }
00339
00340 CallbackQueueInterface* recursiveTimerQueue;
00341
00342 void recursiveTimer(const ros::TimerEvent&)
00343 {
00344
00345 WallDuration(2).sleep();
00346
00347 TimerOptions ops(Duration(0.1), dummyTimer, recursiveTimerQueue, false, false);
00348 Timer t = ros::NodeHandle::createTimer(ops);
00349 t.start();
00350 }
00351
00352 class TimerRecursionCallback : public CallbackInterface
00353 {
00354 public:
00355 TimerRecursionCallback(CallbackQueueInterface* _queue)
00356 : queue(_queue)
00357 {}
00358
00359 virtual CallResult call()
00360 {
00361 TimerOptions ops(Duration(0.1), recursiveTimer, queue, false, false);
00362 Timer t = ros::NodeHandle::createTimer(ops);
00363 t.start();
00364
00365
00366 WallDuration(1).sleep();
00367
00368 return Success;
00369 }
00370
00371 CallbackQueueInterface* queue;
00372 };
00373 typedef boost::shared_ptr<TimerRecursionCallback> TimerRecursionCallbackPtr;
00374
00375 TEST(CallbackQueue, recursiveTimer)
00376 {
00377
00378 ros::Time::init();
00379 CallbackQueue queue;
00380 recursiveTimerQueue = &queue;
00381 TimerRecursionCallbackPtr cb(new TimerRecursionCallback(&queue));
00382 queue.addCallback(cb, 1);
00383
00384 boost::thread_group tg;
00385 bool done = false;
00386
00387 for (uint32_t i = 0; i < 2; ++i)
00388 {
00389 tg.create_thread(boost::bind(callOneThread, &queue, boost::ref(done)));
00390 }
00391
00392 while (!queue.isEmpty())
00393 {
00394 ros::WallDuration(0.01).sleep();
00395 }
00396
00397 done = true;
00398 tg.join_all();
00399 }
00400
00401 int main(int argc, char** argv)
00402 {
00403 testing::InitGoogleTest(&argc, argv);
00404 return RUN_ALL_TESTS();
00405 }
00406
00407
00408