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 #include <gtest/gtest.h>
00036
00037 #include "ros/time.h"
00038 #include "roscpp/Logger.h"
00039 #include "message_filters/subscriber.h"
00040 #include "message_filters/chain.h"
00041
00042 using namespace message_filters;
00043 typedef roscpp::Logger Msg;
00044 typedef roscpp::LoggerPtr MsgPtr;
00045 typedef roscpp::LoggerConstPtr MsgConstPtr;
00046
00047 class Helper
00048 {
00049 public:
00050 Helper()
00051 : count_(0)
00052 {}
00053
00054 void cb(const MsgConstPtr&)
00055 {
00056 ++count_;
00057 }
00058
00059 int32_t count_;
00060 };
00061
00062 TEST(Subscriber, simple)
00063 {
00064 ros::NodeHandle nh;
00065 Helper h;
00066 Subscriber<Msg> sub(nh, "test_topic", 0);
00067 sub.registerCallback(boost::bind(&Helper::cb, &h, _1));
00068 ros::Publisher pub = nh.advertise<Msg>("test_topic", 0);
00069
00070 ros::Time start = ros::Time::now();
00071 while (h.count_ == 0 && (ros::Time::now() - start) < ros::Duration(1.0))
00072 {
00073 pub.publish(Msg());
00074 ros::Duration(0.01).sleep();
00075 ros::spinOnce();
00076 }
00077
00078 ASSERT_GT(h.count_, 0);
00079 }
00080
00081 TEST(Subscriber, subUnsubSub)
00082 {
00083 ros::NodeHandle nh;
00084 Helper h;
00085 Subscriber<Msg> sub(nh, "test_topic", 0);
00086 sub.registerCallback(boost::bind(&Helper::cb, &h, _1));
00087 ros::Publisher pub = nh.advertise<Msg>("test_topic", 0);
00088
00089 sub.unsubscribe();
00090 sub.subscribe();
00091
00092 ros::Time start = ros::Time::now();
00093 while (h.count_ == 0 && (ros::Time::now() - start) < ros::Duration(1.0))
00094 {
00095 pub.publish(Msg());
00096 ros::Duration(0.01).sleep();
00097 ros::spinOnce();
00098 }
00099
00100 ASSERT_GT(h.count_, 0);
00101 }
00102
00103 TEST(Subscriber, subInChain)
00104 {
00105 ros::NodeHandle nh;
00106 Helper h;
00107 Chain<Msg> c;
00108 c.addFilter(boost::make_shared<Subscriber<Msg> >(boost::ref(nh), "test_topic", 0));
00109 c.registerCallback(boost::bind(&Helper::cb, &h, _1));
00110 ros::Publisher pub = nh.advertise<Msg>("test_topic", 0);
00111
00112 ros::Time start = ros::Time::now();
00113 while (h.count_ == 0 && (ros::Time::now() - start) < ros::Duration(1.0))
00114 {
00115 pub.publish(Msg());
00116 ros::Duration(0.01).sleep();
00117 ros::spinOnce();
00118 }
00119
00120 ASSERT_GT(h.count_, 0);
00121 }
00122
00123 struct ConstHelper
00124 {
00125 void cb(const MsgConstPtr& msg)
00126 {
00127 msg_ = msg;
00128 }
00129
00130 MsgConstPtr msg_;
00131 };
00132
00133 struct NonConstHelper
00134 {
00135 void cb(const MsgPtr& msg)
00136 {
00137 msg_ = msg;
00138 }
00139
00140 MsgPtr msg_;
00141 };
00142
00143 TEST(Subscriber, singleNonConstCallback)
00144 {
00145 ros::NodeHandle nh;
00146 NonConstHelper h;
00147 Subscriber<Msg> sub(nh, "test_topic", 0);
00148 sub.registerCallback(&NonConstHelper::cb, &h);
00149 ros::Publisher pub = nh.advertise<Msg>("test_topic", 0);
00150 MsgPtr msg(boost::make_shared<Msg>());
00151 pub.publish(msg);
00152
00153 ros::spinOnce();
00154
00155 ASSERT_TRUE(h.msg_);
00156 ASSERT_EQ(msg.get(), h.msg_.get());
00157 }
00158
00159 TEST(Subscriber, multipleNonConstCallbacksFilterSubscriber)
00160 {
00161 ros::NodeHandle nh;
00162 NonConstHelper h, h2;
00163 Subscriber<Msg> sub(nh, "test_topic", 0);
00164 sub.registerCallback(&NonConstHelper::cb, &h);
00165 sub.registerCallback(&NonConstHelper::cb, &h2);
00166 ros::Publisher pub = nh.advertise<Msg>("test_topic", 0);
00167 MsgPtr msg(boost::make_shared<Msg>());
00168 pub.publish(msg);
00169
00170 ros::spinOnce();
00171
00172 ASSERT_TRUE(h.msg_);
00173 ASSERT_TRUE(h2.msg_);
00174 EXPECT_NE(msg.get(), h.msg_.get());
00175 EXPECT_NE(msg.get(), h2.msg_.get());
00176 EXPECT_NE(h.msg_.get(), h2.msg_.get());
00177 }
00178
00179 TEST(Subscriber, multipleCallbacksSomeFilterSomeDirect)
00180 {
00181 ros::NodeHandle nh;
00182 NonConstHelper h, h2;
00183 Subscriber<Msg> sub(nh, "test_topic", 0);
00184 sub.registerCallback(&NonConstHelper::cb, &h);
00185 ros::Subscriber sub2 = nh.subscribe("test_topic", 0, &NonConstHelper::cb, &h2);
00186 ros::Publisher pub = nh.advertise<Msg>("test_topic", 0);
00187 MsgPtr msg(boost::make_shared<Msg>());
00188 pub.publish(msg);
00189
00190 ros::spinOnce();
00191
00192 ASSERT_TRUE(h.msg_);
00193 ASSERT_TRUE(h2.msg_);
00194 EXPECT_NE(msg.get(), h.msg_.get());
00195 EXPECT_NE(msg.get(), h2.msg_.get());
00196 EXPECT_NE(h.msg_.get(), h2.msg_.get());
00197 }
00198
00199
00200 int main(int argc, char **argv){
00201 testing::InitGoogleTest(&argc, argv);
00202
00203 ros::init(argc, argv, "test_subscriber");
00204 ros::NodeHandle nh;
00205
00206 return RUN_ALL_TESTS();
00207 }
00208
00209