listener: add optional rate and cleanup existing arguments

Tools/uorb_graph/create.py

@@ -222,6 +222,7 @@ class Graph:
         special_cases_sub = [
     ('sensors', r'voted_sensors_update\.cpp$', r'\binit_sensor_class\b\(([^,)]+)', r'^meta$'),
     ('mavlink', r'.*', r'\badd_orb_subscription\b\(([^,)]+)', r'^_topic$'),
+    ('listener', r'.*', None, r'^(id)$'),
     ('logger', r'.*', None, r'^(topic|sub\.metadata|_polling_topic_meta)$'),
 
     ('uavcan', r'uavcan_main\.cpp$', r'\b_control_topics\[[0-9]\]=([^,)]+)', r'^_control_topics\[i\]$'),

src/systemcmds/topic_listener/CMakeLists.txt

@@ -31,14 +31,14 @@
 #
 ############################################################################
 
-add_custom_command(OUTPUT topic_listener.cpp
+add_custom_command(OUTPUT listener_generated.cpp
-                COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_listener.py ${PX4_SOURCE_DIR} ${EXTERNAL_MODULES_LOCATION} > topic_listener.cpp
+                COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_listener.py ${PX4_SOURCE_DIR} ${EXTERNAL_MODULES_LOCATION} > listener_generated.cpp
                 DEPENDS generate_listener.py uorb_msgs
                 )
 
 add_custom_target(generate_topic_listener
 	DEPENDS
-		topic_listener.cpp
+		listener_generated.cpp
 		generate_listener.py
 		uorb_msgs
 	)
@@ -48,8 +48,12 @@ px4_add_module(
 	MAIN listener
 	STACK_MAIN 1800
 	COMPILE_FLAGS
+	INCLUDES
+		${CMAKE_CURRENT_SOURCE_DIR}
+		${CMAKE_CURRENT_BINARY_DIR}
 	SRCS
-		topic_listener.cpp
+		listener_main.cpp
+		listener_generated.cpp
 	DEPENDS
 		generate_topic_listener
 	)

src/systemcmds/topic_listener/generate_listener.py

@@ -99,78 +99,33 @@ print("""
 #include <stdlib.h>
 #include <inttypes.h>
 
-#ifndef PRIu64
+#include <topic_listener.hpp>
-#define PRIu64 "llu"
+#include <topic_listener_generated.hpp>
-#endif
-
-#ifndef PRId64
-#define PRId64 "lld"
-#endif
 
 """)
 for m in set(messages):
 	print("#include <uORB/topics/%s.h>" % m)
 
-print("""
+print ("""
-extern "C" __EXPORT int listener_main(int argc, char *argv[]);
+\nvoid listener_generated(char * topic_name, int topic_instance, int topic_rate, int num_msgs) {
-
-static bool check_timeout(const hrt_abstime& time) {
-    if (hrt_elapsed_time(&time) > 2*1000*1000) {
-		printf("Waited for 2 seconds without a message. Giving up.\\n");
-        return true;
-    }
-    return false;
-}
 """)
 
-for index, (m, t) in enumerate(zip(messages, topics)):
+print("""
-	print("void listen_%s(unsigned num_msgs, unsigned topic_instance);" % t)
+	unsigned topic_interval = 0;
-
+	if (topic_rate != 0) {
-print ("""
+		topic_interval = 1000 / topic_rate;
-\nint listener_main(int argc, char *argv[]) {
-	if(argc < 2) {
-		printf("need at least two arguments: topic name. [optional number of messages to print] [optional instance]\\n");
-		return 1;
 	}
 """)
-print("\tunsigned num_msgs = (argc > 2) ? atoi(argv[2]) : 1;")
+
-print("\tunsigned topic_instance = (argc > 3) ? atoi(argv[3]) : 0;\n")
 for index, (m, t) in enumerate(zip(messages, topics)):
 	if index == 0:
-		print("\tif (strncmp(argv[1],\"%s\",50) == 0) {" % t)
+		print("\tif (strncmp(topic_name,\"%s\", %d) == 0) {" % (t, len(t)))
 	else:
-		print("\t} else if (strncmp(argv[1],\"%s\",50) == 0) {" % t)
+		print("\t} else if (strcmp(topic_name,\"%s\") == 0) {" % (t))
-	print("\t\tlisten_%s(num_msgs, topic_instance);" % t)
+	print("\t\tlistener<%s_s>(ORB_ID(%s), num_msgs, topic_instance, topic_interval);" % (m, t))
 
 print("\t} else {")
 print("\t\t printf(\" Topic did not match any known topics\\n\");")
 print("\t}")
-print("\t return 0;")
-print("}\n")
 
-for index, (m, t) in enumerate(zip(messages, topics)):
+print("}\n")
-	print("void listen_%s(unsigned num_msgs, unsigned topic_instance) {" % t)
-	print("\torb_id_t ID = ORB_ID(%s);" % t)
-	print("\tif (orb_exists(ID, topic_instance) != 0) { printf(\"never published\\n\"); return; }")
-	print("\tint sub = orb_subscribe_multi(ORB_ID(%s), topic_instance);" % t)
-	print("\tbool updated = false;")
-	print("\tunsigned i = 0;")
-	print("\thrt_abstime start_time = hrt_absolute_time();")
-	print("\twhile(i < num_msgs) {")
-	print("\t\torb_check(sub, &updated);")
-	print("\t\tif (i == 0) { updated = true; } else { usleep(500); }")
-	print("\t\tif (updated) {")
-	print("\t\t\tstart_time = hrt_absolute_time();")
-	print("\t\t\ti++;")
-	print("\t\tprintf(\"\\nTOPIC: %s instance %%d #%%d\\n\", topic_instance, i);" % t)
-	print("\t\t%s_s container = {};" % m)
-	print("\t\torb_copy(ID, sub, &container);")
-	print("\t\tprint_message(container);")
-	print("\t\t} else {")
-	print("\t\t\tif (check_timeout(start_time)) {")
-	print("\t\t\t\tbreak;")
-	print("\t\t\t}")
-	print("\t\t}")
-	print("\t}")
-	print("\torb_unsubscribe(sub);")
-	print("}\n")

src/systemcmds/topic_listener/listener_main.cpp

@@ -0,0 +1,118 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file listener_main.cpp
+ *
+ * Tool for listening to topics.
+ */
+
+#include <px4_module.h>
+#include <px4_getopt.h>
+
+#include "topic_listener.hpp"
+#include "topic_listener_generated.hpp"
+
+extern "C" __EXPORT int listener_main(int argc, char *argv[]);
+
+static void usage();
+
+int listener_main(int argc, char *argv[])
+{
+	if (argc <= 1) {
+		usage();
+		return 1;
+	}
+
+	char *topic_name = argv[1];
+
+	unsigned topic_instance = 0;
+	unsigned topic_rate = 0;
+	unsigned num_msgs = 0;
+
+	int myoptind = 1;
+	int ch;
+	const char *myoptarg = nullptr;
+
+	while ((ch = px4_getopt(argc, argv, "i:r:n:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+
+		case 'i':
+			topic_instance = strtol(myoptarg, nullptr, 0);
+			break;
+
+		case 'r':
+			topic_rate = strtol(myoptarg, nullptr, 0);
+			break;
+
+		case 'n':
+			num_msgs = strtol(myoptarg, nullptr, 0);
+			break;
+
+		default:
+			usage();
+			return -1;
+			break;
+		}
+	}
+
+	if (num_msgs == 0) {
+		if (topic_rate != 0) {
+			num_msgs = 10 * topic_rate; // arbitrary limit (10 seconds at max rate)
+
+		} else {
+			num_msgs = 1;
+		}
+	}
+
+	listener_generated(topic_name, topic_instance, topic_rate, num_msgs);
+
+	return 0;
+}
+
+
+static void
+usage()
+{
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+Utility to listen on uORB topics and print the data to the console.
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("listener", "command");
+	PRINT_MODULE_USAGE_ARG("<topic_name>", "uORB topic name", false);
+
+	PRINT_MODULE_USAGE_PARAM_INT('i', 0, 0, ORB_MULTI_MAX_INSTANCES - 1, "Topic instance", true);
+	PRINT_MODULE_USAGE_PARAM_INT('n', 1, 0, 100, "Number of messages", true);
+	PRINT_MODULE_USAGE_PARAM_INT('r', 0, 0, 1000, "Subscription rate (unlimited if 0)", true);
+}

src/systemcmds/topic_listener/topic_listener.hpp

@@ -0,0 +1,102 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file listener.hpp
+ *
+ */
+
+#pragma once
+
+#include <px4_defines.h>
+#include <drivers/drv_hrt.h>
+#include <px4_middleware.h>
+#include <px4_app.h>
+#include <px4_config.h>
+#include <uORB/uORB.h>
+#include <string.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <inttypes.h>
+
+template <typename T>
+void listener(const orb_id_t &id, unsigned num_msgs, unsigned topic_instance, unsigned topic_interval)
+{
+	if (orb_exists(id, topic_instance) != 0) {
+		printf("never published\n");
+		return;
+	}
+
+	int sub = orb_subscribe_multi(id, topic_instance);
+	orb_set_interval(sub, topic_interval);
+
+	bool updated = false;
+	unsigned i = 0;
+	hrt_abstime start_time = hrt_absolute_time();
+
+	while (i < num_msgs) {
+		orb_check(sub, &updated);
+
+		if (i == 0) {
+			updated = true;
+
+		} else {
+			usleep(500);
+		}
+
+		if (updated) {
+			start_time = hrt_absolute_time();
+			i++;
+
+			printf("\nTOPIC: %s instance %d #%d\n", id->o_name, topic_instance, i);
+
+			T container;
+
+			if (orb_copy(id, sub, &container) == PX4_OK) {
+				print_message(container);
+
+			} else {
+				PX4_ERR("orb_copy failed");
+			}
+
+		} else {
+			if (hrt_elapsed_time(&start_time) > 2 * 1000 * 1000) {
+				printf("Waited for 2 seconds without a message. Giving up.\n");
+				break;
+			}
+		}
+	}
+
+	orb_unsubscribe(sub);
+}

src/systemcmds/topic_listener/topic_listener_generated.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2017 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -31,25 +31,6 @@
  *
  ****************************************************************************/
 
-/**
+#pragma once
- * @file usage.cpp
- * Note: this file is not used in the build, it's just for documenting the module
- */
-
-#include <px4_module.h>
-
-static void
-usage()
-{
-	PRINT_MODULE_DESCRIPTION(
-		R"DESCR_STR(
-Utility to listen on uORB topics and print the data to the console.
-
-Limitation: it can only listen to the first instance of a topic.
-
-)DESCR_STR");
-
-	PRINT_MODULE_USAGE_NAME_SIMPLE("listener", "command");
-	PRINT_MODULE_USAGE_ARG("<topic_name> [<num_msgs>]", "uORB topic name and optionally number of messages (default=1)", false);
-}
 
+void listener_generated(char *topic_name, int topic_instance, int topic_rate, int num_msgs);