$search
00001 /* 00002 * Copyright (c) 2008, 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 handles 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 // sanity checks 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