listener print all instances by default

PX4 · Aug 26, 2019 · 2f10c31 · 2f10c31
1 parent 693d895
commit 2f10c31
Show file tree

Hide file tree

Showing 3 changed files with 84 additions and 50 deletions.
diff --git a/src/systemcmds/topic_listener/generate_listener.py b/src/systemcmds/topic_listener/generate_listener.py
@@ -122,7 +122,7 @@
 	if index == 0:
 		print("\tif (strncmp(topic_name,\"%s\", %d) == 0) {" % (t, len(t)))
 	else:
-		print("\t} else if (strcmp(topic_name,\"%s\") == 0) {" % (t))
+		print("\t} else if (strncmp(topic_name,\"%s\", %d) == 0) {" % (t, len(t)))
 	print("\t\tlistener(listener_print_topic<%s_s>, ORB_ID(%s), num_msgs, topic_instance, topic_interval);" % (m, t))
 
 print("\t} else {")

diff --git a/src/systemcmds/topic_listener/listener_main.cpp b/src/systemcmds/topic_listener/listener_main.cpp
@@ -49,77 +49,111 @@ extern "C" __EXPORT int listener_main(int argc, char *argv[]);
 
 static void usage();
 
-void listener(listener_print_topic_cb cb, const orb_id_t &id, unsigned num_msgs, unsigned topic_instance,
+void listener(listener_print_topic_cb cb, const orb_id_t &id, unsigned num_msgs, int topic_instance,
 	      unsigned topic_interval)
 {
-	if (orb_exists(id, topic_instance) != 0) {
-		PX4_INFO_RAW("never published\n");
-		return;
-	}
 
-	int sub = orb_subscribe_multi(id, topic_instance);
-	orb_set_interval(sub, topic_interval);
+	if (topic_instance == -1 && num_msgs == 1) {
+		// first count the number of instances
+		int instances = 0;
 
-	bool updated = false;
-	unsigned i = 0;
-	hrt_abstime start_time = hrt_absolute_time();
+		for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+			if (orb_exists(id, i) == PX4_OK) {
+				instances++;
+			}
+		}
 
-	while (i < num_msgs) {
+		if (instances == 1) {
+			PX4_INFO_RAW("\nTOPIC: %s\n", id->o_name);
+			int sub = orb_subscribe(id);
+			cb(id, sub);
+			orb_unsubscribe(sub);
+
+		} else if (instances > 1) {
+			PX4_INFO_RAW("\nTOPIC: %s %d instances\n", id->o_name, instances);
+
+			for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+				if (orb_exists(id, i) == PX4_OK) {
+					PX4_INFO_RAW("\nInstance %d:\n", i);
+					int sub = orb_subscribe_multi(id, i);
+					cb(id, sub);
+					orb_unsubscribe(sub);
+				}
+			}
+		}
 
-		// check for user input to quit
-		int user_input_timeout = 1;
+	} else {
+		if (orb_exists(id, topic_instance) != 0) {
+			PX4_INFO_RAW("never published\n");
+			return;
+		}
 
-		orb_check(sub, &updated);
+		int sub = orb_subscribe_multi(id, topic_instance);
+		orb_set_interval(sub, topic_interval);
 
-		if (i == 0) {
-			updated = true;
-			user_input_timeout = 0;	// don't wait
-		}
+		bool updated = false;
+		unsigned i = 0;
+		hrt_abstime start_time = hrt_absolute_time();
 
-		// check for user input
-		struct pollfd fds {};
-		fds.fd = 0; /* stdin */
-		fds.events = POLLIN;
+		while (i < num_msgs) {
 
-		if (poll(&fds, 1, 0) > 0) {
+			// check for user input to quit
+			int user_input_timeout = 1;
 
-			char c = 0;
-			int ret = read(0, &c, user_input_timeout);
+			orb_check(sub, &updated);
 
-			if (ret) {
-				return;
+			if (i == 0) {
+				updated = true;
+				user_input_timeout = 0;	// don't wait
 			}
 
-			switch (c) {
-			case 0x03: // ctrl-c
-			case 0x1b: // esc
-			case 'q':
-				return;
-				/* not reached */
-			}
-		}
+			// check for user input
+			struct pollfd fds {};
+			fds.fd = 0; /* stdin */
+			fds.events = POLLIN;
 
-		if (updated) {
-			start_time = hrt_absolute_time();
-			i++;
+			if (poll(&fds, 1, 0) > 0) {
 
-			PX4_INFO_RAW("\nTOPIC: %s instance %d #%d\n", id->o_name, topic_instance, i);
+				char c = 0;
+				int ret = read(0, &c, user_input_timeout);
 
-			int ret = cb(id, sub);
+				if (ret) {
+					return;
+				}
 
-			if (ret != PX4_OK) {
-				PX4_ERR("listener callback failed (%i)", ret);
+				switch (c) {
+				case 0x03: // ctrl-c
+				case 0x1b: // esc
+				case 'q':
+					return;
+					/* not reached */
+				}
 			}
 
-		} else {
-			if (hrt_elapsed_time(&start_time) > 2 * 1000 * 1000) {
-				PX4_INFO_RAW("Waited for 2 seconds without a message. Giving up.\n");
-				break;
+			if (updated) {
+				start_time = hrt_absolute_time();
+				i++;
+
+				PX4_INFO_RAW("\nTOPIC: %s instance %d #%d\n", id->o_name, topic_instance, i);
+
+				int ret = cb(id, sub);
+
+				if (ret != PX4_OK) {
+					PX4_ERR("listener callback failed (%i)", ret);
+				}
+
+			} else {
+				if (hrt_elapsed_time(&start_time) > 2 * 1000 * 1000) {
+					PX4_INFO_RAW("Waited for 2 seconds without a message. Giving up.\n");
+					break;
+				}
 			}
 		}
+
+		orb_unsubscribe(sub);
 	}
 
-	orb_unsubscribe(sub);
+
 }
 
 int listener_main(int argc, char *argv[])
@@ -131,7 +165,7 @@ int listener_main(int argc, char *argv[])
 
 	char *topic_name = argv[1];
 
-	unsigned topic_instance = 0;
+	int topic_instance = -1;
 	unsigned topic_rate = 0;
 	unsigned num_msgs = 0;
 

diff --git a/src/systemcmds/topic_listener/topic_listener.hpp b/src/systemcmds/topic_listener/topic_listener.hpp
@@ -67,5 +67,5 @@ int listener_print_topic(const orb_id_t &orb_id, int subscription)
 	return ret;
 }
 
-void listener(listener_print_topic_cb cb, const orb_id_t &id, unsigned num_msgs, unsigned topic_instance,
+void listener(listener_print_topic_cb cb, const orb_id_t &id, unsigned num_msgs, int topic_instance,
 	      unsigned topic_interval);