Add test for queue size and correct subscriber count for local subscr…

…ibers

include/pubsub_cpp/Node.h

@@ -439,7 +439,7 @@ class Publisher: public PublisherBase
 
 	unsigned int getNumSubscribers()
 	{
-		return ps_pub_get_subscriber_count(&publisher_);
+		return ps_pub_get_subscriber_count(&publisher_) + subs_.size();
 	}
 
 	void addCustomEndpoint(const int ip_addr, const short port, const unsigned int stream_id)
@@ -475,7 +475,6 @@ class SubscriberBase
 				// if its latched, get the message from it
 				if (it->second->latched_)
 				{
-					// hmm, this should just queue not call
 					cb(it->second);
 				}
 				// add me to its sub list

msg/Int.msg

@@ -0,0 +1 @@
+int64 value

tests/test_pubsub_cpp.cpp

@@ -2,6 +2,7 @@
 #include <pubsub_cpp/Node.h>
 #include <pubsub_cpp/Spinners.h>
 
+#include <pubsub/Int.msg.h>
 #include <pubsub/String.msg.h>
 
 #include "mini_mock.hpp"
@@ -25,6 +26,9 @@ TEST(test_publish_subscribe_latched_cpp, []() {
 		EXPECT(strcmp(omsg.value, msg->value) == 0);
 		got_message = true;
 		spinner.stop();
+
+		// make sure subscriber count is correct
+		EXPECT(string_pub.getNumSubscribers() == 1);
 	}, 10);
 
 	spinner.run();
@@ -51,12 +55,47 @@ TEST(test_publish_subscribe_zero_copy, []() {
 		got_message = true;
 		EXPECT(msg.get() == omsg.get());
 		spinner.stop();
+
+		// make sure subscriber count is correct
+		EXPECT(string_pub.getNumSubscribers() == 1);
 	}, 10);
 
 	spinner.run();
 	EXPECT(got_message);
 });
 
+TEST(test_publish_subscribe_queue_behavior, []() {
+	// test that the queue drops messages as expected and we only get the newest
+	pubsub::Node node("simple_publisher");
+
+	pubsub::Publisher<pubsub::msg::Int> int_pub(node, "/data");
+
+	pubsub::BlockingSpinnerWithTimers spinner;
+	spinner.setNode(node);
+
+	std::vector<int> received;
+	pubsub::Subscriber<pubsub::msg::Int> subscriber(node, "/data", [&](const pubsub::msg::IntSharedPtr& msg) {
+		printf("Got message %i in sub1\n", msg->value);
+		received.push_back(msg->value);
+		spinner.stop();
+	}, 10);
+
+	for (int i = 0; i < 100; i++)
+	{
+	  pubsub::msg::Int omsg;
+	  omsg.value = i;
+	  int_pub.publish(omsg);
+	}
+
+  // spin after publishing so the queue fills up
+	spinner.run();
+	EXPECT(received.size() == 10);
+	for (int i = 0; i < 10; i++)
+	{
+	  EXPECT(received[i] == 90 + i);
+	}
+});
+
 TEST(test_publish_subscribe_nodelets, []() {
 	// test that data gets passed through without copying between multiple nodes
 	pubsub::Node nodep("simple_publisher");
@@ -104,33 +143,6 @@ TEST(test_publisher_subscriber_close_cpp, []() {
 	sub.close();
 });
 
-/*TEST(test_publish_subscribe_latched_cpp, []() {
-	// test that latched topics make it through the local message passing between nodes
-	pubsub::Node node("simple_publisher");
-
-	pubsub::Publisher<pubsub::msg::String> string_pub(node, "/data", true);
-
-	pubsub::msg::String omsg;
-	omsg.value = "Hello";
-	string_pub.publish(omsg);
-
-	
-	pubsub::BlockingSpinnerWithTimers spinner;
-	spinner.setNode(node);
-
-	bool got_message = false;
-	pubsub::Subscriber<pubsub::msg::String> subscriber(node, "/data", [&](const pubsub::msg::StringSharedPtr& msg) {
-		printf("Got message %s in sub1\n", msg->value);
-		EXPECT(strcmp(omsg.value, msg->value) == 0);
-		got_message = true;
-		spinner.stop();
-	}, 10);
-
-	spinner.wait();
-	EXPECT(got_message);
-});*/
-
-
 TEST(test_publish_subscribe_cpp, []() {
 	// test that normal messages make it through message passing
 	pubsub::Node node("simple_publisher");

tools/CMakeLists.txt

@@ -69,6 +69,7 @@ generate_messages(FILES
   Log.msg
   Path2D.msg
   Odom.msg
+  Int.msg
 )