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/thread.hpp>
00051
00052 using namespace ros;
00053 using namespace test_roscpp;
00054
00055 TEST(RoscppHandles, nodeHandleConstructionDestruction)
00056 {
00057 {
00058 ASSERT_FALSE(ros::isStarted());
00059
00060 ros::NodeHandle n1;
00061 ASSERT_TRUE(ros::isStarted());
00062
00063 {
00064 ros::NodeHandle n2;
00065 ASSERT_TRUE(ros::isStarted());
00066
00067 {
00068 ros::NodeHandle n3(n2);
00069 ASSERT_TRUE(ros::isStarted());
00070
00071 {
00072 ros::NodeHandle n4 = n3;
00073 ASSERT_TRUE(ros::isStarted());
00074 }
00075 }
00076 }
00077
00078 ASSERT_TRUE(ros::isStarted());
00079 }
00080
00081 ASSERT_FALSE(ros::isStarted());
00082
00083 {
00084 ros::NodeHandle n;
00085 ASSERT_TRUE(ros::isStarted());
00086 }
00087
00088 ASSERT_FALSE(ros::isStarted());
00089 }
00090
00091 TEST(RoscppHandles, nodeHandleParentWithRemappings)
00092 {
00093 ros::M_string remappings;
00094 remappings["a"] = "b";
00095 remappings["c"] = "d";
00096 ros::NodeHandle n1("", remappings);
00097
00098
00099 EXPECT_STREQ(n1.resolveName("a").c_str(), "/b");
00100 EXPECT_STREQ(n1.resolveName("/a").c_str(), "/b");
00101 EXPECT_STREQ(n1.resolveName("c").c_str(), "/d");
00102 EXPECT_STREQ(n1.resolveName("/c").c_str(), "/d");
00103
00104 ros::NodeHandle n2(n1, "my_ns");
00105 EXPECT_STREQ(n2.resolveName("a").c_str(), "/my_ns/a");
00106 EXPECT_STREQ(n2.resolveName("/a").c_str(), "/b");
00107 EXPECT_STREQ(n2.resolveName("c").c_str(), "/my_ns/c");
00108 EXPECT_STREQ(n2.resolveName("/c").c_str(), "/d");
00109
00110 ros::NodeHandle n3(n2);
00111 EXPECT_STREQ(n3.resolveName("a").c_str(), "/my_ns/a");
00112 EXPECT_STREQ(n3.resolveName("/a").c_str(), "/b");
00113 EXPECT_STREQ(n3.resolveName("c").c_str(), "/my_ns/c");
00114 EXPECT_STREQ(n3.resolveName("/c").c_str(), "/d");
00115
00116 ros::NodeHandle n4;
00117 n4 = n3;
00118 EXPECT_STREQ(n4.resolveName("a").c_str(), "/my_ns/a");
00119 EXPECT_STREQ(n4.resolveName("/a").c_str(), "/b");
00120 EXPECT_STREQ(n4.resolveName("c").c_str(), "/my_ns/c");
00121 EXPECT_STREQ(n4.resolveName("/c").c_str(), "/d");
00122 }
00123
00124 int32_t g_recv_count = 0;
00125 void subscriberCallback(const test_roscpp::TestArray::ConstPtr& msg)
00126 {
00127 ++g_recv_count;
00128 }
00129
00130 class SubscribeHelper
00131 {
00132 public:
00133 SubscribeHelper()
00134 : recv_count_(0)
00135 {}
00136
00137 void callback(const test_roscpp::TestArray::ConstPtr& msg)
00138 {
00139 ++recv_count_;
00140 }
00141
00142 int32_t recv_count_;
00143 };
00144
00145 TEST(RoscppHandles, subscriberValidity)
00146 {
00147 ros::NodeHandle n;
00148
00149 ros::Subscriber sub;
00150 ASSERT_FALSE(sub);
00151
00152 sub = n.subscribe("test", 0, subscriberCallback);
00153 ASSERT_TRUE(sub);
00154 }
00155
00156 TEST(RoscppHandles, subscriberDestructionMultipleCallbacks)
00157 {
00158 ros::NodeHandle n;
00159 ros::Publisher pub = n.advertise<test_roscpp::TestArray>("test", 0);
00160 test_roscpp::TestArray msg;
00161
00162 {
00163 SubscribeHelper helper;
00164 ros::Subscriber sub_class = n.subscribe("test", 0, &SubscribeHelper::callback, &helper);
00165
00166 ros::Duration d(0.05);
00167 int32_t last_class_count = helper.recv_count_;
00168 while (last_class_count == helper.recv_count_)
00169 {
00170 pub.publish(msg);
00171 ros::spinOnce();
00172 d.sleep();
00173 }
00174
00175 int32_t last_fn_count = g_recv_count;
00176 {
00177 ros::Subscriber sub_fn = n.subscribe("test", 0, subscriberCallback);
00178
00179 ASSERT_TRUE(sub_fn != sub_class);
00180
00181 last_fn_count = g_recv_count;
00182 while (last_fn_count == g_recv_count)
00183 {
00184 pub.publish(msg);
00185 ros::spinOnce();
00186 d.sleep();
00187 }
00188 }
00189
00190 last_fn_count = g_recv_count;
00191 last_class_count = helper.recv_count_;
00192 while (last_class_count == helper.recv_count_)
00193 {
00194 pub.publish(msg);
00195 ros::spinOnce();
00196 d.sleep();
00197 }
00198 d.sleep();
00199
00200 ASSERT_EQ(last_fn_count, g_recv_count);
00201 }
00202 }
00203
00204 TEST(RoscppHandles, subscriberSpinAfterSubscriberShutdown)
00205 {
00206 ros::NodeHandle n;
00207 ros::Publisher pub = n.advertise<test_roscpp::TestArray>("test", 0);
00208 test_roscpp::TestArray msg;
00209
00210 int32_t last_fn_count = g_recv_count;
00211
00212 {
00213 ros::Subscriber sub_fn = n.subscribe("test", 0, subscriberCallback);
00214
00215 last_fn_count = g_recv_count;
00216 for (int i = 0; i < 10; ++i)
00217 {
00218 pub.publish(msg);
00219 }
00220
00221 ros::WallDuration(0.1).sleep();
00222 }
00223
00224 ros::spinOnce();
00225
00226 ASSERT_EQ(last_fn_count, g_recv_count);
00227 }
00228
00229 TEST(RoscppHandles, subscriberGetNumPublishers)
00230 {
00231 ros::NodeHandle n;
00232 ros::Publisher pub = n.advertise<test_roscpp::TestArray>("test", 0);
00233
00234 ros::Subscriber sub = n.subscribe("test", 0, subscriberCallback);
00235
00236 ros::WallTime begin = ros::WallTime::now();
00237 while (sub.getNumPublishers() < 1 && (ros::WallTime::now() - begin < ros::WallDuration(1)))
00238 {
00239 ros::spinOnce();
00240 ros::WallDuration(0.1).sleep();
00241 }
00242
00243 ASSERT_EQ(sub.getNumPublishers(), 1ULL);
00244 }
00245
00246 TEST(RoscppHandles, subscriberCopy)
00247 {
00248 ros::NodeHandle n;
00249
00250 g_recv_count = 0;
00251
00252 {
00253 ros::Subscriber sub1 = n.subscribe("/test", 0, subscriberCallback);
00254
00255 {
00256 ros::Subscriber sub2 = sub1;
00257
00258 {
00259 ros::Subscriber sub3(sub2);
00260
00261 ASSERT_TRUE(sub3 == sub2);
00262
00263 V_string topics;
00264 this_node::getSubscribedTopics(topics);
00265 ASSERT_TRUE(std::find(topics.begin(), topics.end(), "/test") != topics.end());
00266 }
00267
00268 ASSERT_TRUE(sub2 == sub1);
00269
00270 V_string topics;
00271 this_node::getSubscribedTopics(topics);
00272 ASSERT_TRUE(std::find(topics.begin(), topics.end(), "/test") != topics.end());
00273 }
00274
00275 V_string topics;
00276 this_node::getSubscribedTopics(topics);
00277 ASSERT_TRUE(std::find(topics.begin(), topics.end(), "/test") != topics.end());
00278 }
00279
00280 V_string topics;
00281 this_node::getSubscribedTopics(topics);
00282 ASSERT_TRUE(std::find(topics.begin(), topics.end(), "/test") == topics.end());
00283 }
00284
00285 TEST(RoscppHandles, publisherCopy)
00286 {
00287 ros::NodeHandle n;
00288
00289 g_recv_count = 0;
00290
00291 {
00292 ros::Publisher pub1 = n.advertise<test_roscpp::TestArray>("/test", 0);
00293
00294 {
00295 ros::Publisher pub2 = pub1;
00296
00297 {
00298 ros::Publisher pub3(pub2);
00299
00300 ASSERT_TRUE(pub3 == pub2);
00301
00302 V_string topics;
00303 this_node::getAdvertisedTopics(topics);
00304 ASSERT_TRUE(std::find(topics.begin(), topics.end(), "/test") != topics.end());
00305 }
00306
00307 ASSERT_TRUE(pub2 == pub1);
00308
00309 V_string topics;
00310 this_node::getAdvertisedTopics(topics);
00311 ASSERT_TRUE(std::find(topics.begin(), topics.end(), "/test") != topics.end());
00312 }
00313
00314 V_string topics;
00315 this_node::getAdvertisedTopics(topics);
00316 ASSERT_TRUE(std::find(topics.begin(), topics.end(), "/test") != topics.end());
00317 }
00318
00319 V_string topics;
00320 this_node::getAdvertisedTopics(topics);
00321 ASSERT_TRUE(std::find(topics.begin(), topics.end(), "/test") == topics.end());
00322 }
00323
00324 TEST(RoscppHandles, publisherMultiple)
00325 {
00326 ros::NodeHandle n;
00327
00328 g_recv_count = 0;
00329
00330 {
00331 ros::Publisher pub1 = n.advertise<test_roscpp::TestArray>("/test", 0);
00332
00333 {
00334 ros::Publisher pub2 = n.advertise<test_roscpp::TestArray>("/test", 0);
00335
00336 ASSERT_TRUE(pub1 != pub2);
00337
00338 V_string topics;
00339 this_node::getAdvertisedTopics(topics);
00340 ASSERT_TRUE(std::find(topics.begin(), topics.end(), "/test") != topics.end());
00341 }
00342
00343 V_string topics;
00344 this_node::getAdvertisedTopics(topics);
00345 ASSERT_TRUE(std::find(topics.begin(), topics.end(), "/test") != topics.end());
00346 }
00347
00348 V_string topics;
00349 this_node::getAdvertisedTopics(topics);
00350 ASSERT_TRUE(std::find(topics.begin(), topics.end(), "/test") == topics.end());
00351 }
00352
00353 bool serviceCallback(TestStringString::Request& req, TestStringString::Response& res)
00354 {
00355 return true;
00356 }
00357
00358 void pump(ros::CallbackQueue* queue)
00359 {
00360 while (queue->isEnabled())
00361 {
00362 queue->callAvailable(ros::WallDuration(0.1));
00363 }
00364 }
00365
00366 TEST(RoscppHandles, serviceAdv)
00367 {
00368 ros::NodeHandle n;
00369 TestStringString t;
00370
00371 ros::CallbackQueue queue;
00372 n.setCallbackQueue(&queue);
00373 boost::thread th(boost::bind(pump, &queue));
00374 {
00375 ros::ServiceServer srv = n.advertiseService("/test_srv", serviceCallback);
00376
00377 ASSERT_TRUE(ros::service::call("/test_srv", t));
00378 }
00379
00380 queue.disable();
00381 th.join();
00382
00383 ASSERT_FALSE(ros::service::call("/test_srv", t));
00384 }
00385
00386 TEST(RoscppHandles, serviceAdvCopy)
00387 {
00388 ros::NodeHandle n;
00389 TestStringString t;
00390
00391 ros::CallbackQueue queue;
00392 n.setCallbackQueue(&queue);
00393 boost::thread th(boost::bind(pump, &queue));
00394
00395 {
00396 ros::ServiceServer srv1 = n.advertiseService("/test_srv", serviceCallback);
00397
00398 {
00399 ros::ServiceServer srv2 = srv1;
00400
00401 {
00402 ros::ServiceServer srv3(srv2);
00403
00404 ASSERT_TRUE(srv3 == srv2);
00405
00406 ASSERT_TRUE(ros::service::call("/test_srv", t));
00407 }
00408
00409 ASSERT_TRUE(srv2 == srv1);
00410
00411 ASSERT_TRUE(ros::service::call("/test_srv", t));
00412 }
00413
00414 ASSERT_TRUE(ros::service::call("/test_srv", t));
00415 }
00416
00417 ASSERT_FALSE(ros::service::call("/test_srv", t));
00418
00419 queue.disable();
00420 th.join();
00421 }
00422
00423 TEST(RoscppHandles, serviceAdvMultiple)
00424 {
00425 ros::NodeHandle n;
00426
00427 ros::ServiceServer srv = n.advertiseService("/test_srv", serviceCallback);
00428 ros::ServiceServer srv2 = n.advertiseService("/test_srv", serviceCallback);
00429 ASSERT_TRUE(srv);
00430 ASSERT_FALSE(srv2);
00431
00432 ASSERT_TRUE(srv != srv2);
00433 }
00434
00435 int32_t g_sub_count = 0;
00436 void connectedCallback(const ros::SingleSubscriberPublisher& pub)
00437 {
00438 ++g_sub_count;
00439 }
00440
00441 TEST(RoscppHandles, trackedObjectWithAdvertiseSubscriberCallback)
00442 {
00443 ros::NodeHandle n;
00444
00445 boost::shared_ptr<char> tracked(new char);
00446
00447 ros::Publisher pub = n.advertise<test_roscpp::TestArray>("/test", 0, connectedCallback, SubscriberStatusCallback(), tracked);
00448
00449 g_recv_count = 0;
00450 g_sub_count = 0;
00451 ros::Subscriber sub = n.subscribe("/test", 0, subscriberCallback);
00452
00453 Duration d(0.01);
00454 while (g_sub_count == 0)
00455 {
00456 d.sleep();
00457 ros::spinOnce();
00458 }
00459 ASSERT_EQ(g_sub_count, 1);
00460
00461 sub.shutdown();
00462
00463 tracked.reset();
00464 sub = n.subscribe("/test", 0, subscriberCallback);
00465
00466 Duration d2(0.01);
00467 for (int i = 0; i < 10; ++i)
00468 {
00469 d2.sleep();
00470 ros::spinOnce();
00471 }
00472
00473 ASSERT_EQ(g_sub_count, 1);
00474 }
00475
00476 TEST(RoscppHandles, spinAfterHandleShutdownWithAdvertiseSubscriberCallback)
00477 {
00478 ros::NodeHandle n;
00479 ros::Publisher pub = n.advertise<test_roscpp::TestArray>("/test", 0, connectedCallback, SubscriberStatusCallback());
00480
00481 g_sub_count = 0;
00482 ros::Subscriber sub = n.subscribe("/test", 0, subscriberCallback);
00483
00484 while (pub.getNumSubscribers() == 0)
00485 {
00486 ros::WallDuration(0.01).sleep();
00487 }
00488
00489 pub.shutdown();
00490
00491 ros::spinOnce();
00492
00493 ASSERT_EQ(g_sub_count, 0);
00494 }
00495
00496 TEST(RoscppHandles, multiplePublishersWithSubscriberConnectCallback)
00497 {
00498 ros::NodeHandle n;
00499 ros::Publisher pub = n.advertise<test_roscpp::TestArray>("/test", 0, connectedCallback, SubscriberStatusCallback());
00500
00501 g_sub_count = 0;
00502 ros::Subscriber sub = n.subscribe("/test", 0, subscriberCallback);
00503
00504 while (g_sub_count == 0)
00505 {
00506 ros::WallDuration(0.01).sleep();
00507 ros::spinOnce();
00508 }
00509
00510 ASSERT_EQ(g_sub_count, 1);
00511 g_sub_count = 0;
00512
00513 ros::Publisher pub2 = n.advertise<test_roscpp::TestArray>("/test", 0, connectedCallback, SubscriberStatusCallback());
00514 ros::spinOnce();
00515
00516 ASSERT_EQ(g_sub_count, 1);
00517 }
00518
00519 class ServiceClass
00520 {
00521 public:
00522 bool serviceCallback(TestStringString::Request& req, TestStringString::Response& res)
00523 {
00524 return true;
00525 }
00526 };
00527
00528 TEST(RoscppHandles, trackedObjectWithServiceCallback)
00529 {
00530 ros::NodeHandle n;
00531
00532 ros::CallbackQueue queue;
00533 n.setCallbackQueue(&queue);
00534 boost::thread th(boost::bind(pump, &queue));
00535
00536 boost::shared_ptr<ServiceClass> tracked(new ServiceClass);
00537 ros::ServiceServer srv = n.advertiseService("/test_srv", &ServiceClass::serviceCallback, tracked);
00538
00539 TestStringString t;
00540 ASSERT_TRUE(ros::service::call("/test_srv", t));
00541
00542 tracked.reset();
00543
00544 ASSERT_FALSE(ros::service::call("/test_srv", t));
00545
00546 queue.disable();
00547 th.join();
00548 }
00549
00550 TEST(RoscppHandles, trackedObjectWithSubscriptionCallback)
00551 {
00552 ros::NodeHandle n;
00553
00554 boost::shared_ptr<SubscribeHelper> tracked(new SubscribeHelper);
00555
00556 g_recv_count = 0;
00557 ros::Subscriber sub = n.subscribe("/test", 0, &SubscribeHelper::callback, tracked);
00558
00559 ros::Publisher pub = n.advertise<test_roscpp::TestArray>("/test", 0);
00560
00561 test_roscpp::TestArray msg;
00562 Duration d(0.01);
00563 while (tracked->recv_count_ == 0)
00564 {
00565 pub.publish(msg);
00566 d.sleep();
00567 ros::spinOnce();
00568 }
00569 ASSERT_GE(tracked->recv_count_, 1);
00570
00571 tracked.reset();
00572
00573 pub.publish(msg);
00574 Duration d2(0.01);
00575 for (int i = 0; i < 10; ++i)
00576 {
00577 d2.sleep();
00578 ros::spinOnce();
00579 }
00580 }
00581
00582 TEST(RoscppHandles, nodeHandleNames)
00583 {
00584 ros::NodeHandle n1;
00585 EXPECT_STREQ(n1.resolveName("blah").c_str(), "/blah");
00586
00587 try
00588 {
00589 n1.resolveName("~blah");
00590 FAIL();
00591 }
00592 catch (ros::InvalidNameException&)
00593 {
00594 }
00595
00596 ros::NodeHandle n2("internal_ns");
00597 EXPECT_STREQ(n2.resolveName("blah").c_str(), "/internal_ns/blah");
00598
00599 ros::NodeHandle n3(n2, "2");
00600 EXPECT_STREQ(n3.resolveName("blah").c_str(), "/internal_ns/2/blah");
00601
00602 ros::NodeHandle n4("~");
00603 EXPECT_STREQ(n4.resolveName("blah").c_str(), (ros::this_node::getName() + "/blah").c_str());
00604
00605 try {
00606 ros::NodeHandle n5(n2, "illegal_name!!!");
00607 FAIL();
00608 } catch (ros::InvalidNameException&) { }
00609
00610 }
00611
00612 TEST(RoscppHandles, nodeHandleShutdown)
00613 {
00614 ros::NodeHandle n;
00615
00616 ros::Subscriber sub = n.subscribe("/test", 0, subscriberCallback);
00617 ros::Publisher pub = n.advertise<test_roscpp::TestArray>("/test", 0);
00618 ros::ServiceServer srv = n.advertiseService("/test_srv", serviceCallback);
00619
00620 n.shutdown();
00621
00622 ASSERT_FALSE(pub);
00623 ASSERT_FALSE(sub);
00624 ASSERT_FALSE(srv);
00625 }
00626
00627 int main(int argc, char** argv)
00628 {
00629 testing::InitGoogleTest(&argc, argv);
00630 ros::init(argc, argv, "test_handles");
00631
00632 return RUN_ALL_TESTS();
00633 }
00634