handles.cpp
Go to the documentation of this file.
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     EXPECT_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         EXPECT_TRUE(ros::service::call("/test_srv", t));
00407       }
00408 
00409       ASSERT_TRUE(srv2 == srv1);
00410 
00411       EXPECT_TRUE(ros::service::call("/test_srv", t));
00412     }
00413 
00414     EXPECT_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   EXPECT_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 


test_roscpp
Author(s): Morgan Quigley mquigley@cs.stanford.edu, Ken Conley kwc@willowgarage.com
autogenerated on Sat Dec 28 2013 17:36:21