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 #include <string>
00033 #include <sstream>
00034 #include <fstream>
00035
00036 #include <gtest/gtest.h>
00037
00038 #include <time.h>
00039 #include <stdlib.h>
00040
00041 #include "ros/ros.h"
00042 #include <test_roscpp/TestArray.h>
00043
00044 using namespace ros;
00045 using namespace test_roscpp;
00046
00047 std::string g_node_name = "test_latching_publisher";
00048
00049 class Helper
00050 {
00051 public:
00052 Helper()
00053 : count_(0)
00054 {}
00055
00056 void cb(const TestArray::ConstPtr& msg)
00057 {
00058 ++count_;
00059 last_msg_ = msg;
00060 }
00061
00062 int32_t count_;
00063 TestArray::ConstPtr last_msg_;
00064 };
00065
00066 TEST(RoscppLatchingPublisher, nonLatching)
00067 {
00068 ros::NodeHandle n;
00069 ros::Publisher pub = n.advertise<TestArray>("test", 1, false);
00070 TestArray arr;
00071 pub.publish(arr);
00072
00073 Helper h;
00074 ros::Subscriber sub = n.subscribe("test", 1, &Helper::cb, &h);
00075 ros::Duration(0.1).sleep();
00076 ros::spinOnce();
00077
00078 ASSERT_EQ(h.count_, 0);
00079 }
00080
00081 TEST(RoscppLatchingPublisher, latching)
00082 {
00083 ros::NodeHandle n;
00084 ros::Publisher pub = n.advertise<TestArray>("test", 1, true);
00085 TestArray arr;
00086 pub.publish(arr);
00087
00088 Helper h;
00089 ros::Subscriber sub = n.subscribe("test", 1, &Helper::cb, &h);
00090 ros::Duration(0.1).sleep();
00091 ros::spinOnce();
00092
00093 ASSERT_EQ(h.count_, 1);
00094
00095 ASSERT_STREQ((*h.last_msg_->__connection_header)["latching"].c_str(), "1");
00096 }
00097
00098 TEST(RoscppLatchingPublisher, latchingMultipleSubscriptions)
00099 {
00100 ros::NodeHandle n;
00101 ros::Publisher pub = n.advertise<TestArray>("test", 1, true);
00102 TestArray arr;
00103 pub.publish(arr);
00104
00105 Helper h1, h2;
00106 ros::Subscriber sub1 = n.subscribe("test", 1, &Helper::cb, &h1);
00107 ros::Duration(0.1).sleep();
00108 ros::spinOnce();
00109
00110 ASSERT_EQ(h1.count_, 1);
00111 ASSERT_EQ(h2.count_, 0);
00112
00113 ros::Subscriber sub2 = n.subscribe("test", 1, &Helper::cb, &h2);
00114 ros::spinOnce();
00115
00116 ASSERT_EQ(h1.count_, 1);
00117 ASSERT_EQ(h2.count_, 1);
00118 }
00119
00120 int main(int argc, char** argv)
00121 {
00122 testing::InitGoogleTest(&argc, argv);
00123 ros::init(argc, argv, g_node_name);
00124
00125 ros::NodeHandle nh;
00126
00127 return RUN_ALL_TESTS();
00128 }
00129