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 #include <string>
00033
00034 #include <gtest/gtest.h>
00035
00036 #include <time.h>
00037 #include <stdlib.h>
00038
00039 #include "ros/ros.h"
00040 #include <test_roscpp/TestArray.h>
00041
00042 static int g_argc;
00043 static char** g_argv;
00044
00045 bool failure;
00046 bool advertised;
00047
00048 class Publications : public testing::Test
00049 {
00050 public:
00051 ros::NodeHandle nh_;
00052 ros::Publisher pub_;
00053
00054 void subscriberCallback(const ros::SingleSubscriberPublisher&)
00055 {
00056 ROS_INFO("subscriberCallback invoked");
00057 if(!advertised)
00058 {
00059 ROS_INFO("but not advertised");
00060 failure = true;
00061 }
00062 }
00063
00064 bool adv()
00065 {
00066 pub_ = nh_.advertise<test_roscpp::TestArray>("test_roscpp/pubsub_test", 1, boost::bind(&Publications::subscriberCallback, this, _1));
00067 return pub_;
00068 }
00069
00070 void unadv()
00071 {
00072 pub_.shutdown();
00073 }
00074
00075 protected:
00076 Publications() {}
00077 void SetUp()
00078 {
00079 advertised = false;
00080 failure = false;
00081
00082 ASSERT_TRUE(g_argc == 1);
00083 }
00084 void TearDown()
00085 {
00086 }
00087 };
00088
00089 TEST_F(Publications, pubUnadvertise)
00090 {
00091 advertised = true;
00092 ROS_INFO("advertising");
00093 ASSERT_TRUE(adv());
00094 ros::Time t1(ros::Time::now()+ros::Duration(2.0));
00095
00096 while(ros::Time::now() < t1 && !failure)
00097 {
00098 ros::WallDuration(0.01).sleep();
00099 ros::spinOnce();
00100 }
00101
00102 unadv();
00103
00104 ROS_INFO("unadvertised");
00105 advertised = false;
00106
00107 ros::Time t2(ros::Time::now()+ros::Duration(2.0));
00108 while(ros::Time::now() < t2 && !failure)
00109 {
00110 ros::WallDuration(0.01).sleep();
00111 ros::spinOnce();
00112 }
00113
00114 advertised = true;
00115 ASSERT_TRUE(adv());
00116 unadv();
00117 ASSERT_TRUE(adv());
00118
00119 if(failure)
00120 FAIL();
00121 else
00122 SUCCEED();
00123 }
00124
00125 int
00126 main(int argc, char** argv)
00127 {
00128 ros::init(argc, argv, "publish_unadvertise");
00129 testing::InitGoogleTest(&argc, argv);
00130 g_argc = argc;
00131 g_argv = argv;
00132 return RUN_ALL_TESTS();
00133 }