9 #include "gtest/gtest.h"
12 #define private public
22 #include <std_msgs/Header.h>
23 #include <topic_tools/shape_shifter.h>
27 void spin(
double duration,
double wait)
30 while (waited < duration)
40 spin(duration, duration / 1000.0);
47 for (
const auto& sub : subs)
49 if (sub->getName() == topic)
51 num += sub->getNumCallbacks();
72 TEST(LazySubscriberBase, Test)
76 auto outPub = pnh.advertise<std_msgs::Header>(
"out", 10);
77 auto inPub = nh.
advertise<std_msgs::Header>(
"in", 10);
79 size_t numOutSubscribes = 0;
80 size_t numInReceived = 0;
81 size_t numOutReceived = 0;
82 boost::function<void(
const std_msgs::Header&)> relayCb = [&](
const std_msgs::Header& h)
87 boost::function<void(
const std_msgs::Header&)> outCb = [&](
const std_msgs::Header& h)
92 auto lazySub = std::make_unique<TestLazySubscriber<std_msgs::Header>>(
96 sub = nh.
subscribe<std_msgs::Header>(
"in", 10, relayCb);
100 EXPECT_EQ(0, numOutSubscribes);
101 EXPECT_EQ(0, numInReceived);
102 EXPECT_EQ(0, outPub.getNumSubscribers());
103 EXPECT_EQ(0, inPub.getNumSubscribers());
105 EXPECT_EQ(
false, lazySub->isSubscribed());
108 auto outSub = pnh.subscribe<std_msgs::Header>(
"out", 10, outCb);
111 EXPECT_EQ(1, numOutSubscribes);
112 EXPECT_EQ(0, numInReceived);
113 EXPECT_EQ(0, numOutReceived);
114 EXPECT_EQ(1, outPub.getNumSubscribers());
115 EXPECT_EQ(1, inPub.getNumSubscribers());
118 EXPECT_EQ(
true, lazySub->isSubscribed());
120 inPub.publish(std_msgs::Header());
123 EXPECT_EQ(1, numOutSubscribes);
124 EXPECT_EQ(1, numInReceived);
125 EXPECT_EQ(1, numOutReceived);
126 EXPECT_EQ(1, outPub.getNumSubscribers());
127 EXPECT_EQ(1, inPub.getNumSubscribers());
130 EXPECT_EQ(
true, lazySub->isSubscribed());
134 EXPECT_EQ(1, numOutSubscribes);
135 EXPECT_EQ(1, numInReceived);
136 EXPECT_EQ(1, numOutReceived);
137 EXPECT_EQ(0, outPub.getNumSubscribers());
138 EXPECT_EQ(0, inPub.getNumSubscribers());
141 EXPECT_EQ(
false, lazySub->isSubscribed());
143 inPub.publish(std_msgs::Header());
146 EXPECT_EQ(1, numOutSubscribes);
147 EXPECT_EQ(1, numInReceived);
148 EXPECT_EQ(1, numOutReceived);
149 EXPECT_EQ(0, outPub.getNumSubscribers());
150 EXPECT_EQ(0, inPub.getNumSubscribers());
153 EXPECT_EQ(
false, lazySub->isSubscribed());
156 auto outSub = pnh.subscribe<std_msgs::Header>(
"out", 10, outCb);
157 auto outSub2 = pnh.subscribe<std_msgs::Header>(
"out", 10, outCb);
160 EXPECT_EQ(2, numOutSubscribes);
161 EXPECT_EQ(1, numInReceived);
162 EXPECT_EQ(1, numOutReceived);
163 EXPECT_EQ(1, outPub.getNumSubscribers());
164 EXPECT_EQ(1, inPub.getNumSubscribers());
167 EXPECT_EQ(
true, lazySub->isSubscribed());
171 lazySub->connectCb({{}});
174 EXPECT_EQ(2, numOutSubscribes);
175 EXPECT_EQ(1, numInReceived);
176 EXPECT_EQ(1, numOutReceived);
177 EXPECT_EQ(1, outPub.getNumSubscribers());
178 EXPECT_EQ(1, inPub.getNumSubscribers());
181 EXPECT_EQ(
true, lazySub->isSubscribed());
183 inPub.publish(std_msgs::Header());
184 inPub.publish(std_msgs::Header());
187 EXPECT_EQ(2, numOutSubscribes);
188 EXPECT_EQ(3, numInReceived);
189 EXPECT_EQ(5, numOutReceived);
190 EXPECT_EQ(1, outPub.getNumSubscribers());
191 EXPECT_EQ(1, inPub.getNumSubscribers());
194 EXPECT_EQ(
true, lazySub->isSubscribed());
198 EXPECT_EQ(2, numOutSubscribes);
199 EXPECT_EQ(3, numInReceived);
200 EXPECT_EQ(5, numOutReceived);
201 EXPECT_EQ(0, outPub.getNumSubscribers());
202 EXPECT_EQ(0, inPub.getNumSubscribers());
205 EXPECT_EQ(
false, lazySub->isSubscribed());
207 inPub.publish(std_msgs::Header());
208 inPub.publish(std_msgs::Header());
209 inPub.publish(std_msgs::Header());
212 EXPECT_EQ(2, numOutSubscribes);
213 EXPECT_EQ(3, numInReceived);
214 EXPECT_EQ(5, numOutReceived);
215 EXPECT_EQ(0, outPub.getNumSubscribers());
216 EXPECT_EQ(0, inPub.getNumSubscribers());
219 EXPECT_EQ(
false, lazySub->isSubscribed());
222 auto outSub = pnh.subscribe<std_msgs::Header>(
"out", 10, outCb);
225 EXPECT_EQ(3, numOutSubscribes);
226 EXPECT_EQ(3, numInReceived);
227 EXPECT_EQ(5, numOutReceived);
228 EXPECT_EQ(1, outPub.getNumSubscribers());
229 EXPECT_EQ(1, inPub.getNumSubscribers());
232 EXPECT_EQ(
true, lazySub->isSubscribed());
238 EXPECT_EQ(3, numOutSubscribes);
239 EXPECT_EQ(3, numInReceived);
240 EXPECT_EQ(5, numOutReceived);
241 EXPECT_EQ(1, outPub.getNumSubscribers());
242 EXPECT_EQ(0, inPub.getNumSubscribers());
248 TEST(LazySubscriberBase, SetLazy)
252 auto outPub = pnh.advertise<std_msgs::Header>(
"out", 10);
253 auto inPub = nh.
advertise<std_msgs::Header>(
"in", 10);
255 size_t numOutSubscribes = 0;
256 size_t numInReceived = 0;
257 size_t numOutReceived = 0;
258 boost::function<void(
const std_msgs::Header&)> relayCb = [&](
const std_msgs::Header& h)
263 boost::function<void(
const std_msgs::Header&)> outCb = [&](
const std_msgs::Header& h)
268 auto lazySub = std::make_unique<TestLazySubscriber<std_msgs::Header>>(
272 sub = nh.
subscribe<std_msgs::Header>(
"in", 10, relayCb);
276 EXPECT_EQ(0, numOutSubscribes);
277 EXPECT_EQ(0, numInReceived);
278 EXPECT_EQ(0, numOutReceived);
279 EXPECT_EQ(0, outPub.getNumSubscribers());
280 EXPECT_EQ(0, inPub.getNumSubscribers());
283 EXPECT_EQ(
false, lazySub->isSubscribed());
284 EXPECT_EQ(
true, lazySub->isLazy());
286 lazySub->setLazy(
false);
289 EXPECT_EQ(1, numOutSubscribes);
290 EXPECT_EQ(0, numInReceived);
291 EXPECT_EQ(0, numOutReceived);
292 EXPECT_EQ(0, outPub.getNumSubscribers());
293 EXPECT_EQ(1, inPub.getNumSubscribers());
296 EXPECT_EQ(
true, lazySub->isSubscribed());
297 EXPECT_EQ(
false, lazySub->isLazy());
299 inPub.publish(std_msgs::Header());
302 EXPECT_EQ(1, numOutSubscribes);
303 EXPECT_EQ(1, numInReceived);
304 EXPECT_EQ(0, numOutReceived);
305 EXPECT_EQ(0, outPub.getNumSubscribers());
306 EXPECT_EQ(1, inPub.getNumSubscribers());
309 EXPECT_EQ(
true, lazySub->isSubscribed());
310 EXPECT_EQ(
false, lazySub->isLazy());
313 auto outSub = pnh.subscribe<std_msgs::Header>(
"out", 10, outCb);
316 EXPECT_EQ(1, numOutSubscribes);
317 EXPECT_EQ(1, numInReceived);
318 EXPECT_EQ(0, numOutReceived);
319 EXPECT_EQ(1, outPub.getNumSubscribers());
320 EXPECT_EQ(1, inPub.getNumSubscribers());
323 EXPECT_EQ(
true, lazySub->isSubscribed());
324 EXPECT_EQ(
false, lazySub->isLazy());
326 inPub.publish(std_msgs::Header());
329 EXPECT_EQ(1, numOutSubscribes);
330 EXPECT_EQ(2, numInReceived);
331 EXPECT_EQ(1, numOutReceived);
332 EXPECT_EQ(1, outPub.getNumSubscribers());
333 EXPECT_EQ(1, inPub.getNumSubscribers());
336 EXPECT_EQ(
true, lazySub->isSubscribed());
337 EXPECT_EQ(
false, lazySub->isLazy());
341 EXPECT_EQ(1, numOutSubscribes);
342 EXPECT_EQ(2, numInReceived);
343 EXPECT_EQ(1, numOutReceived);
344 EXPECT_EQ(0, outPub.getNumSubscribers());
345 EXPECT_EQ(1, inPub.getNumSubscribers());
348 EXPECT_EQ(
true, lazySub->isSubscribed());
349 EXPECT_EQ(
false, lazySub->isLazy());
351 lazySub->setLazy(
true);
354 EXPECT_EQ(1, numOutSubscribes);
355 EXPECT_EQ(2, numInReceived);
356 EXPECT_EQ(1, numOutReceived);
357 EXPECT_EQ(0, outPub.getNumSubscribers());
358 EXPECT_EQ(0, inPub.getNumSubscribers());
361 EXPECT_EQ(
false, lazySub->isSubscribed());
362 EXPECT_EQ(
true, lazySub->isLazy());
365 auto outSub = pnh.subscribe<std_msgs::Header>(
"out", 10, outCb);
368 EXPECT_EQ(2, numOutSubscribes);
369 EXPECT_EQ(2, numInReceived);
370 EXPECT_EQ(1, numOutReceived);
371 EXPECT_EQ(1, outPub.getNumSubscribers());
372 EXPECT_EQ(1, inPub.getNumSubscribers());
375 EXPECT_EQ(
true, lazySub->isSubscribed());
376 EXPECT_EQ(
true, lazySub->isLazy());
378 inPub.publish(std_msgs::Header());
381 EXPECT_EQ(2, numOutSubscribes);
382 EXPECT_EQ(3, numInReceived);
383 EXPECT_EQ(2, numOutReceived);
384 EXPECT_EQ(1, outPub.getNumSubscribers());
385 EXPECT_EQ(1, inPub.getNumSubscribers());
388 EXPECT_EQ(
true, lazySub->isSubscribed());
389 EXPECT_EQ(
true, lazySub->isLazy());
393 EXPECT_EQ(2, numOutSubscribes);
394 EXPECT_EQ(3, numInReceived);
395 EXPECT_EQ(2, numOutReceived);
396 EXPECT_EQ(0, outPub.getNumSubscribers());
397 EXPECT_EQ(0, inPub.getNumSubscribers());
400 EXPECT_EQ(
false, lazySub->isSubscribed());
401 EXPECT_EQ(
true, lazySub->isLazy());
404 TEST(LazySubscriberBase, Chain)
408 auto outPub = pnh.advertise<std_msgs::Header>(
"out", 10);
409 auto inPub = nh.
advertise<std_msgs::Header>(
"in", 10);
410 auto inter1Pub = nh.
advertise<std_msgs::Header>(
"inter1", 10);
411 auto inter2Pub = nh.
advertise<std_msgs::Header>(
"inter2", 10);
413 size_t numInter1Subscribes = 0;
414 size_t numInter2Subscribes = 0;
415 size_t numOutSubscribes = 0;
416 size_t numInReceived = 0;
417 size_t numInter1Received = 0;
418 size_t numInter2Received = 0;
419 size_t numOutReceived = 0;
420 boost::function<void(
const std_msgs::Header&)> relayToInter1Cb = [&](
const std_msgs::Header& h)
425 boost::function<void(
const std_msgs::Header&)> relayToInter2Cb = [&](
const std_msgs::Header& h)
428 inter2Pub.publish(h);
430 boost::function<void(
const std_msgs::Header&)> relayToOutCb = [&](
const std_msgs::Header& h)
435 boost::function<void(
const std_msgs::Header&)> outCb = [&](
const std_msgs::Header& h)
441 ++numInter1Subscribes;
442 sub = nh.subscribe<std_msgs::Header>(
"in", 10, relayToInter1Cb);
446 ++numInter2Subscribes;
447 sub = nh.subscribe<std_msgs::Header>(
"inter1", 10, relayToInter2Cb);
452 sub = nh.subscribe<std_msgs::Header>(
"inter2", 10, relayToOutCb);
456 EXPECT_EQ(0, numInter1Subscribes);
457 EXPECT_EQ(0, numInter2Subscribes);
458 EXPECT_EQ(0, numOutSubscribes);
459 EXPECT_EQ(0, numInReceived);
460 EXPECT_EQ(0, numInter1Received);
461 EXPECT_EQ(0, numInter2Received);
462 EXPECT_EQ(0, numOutReceived);
463 EXPECT_EQ(0, inPub.getNumSubscribers());
464 EXPECT_EQ(0, inter1Pub.getNumSubscribers());
465 EXPECT_EQ(0, inter2Pub.getNumSubscribers());
466 EXPECT_EQ(0, outPub.getNumSubscribers());
472 auto outSub = pnh.subscribe<std_msgs::Header>(
"out", 10, outCb);
475 EXPECT_EQ(1, numInter1Subscribes);
476 EXPECT_EQ(1, numInter2Subscribes);
477 EXPECT_EQ(1, numOutSubscribes);
478 EXPECT_EQ(0, numInReceived);
479 EXPECT_EQ(0, numInter1Received);
480 EXPECT_EQ(0, numInter2Received);
481 EXPECT_EQ(0, numOutReceived);
482 EXPECT_EQ(1, inPub.getNumSubscribers());
483 EXPECT_EQ(1, inter1Pub.getNumSubscribers());
484 EXPECT_EQ(1, inter2Pub.getNumSubscribers());
485 EXPECT_EQ(1, outPub.getNumSubscribers());
490 inPub.publish(std_msgs::Header());
493 EXPECT_EQ(1, numInter1Subscribes);
494 EXPECT_EQ(1, numInter2Subscribes);
495 EXPECT_EQ(1, numOutSubscribes);
496 EXPECT_EQ(1, numInReceived);
497 EXPECT_EQ(1, numInter1Received);
498 EXPECT_EQ(1, numInter2Received);
499 EXPECT_EQ(1, numOutReceived);
500 EXPECT_EQ(1, inPub.getNumSubscribers());
501 EXPECT_EQ(1, inter1Pub.getNumSubscribers());
502 EXPECT_EQ(1, inter2Pub.getNumSubscribers());
503 EXPECT_EQ(1, outPub.getNumSubscribers());
508 inPub.publish(std_msgs::Header());
511 EXPECT_EQ(1, numInter1Subscribes);
512 EXPECT_EQ(1, numInter2Subscribes);
513 EXPECT_EQ(1, numOutSubscribes);
514 EXPECT_EQ(2, numInReceived);
515 EXPECT_EQ(2, numInter1Received);
516 EXPECT_EQ(2, numInter2Received);
517 EXPECT_EQ(2, numOutReceived);
518 EXPECT_EQ(1, inPub.getNumSubscribers());
519 EXPECT_EQ(1, inter1Pub.getNumSubscribers());
520 EXPECT_EQ(1, inter2Pub.getNumSubscribers());
521 EXPECT_EQ(1, outPub.getNumSubscribers());
528 EXPECT_EQ(1, numInter1Subscribes);
529 EXPECT_EQ(1, numInter2Subscribes);
530 EXPECT_EQ(1, numOutSubscribes);
531 EXPECT_EQ(2, numInReceived);
532 EXPECT_EQ(2, numInter1Received);
533 EXPECT_EQ(2, numInter2Received);
534 EXPECT_EQ(2, numOutReceived);
535 EXPECT_EQ(0, inPub.getNumSubscribers());
536 EXPECT_EQ(0, inter1Pub.getNumSubscribers());
537 EXPECT_EQ(0, inter2Pub.getNumSubscribers());
538 EXPECT_EQ(0, outPub.getNumSubscribers());
543 inPub.publish(std_msgs::Header());
546 EXPECT_EQ(1, numInter1Subscribes);
547 EXPECT_EQ(1, numInter2Subscribes);
548 EXPECT_EQ(1, numOutSubscribes);
549 EXPECT_EQ(2, numInReceived);
550 EXPECT_EQ(2, numInter1Received);
551 EXPECT_EQ(2, numInter2Received);
552 EXPECT_EQ(2, numOutReceived);
553 EXPECT_EQ(0, inPub.getNumSubscribers());
554 EXPECT_EQ(0, inter1Pub.getNumSubscribers());
555 EXPECT_EQ(0, inter2Pub.getNumSubscribers());
556 EXPECT_EQ(0, outPub.getNumSubscribers());
566 auto outPub = pnh.advertise<std_msgs::Header>(
"out", 10);
567 auto inPub = nh.
advertise<std_msgs::Header>(
"in", 10);
569 size_t numInReceived = 0;
570 size_t numOutReceived = 0;
571 boost::function<void(
const std_msgs::Header&)> relayCb = [&](
const std_msgs::Header& h)
576 boost::function<void(
const std_msgs::Header&)> outCb = [&](
const std_msgs::Header& h)
581 auto lazySub = std::make_unique<cras::LazySubscriber<std_msgs::Header>>(
582 pnh, outPub.getTopic(), nh,
"in", 10, relayCb);
585 EXPECT_EQ(0, numInReceived);
586 EXPECT_EQ(0, outPub.getNumSubscribers());
587 EXPECT_EQ(0, inPub.getNumSubscribers());
589 EXPECT_EQ(
false, lazySub->isSubscribed());
592 auto outSub = pnh.subscribe<std_msgs::Header>(
"out", 10, outCb);
595 EXPECT_EQ(0, numInReceived);
596 EXPECT_EQ(0, numOutReceived);
597 EXPECT_EQ(1, outPub.getNumSubscribers());
598 EXPECT_EQ(1, inPub.getNumSubscribers());
601 EXPECT_EQ(
true, lazySub->isSubscribed());
603 inPub.publish(std_msgs::Header());
606 EXPECT_EQ(1, numInReceived);
607 EXPECT_EQ(1, numOutReceived);
608 EXPECT_EQ(1, outPub.getNumSubscribers());
609 EXPECT_EQ(1, inPub.getNumSubscribers());
612 EXPECT_EQ(
true, lazySub->isSubscribed());
616 EXPECT_EQ(1, numInReceived);
617 EXPECT_EQ(1, numOutReceived);
618 EXPECT_EQ(0, outPub.getNumSubscribers());
619 EXPECT_EQ(0, inPub.getNumSubscribers());
622 EXPECT_EQ(
false, lazySub->isSubscribed());
624 inPub.publish(std_msgs::Header());
627 EXPECT_EQ(1, numInReceived);
628 EXPECT_EQ(1, numOutReceived);
629 EXPECT_EQ(0, outPub.getNumSubscribers());
630 EXPECT_EQ(0, inPub.getNumSubscribers());
633 EXPECT_EQ(
false, lazySub->isSubscribed());
636 auto outSub = pnh.subscribe<std_msgs::Header>(
"out", 10, outCb);
637 auto outSub2 = pnh.subscribe<std_msgs::Header>(
"out", 10, outCb);
640 EXPECT_EQ(1, numInReceived);
641 EXPECT_EQ(1, numOutReceived);
642 EXPECT_EQ(1, outPub.getNumSubscribers());
643 EXPECT_EQ(1, inPub.getNumSubscribers());
646 EXPECT_EQ(
true, lazySub->isSubscribed());
648 inPub.publish(std_msgs::Header());
649 inPub.publish(std_msgs::Header());
652 EXPECT_EQ(3, numInReceived);
653 EXPECT_EQ(5, numOutReceived);
654 EXPECT_EQ(1, outPub.getNumSubscribers());
655 EXPECT_EQ(1, inPub.getNumSubscribers());
658 EXPECT_EQ(
true, lazySub->isSubscribed());
662 EXPECT_EQ(3, numInReceived);
663 EXPECT_EQ(5, numOutReceived);
664 EXPECT_EQ(0, outPub.getNumSubscribers());
665 EXPECT_EQ(0, inPub.getNumSubscribers());
668 EXPECT_EQ(
false, lazySub->isSubscribed());
670 inPub.publish(std_msgs::Header());
671 inPub.publish(std_msgs::Header());
672 inPub.publish(std_msgs::Header());
675 EXPECT_EQ(3, numInReceived);
676 EXPECT_EQ(5, numOutReceived);
677 EXPECT_EQ(0, outPub.getNumSubscribers());
678 EXPECT_EQ(0, inPub.getNumSubscribers());
681 EXPECT_EQ(
false, lazySub->isSubscribed());
684 auto outSub = pnh.subscribe<std_msgs::Header>(
"out", 10, outCb);
687 EXPECT_EQ(3, numInReceived);
688 EXPECT_EQ(5, numOutReceived);
689 EXPECT_EQ(1, outPub.getNumSubscribers());
690 EXPECT_EQ(1, inPub.getNumSubscribers());
693 EXPECT_EQ(
true, lazySub->isSubscribed());
699 EXPECT_EQ(3, numInReceived);
700 EXPECT_EQ(5, numOutReceived);
701 EXPECT_EQ(1, outPub.getNumSubscribers());
702 EXPECT_EQ(0, inPub.getNumSubscribers());
708 TEST(LazySubscriber, MessageEvent)
712 auto outPub = pnh.advertise<std_msgs::Header>(
"out", 10);
713 auto inPub = nh.
advertise<std_msgs::Header>(
"in", 10);
715 size_t numInReceived = 0;
716 size_t numOutReceived = 0;
721 outPub.
publish(*h.getConstMessage());
723 boost::function<void(
const std_msgs::Header&)> outCb = [&](
const std_msgs::Header& h)
728 auto lazySub = std::make_unique<
730 pnh, outPub.getTopic(), nh,
"in", 10, relayCb);
733 EXPECT_EQ(0, numInReceived);
734 EXPECT_EQ(0, outPub.getNumSubscribers());
735 EXPECT_EQ(0, inPub.getNumSubscribers());
737 EXPECT_EQ(
false, lazySub->isSubscribed());
740 auto outSub = pnh.subscribe<std_msgs::Header>(
"out", 10, outCb);
743 EXPECT_EQ(0, numInReceived);
744 EXPECT_EQ(0, numOutReceived);
745 EXPECT_EQ(1, outPub.getNumSubscribers());
746 EXPECT_EQ(1, inPub.getNumSubscribers());
749 EXPECT_EQ(
true, lazySub->isSubscribed());
751 inPub.publish(std_msgs::Header());
754 EXPECT_EQ(1, numInReceived);
755 EXPECT_EQ(1, numOutReceived);
756 EXPECT_EQ(1, outPub.getNumSubscribers());
757 EXPECT_EQ(1, inPub.getNumSubscribers());
760 EXPECT_EQ(
true, lazySub->isSubscribed());
764 EXPECT_EQ(1, numInReceived);
765 EXPECT_EQ(1, numOutReceived);
766 EXPECT_EQ(0, outPub.getNumSubscribers());
767 EXPECT_EQ(0, inPub.getNumSubscribers());
770 EXPECT_EQ(
false, lazySub->isSubscribed());
772 inPub.publish(std_msgs::Header());
775 EXPECT_EQ(1, numInReceived);
776 EXPECT_EQ(1, numOutReceived);
777 EXPECT_EQ(0, outPub.getNumSubscribers());
778 EXPECT_EQ(0, inPub.getNumSubscribers());
781 EXPECT_EQ(
false, lazySub->isSubscribed());
784 auto outSub = pnh.subscribe<std_msgs::Header>(
"out", 10, outCb);
785 auto outSub2 = pnh.subscribe<std_msgs::Header>(
"out", 10, outCb);
788 EXPECT_EQ(1, numInReceived);
789 EXPECT_EQ(1, numOutReceived);
790 EXPECT_EQ(1, outPub.getNumSubscribers());
791 EXPECT_EQ(1, inPub.getNumSubscribers());
794 EXPECT_EQ(
true, lazySub->isSubscribed());
796 inPub.publish(std_msgs::Header());
797 inPub.publish(std_msgs::Header());
800 EXPECT_EQ(3, numInReceived);
801 EXPECT_EQ(5, numOutReceived);
802 EXPECT_EQ(1, outPub.getNumSubscribers());
803 EXPECT_EQ(1, inPub.getNumSubscribers());
806 EXPECT_EQ(
true, lazySub->isSubscribed());
810 EXPECT_EQ(3, numInReceived);
811 EXPECT_EQ(5, numOutReceived);
812 EXPECT_EQ(0, outPub.getNumSubscribers());
813 EXPECT_EQ(0, inPub.getNumSubscribers());
816 EXPECT_EQ(
false, lazySub->isSubscribed());
818 inPub.publish(std_msgs::Header());
819 inPub.publish(std_msgs::Header());
820 inPub.publish(std_msgs::Header());
823 EXPECT_EQ(3, numInReceived);
824 EXPECT_EQ(5, numOutReceived);
825 EXPECT_EQ(0, outPub.getNumSubscribers());
826 EXPECT_EQ(0, inPub.getNumSubscribers());
829 EXPECT_EQ(
false, lazySub->isSubscribed());
832 auto outSub = pnh.subscribe<std_msgs::Header>(
"out", 10, outCb);
835 EXPECT_EQ(3, numInReceived);
836 EXPECT_EQ(5, numOutReceived);
837 EXPECT_EQ(1, outPub.getNumSubscribers());
838 EXPECT_EQ(1, inPub.getNumSubscribers());
841 EXPECT_EQ(
true, lazySub->isSubscribed());
847 EXPECT_EQ(3, numInReceived);
848 EXPECT_EQ(5, numOutReceived);
849 EXPECT_EQ(1, outPub.getNumSubscribers());
850 EXPECT_EQ(0, inPub.getNumSubscribers());
856 int main(
int argc,
char **argv)
858 testing::InitGoogleTest(&argc, argv);
859 ros::init(argc, argv,
"test_lazy_subscriber");
864 return RUN_ALL_TESTS();