Go to the documentation of this file.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
00038 #include <gtest/gtest.h>
00039
00040 #include <time.h>
00041 #include <stdlib.h>
00042
00043 #include "ros/ros.h"
00044 #include <test_roscpp/TestArray.h>
00045
00046 #include <boost/thread.hpp>
00047
00048 int g_argc;
00049 char** g_argv;
00050
00051 class Subscriptions : public testing::Test
00052 {
00053 public:
00054 ros::NodeHandle nh_;
00055 ros::Subscriber sub_;
00056
00057 void messageCallback(const test_roscpp::TestArrayConstPtr& msg)
00058 {
00059 ROS_INFO("in callback");
00060
00061 if(!sub_)
00062 {
00063 ROS_INFO("but not subscribed!");
00064 FAIL();
00065 }
00066 }
00067
00068 void autoUnsubscribeCallback(const test_roscpp::TestArrayConstPtr& msg)
00069 {
00070 ROS_INFO("in autounsub callback");
00071 sub_.shutdown();
00072 }
00073
00074 protected:
00075 Subscriptions() {}
00076 };
00077
00078 TEST_F(Subscriptions, subUnsub)
00079 {
00080 sub_.shutdown();
00081
00082 for(int i=0;i<100;i++)
00083 {
00084 if(!sub_)
00085 {
00086 sub_ = nh_.subscribe("roscpp/pubsub_test", 0, &Subscriptions::messageCallback, (Subscriptions*)this);
00087 ASSERT_TRUE(sub_);
00088 }
00089 else
00090 {
00091 sub_.shutdown();
00092 ASSERT_FALSE(sub_);
00093 }
00094
00095 ros::WallDuration(0.01).sleep();
00096 ros::spinOnce();
00097 }
00098 }
00099
00100 TEST_F(Subscriptions, unsubInCallback)
00101 {
00102 sub_ = nh_.subscribe("roscpp/pubsub_test", 0, &Subscriptions::autoUnsubscribeCallback, (Subscriptions*)this);
00103
00104 while (sub_ && ros::ok())
00105 {
00106 ros::WallDuration(0.01).sleep();
00107 ros::spinOnce();
00108 }
00109 }
00110
00111 void spinThread(bool volatile* cont)
00112 {
00113 while (*cont)
00114 {
00115 ros::spinOnce();
00116 ros::Duration(0.001).sleep();
00117 }
00118 }
00119
00120 void unsubscribeAfterBarrierWait(boost::barrier* barrier, ros::Subscriber& sub)
00121 {
00122 barrier->wait();
00123
00124 sub.shutdown();
00125 }
00126
00127 TEST_F(Subscriptions, unsubInCallbackAndOtherThread)
00128 {
00129 boost::barrier barrier(2);
00130 for (int i = 0; i < 100; ++i)
00131 {
00132 ros::Subscriber sub;
00133 sub_ = nh_.subscribe<test_roscpp::TestArray>("roscpp/pubsub_test", 1, boost::bind(unsubscribeAfterBarrierWait, &barrier, boost::ref(sub)));
00134 sub = sub_;
00135
00136 bool cont = true;
00137 boost::thread t(spinThread, &cont);
00138
00139 barrier.wait();
00140
00141 sub_.shutdown();
00142 cont = false;
00143 t.join();
00144 }
00145 }
00146
00147 int
00148 main(int argc, char** argv)
00149 {
00150 testing::InitGoogleTest(&argc, argv);
00151
00152 g_argc = argc;
00153 g_argv = argv;
00154
00155 ros::init(g_argc, g_argv, "subscribe_unsubscribe");
00156
00157 if (g_argc != 1)
00158 {
00159 puts("Too many arguments\n");
00160 return -1;
00161 }
00162
00163 return RUN_ALL_TESTS();
00164 }