topic_listener: avoid code generation, use existing metadata at runtime

This reduces flash size for v5 by ~110KB, the topic listener now only adds
about 1.2KB.

msg/templates/uorb/msg.cpp.em

@@ -82,19 +82,11 @@ constexpr char __orb_@(topic_name)_fields[] = "@( ";".join(topic_fields) );";
 ORB_DEFINE(@multi_topic, struct @uorb_struct, @(struct_size-padding_end_size), __orb_@(topic_name)_fields, static_cast<uint8_t>(ORB_ID::@multi_topic));
 @[end for]
 
-void print_message(const @uorb_struct &message)
+void print_message(const orb_metadata *meta, const @uorb_struct& message)
 {
-@[if constrained_flash]
+	if (sizeof(message) != meta->o_size) {
-	(void)message;
+		printf("unexpected message size for %s: %zu != %i\n", meta->o_name, sizeof(message), meta->o_size);
-	PX4_INFO_RAW("Not implemented on flash constrained hardware\n");
+		return;
-@[else]
+	}
-	PX4_INFO_RAW(" @(uorb_struct)\n");
+	orb_print_message_internal(meta, &message, true);
-
-	const hrt_abstime now = hrt_absolute_time();
-
-@[for field in sorted_fields]@
-	@( print_field(field) )@
-@[end for]@
-@[end if]@
-
 }

msg/templates/uorb/msg.h.em

@@ -133,5 +133,5 @@ ORB_DECLARE(@multi_topic);
 @[end for]
 
 #ifdef __cplusplus
-void print_message(const @uorb_struct& message);
+void print_message(const orb_metadata *meta, const @uorb_struct& message);
 #endif

msg/tools/px_generate_uorb_topic_helper.py

@@ -241,128 +241,6 @@ def convert_type(spec_type, use_short_type=False):
     return c_type
 
 
-def print_field(field):
-    """
-    Echo printf line
-    """
-
-    # check if there are any upper case letters in the field name
-    assert not any(a.isupper()
-                   for a in field.name), "%r field contains uppercase letters" % field.name
-
-    # skip padding
-    if field.name.startswith('_padding'):
-        return
-
-    bare_type = field.type
-    if '/' in field.type:
-        # removing prefix
-        bare_type = (bare_type.split('/'))[1]
-
-    msg_type, is_array, array_length = genmsg.msgs.parse_type(bare_type)
-
-    field_name = ""
-
-    if is_array:
-        c_type = "["
-
-        if msg_type in type_map:
-            p_type = type_printf_map[msg_type]
-
-        else:
-            for i in range(array_length):
-                print(("PX4_INFO_RAW(\"\\t" + field.type +
-                      " " + field.name + "[" + str(i) + "]\");"))
-                print((" print_message(message." +
-                      field.name + "[" + str(i) + "]);"))
-            return
-
-        for i in range(array_length):
-
-            if i > 0:
-                c_type += ", "
-                field_name += ", "
-
-            if "float32" in field.type:
-                field_name += "(double)message." + \
-                    field.name + "[" + str(i) + "]"
-            else:
-                field_name += "message." + field.name + "[" + str(i) + "]"
-
-            c_type += str(p_type)
-
-        c_type += "]"
-
-    else:
-        c_type = msg_type
-        if msg_type in type_map:
-            c_type = type_printf_map[msg_type]
-
-            field_name = "message." + field.name
-
-            # cast double
-            if field.type == "float32":
-                field_name = "(double)" + field_name
-            elif field.type == "bool":
-                c_type = '%s'
-                field_name = "(" + field_name + " ? \"True\" : \"False\")"
-
-        else:
-            print(("PX4_INFO_RAW(\"\\n\\t" + field.name + "\");"))
-            print(("\tprint_message(message." + field.name + ");"))
-            return
-
-    if field.name == 'timestamp':
-        print(("if (message.timestamp != 0) {\n\t\tPX4_INFO_RAW(\"\\t" + field.name +
-              ": " + c_type + "  (%.6f seconds ago)\\n\", " + field_name +
-               ", (now - message.timestamp) / 1e6);\n\t} else {\n\t\tPX4_INFO_RAW(\"\\n\");\n\t}"))
-    elif field.name == 'timestamp_sample':
-        print(("\n\tPX4_INFO_RAW(\"\\t" + field.name + ": " + c_type + "  (" + c_type +
-              " us before timestamp)\\n\", " + field_name + ", message.timestamp - message.timestamp_sample);\n\t"))
-    elif field.name == 'device_id':
-        print("char device_id_buffer[80];")
-        print("device::Device::device_id_print_buffer(device_id_buffer, sizeof(device_id_buffer), message.device_id);")
-        print("PX4_INFO_RAW(\"\\tdevice_id: %\" PRId32 \" (%s) \\n\", message.device_id, device_id_buffer);")
-    elif field.name == 'accel_device_id':
-        print("char accel_device_id_buffer[80];")
-        print("device::Device::device_id_print_buffer(accel_device_id_buffer, sizeof(accel_device_id_buffer), message.accel_device_id);")
-        print("PX4_INFO_RAW(\"\\taccel_device_id: %\" PRId32 \" (%s) \\n\", message.accel_device_id, accel_device_id_buffer);")
-    elif field.name == 'gyro_device_id':
-        print("char gyro_device_id_buffer[80];")
-        print("device::Device::device_id_print_buffer(gyro_device_id_buffer, sizeof(gyro_device_id_buffer), message.gyro_device_id);")
-        print("PX4_INFO_RAW(\"\\tgyro_device_id: %\" PRId32 \" (%s) \\n\", message.gyro_device_id, gyro_device_id_buffer);")
-    elif field.name == 'baro_device_id':
-        print("char baro_device_id_buffer[80];")
-        print("device::Device::device_id_print_buffer(baro_device_id_buffer, sizeof(baro_device_id_buffer), message.baro_device_id);")
-        print("PX4_INFO_RAW(\"\\tbaro_device_id: %\" PRId32 \" (%s) \\n\", message.baro_device_id, baro_device_id_buffer);")
-    elif field.name == 'mag_device_id':
-        print("char mag_device_id_buffer[80];")
-        print("device::Device::device_id_print_buffer(mag_device_id_buffer, sizeof(mag_device_id_buffer), message.mag_device_id);")
-        print("PX4_INFO_RAW(\"\\tmag_device_id: %\" PRId32 \" (%s) \\n\", message.mag_device_id, mag_device_id_buffer);")
-    elif (field.name == 'q' or 'q_' in field.name) and field.type == 'float32[4]':
-        # float32[4] q/q_d/q_reset/delta_q_reset
-        print("{")
-        print(
-            "\t\tmatrix::Eulerf euler{matrix::Quatf{message." + field.name + "}};")
-        print("\t\tPX4_INFO_RAW(\"\\t" + field.name + ": " + c_type + "  (Roll: %.1f deg, Pitch: %.1f deg, Yaw: %.1f deg" ")\\n\", " +
-              field_name + ", (double)math::degrees(euler(0)), (double)math::degrees(euler(1)), (double)math::degrees(euler(2)));\n\t")
-        print("\t}")
-
-    elif ("flags" in field.name or "bits" in field.name) and "uint" in field.type:
-        # print bits of fixed width unsigned integers (uint8, uint16, uint32) if name contains flags or bits
-        print("PX4_INFO_RAW(\"\\t" + field.name + ": " +
-              c_type + " (0b\", " + field_name + ");")
-        print("\tfor (int i = (sizeof(" + field_name + ") * 8) - 1; i >= 0; i--) { PX4_INFO_RAW(\"%lu%s\", (unsigned long) "
-              + field_name + " >> i & 1, ((unsigned)i < (sizeof(" + field_name + ") * 8) - 1 && i % 4 == 0 && i > 0) ? \"'\" : \"\"); }")
-        print("\tPX4_INFO_RAW(\")\\n\");")
-    elif is_array and 'char' in field.type:
-        print(("PX4_INFO_RAW(\"\\t" + field.name + ": \\\"%." +
-              str(array_length) + "s\\\" \\n\", message." + field.name + ");"))
-    else:
-        print(("PX4_INFO_RAW(\"\\t" + field.name + ": " +
-              c_type + "\\n\", " + field_name + ");"))
-
-
 def print_field_def(field):
     """
     Print the C type from a field

platforms/common/uORB/uORB.cpp

@@ -41,6 +41,10 @@
 #include "uORBManager.hpp"
 #include "uORBCommon.hpp"
 
+#include <lib/drivers/device/Device.hpp>
+#include <matrix/Quaternion.hpp>
+#include <mathlib/mathlib.h>
+
 static uORB::DeviceMaster *g_dev = nullptr;
 
 int uorb_start(void)
@@ -204,3 +208,270 @@ const char *orb_get_c_type(unsigned char short_type)
 
 	return nullptr;
 }
+
+
+void orb_print_message_internal(const orb_metadata *meta, const void *data, bool print_topic_name)
+{
+	if (print_topic_name) {
+		PX4_INFO_RAW(" %s\n", meta->o_name);
+	}
+
+	const hrt_abstime now = hrt_absolute_time();
+	hrt_abstime topic_timestamp = 0;
+
+	const uint8_t *data_ptr = (const uint8_t *)data;
+	int data_offset = 0;
+
+	for (int format_idx = 0; meta->o_fields[format_idx] != 0;) {
+		const char *end_field = strchr(meta->o_fields + format_idx, ';');
+
+		if (!end_field) {
+			PX4_ERR("Format error in %s", meta->o_fields);
+			return;
+		}
+
+		const char *c_type = orb_get_c_type(meta->o_fields[format_idx]);
+		const int end_field_idx = end_field - meta->o_fields;
+
+		int array_idx = -1;
+		int field_name_idx = -1;
+
+		for (int field_idx = format_idx; field_idx != end_field_idx; ++field_idx) {
+			if (meta->o_fields[field_idx] == '[') {
+				array_idx = field_idx + 1;
+
+			} else if (meta->o_fields[field_idx] == ' ') {
+				field_name_idx = field_idx + 1;
+				break;
+			}
+		}
+
+		int array_size = 1;
+
+		if (array_idx >= 0) {
+			array_size = strtol(meta->o_fields + array_idx, nullptr, 10);
+		}
+
+		char field_name[80];
+		size_t field_name_len = end_field_idx - field_name_idx;
+
+		if (field_name_len >= sizeof(field_name)) {
+			PX4_ERR("field name too long %s (max: %u)", meta->o_fields, (unsigned)sizeof(field_name));
+			return;
+		}
+
+		memcpy(field_name, meta->o_fields + field_name_idx, field_name_len);
+		field_name[field_name_len] = '\0';
+
+		if (c_type) { // built-in type
+			bool dont_print = false;
+
+			// handle special cases
+			if (strncmp(field_name, "_padding", 8) == 0) {
+				dont_print = true;
+
+			} else if (strcmp(c_type, "char") == 0 && array_size > 1) { // string
+				PX4_INFO_RAW("    %s: \"%.*s\"\n", field_name, array_size, (char *)(data_ptr + data_offset));
+				dont_print = true;
+			}
+
+			if (!dont_print) {
+				PX4_INFO_RAW("    %s: ", field_name);
+			}
+
+			if (!dont_print && array_size > 1) {
+				PX4_INFO_RAW("[");
+			}
+
+			const int previous_data_offset = data_offset;
+
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wcast-align" // the caller ensures data is aligned
+
+			for (int i = 0; i < array_size; ++i) {
+				if (strcmp(c_type, "int8_t") == 0) {
+					if (!dont_print) { PX4_INFO_RAW("%" PRIi8, *(int8_t *)(data_ptr + data_offset)); }
+
+					data_offset += sizeof(int8_t);
+
+				} else if (strcmp(c_type, "int16_t") == 0) {
+					if (!dont_print) { PX4_INFO_RAW("%" PRIi16, *(int16_t *)(data_ptr + data_offset)); }
+
+					data_offset += sizeof(int16_t);
+
+				} else if (strcmp(c_type, "int32_t") == 0) {
+					if (!dont_print) { PX4_INFO_RAW("%" PRIi32, *(int32_t *)(data_ptr + data_offset)); }
+
+					data_offset += sizeof(int32_t);
+
+				} else if (strcmp(c_type, "int64_t") == 0) {
+					if (!dont_print) { PX4_INFO_RAW("%" PRIi64, *(int64_t *)(data_ptr + data_offset)); }
+
+					data_offset += sizeof(int64_t);
+
+				} else if (strcmp(c_type, "uint8_t") == 0) {
+					if (!dont_print) { PX4_INFO_RAW("%" PRIu8, *(uint8_t *)(data_ptr + data_offset)); }
+
+					data_offset += sizeof(uint8_t);
+
+				} else if (strcmp(c_type, "uint16_t") == 0) {
+					if (!dont_print) { PX4_INFO_RAW("%" PRIu16, *(uint16_t *)(data_ptr + data_offset)); }
+
+					data_offset += sizeof(uint16_t);
+
+				} else if (strcmp(c_type, "uint32_t") == 0) {
+					if (!dont_print) { PX4_INFO_RAW("%" PRIu32, *(uint32_t *)(data_ptr + data_offset)); }
+
+					data_offset += sizeof(uint32_t);
+
+				} else if (strcmp(c_type, "uint64_t") == 0) {
+					if (!dont_print) { PX4_INFO_RAW("%" PRIu64, *(uint64_t *)(data_ptr + data_offset)); }
+
+					data_offset += sizeof(uint64_t);
+
+				} else if (strcmp(c_type, "float") == 0) {
+					if (!dont_print) { PX4_INFO_RAW("%.4f", (double) * (float *)(data_ptr + data_offset)); }
+
+					data_offset += sizeof(float);
+
+				} else if (strcmp(c_type, "double") == 0) {
+					if (!dont_print) { PX4_INFO_RAW("%.4f", *(double *)(data_ptr + data_offset)); }
+
+					data_offset += sizeof(double);
+
+				} else if (strcmp(c_type, "bool") == 0) {
+					if (!dont_print) { PX4_INFO_RAW("%s", *(bool *)(data_ptr + data_offset) ? "True" : "False"); }
+
+					data_offset += sizeof(bool);
+
+				} else if (strcmp(c_type, "char") == 0) {
+					if (!dont_print) { PX4_INFO_RAW("%i", (int) * (char *)(data_ptr + data_offset)); }
+
+					data_offset += sizeof(char);
+
+				} else {
+					PX4_ERR("unknown type: %s", c_type);
+					return;
+				}
+
+				if (!dont_print && i < array_size - 1) {
+					PX4_INFO_RAW(", ");
+				}
+			}
+
+			if (!dont_print && array_size > 1) {
+				PX4_INFO_RAW("]");
+			}
+
+			// handle special cases
+			if (array_size == 1) {
+				if (strcmp(c_type, "uint64_t") == 0 && strcmp(field_name, "timestamp") == 0) {
+					topic_timestamp = *(uint64_t *)(data_ptr + previous_data_offset);
+
+					if (topic_timestamp != 0) {
+						PX4_INFO_RAW(" (%.6f seconds ago)", (double)((now - topic_timestamp) / 1e6f));
+					}
+
+				} else if (strcmp(c_type, "uint64_t") == 0 && strcmp(field_name, "timestamp_sample") == 0) {
+					hrt_abstime timestamp = *(uint64_t *)(data_ptr + previous_data_offset);
+
+					if (topic_timestamp != 0 && timestamp != 0) {
+						PX4_INFO_RAW(" (%i us before timestamp)", (int)(topic_timestamp - timestamp));
+					}
+
+				} else if (strstr(field_name, "flags") != nullptr) {
+					// bitfield
+					unsigned field_size = 0;
+					unsigned long value = 0;
+
+					if (strcmp(c_type, "uint8_t") == 0) {
+						field_size = sizeof(uint8_t);
+						value = *(uint8_t *)(data_ptr + previous_data_offset);
+
+					} else if (strcmp(c_type, "uint16_t") == 0) {
+						field_size = sizeof(uint16_t);
+						value = *(uint16_t *)(data_ptr + previous_data_offset);
+
+					} else if (strcmp(c_type, "uint32_t") == 0) {
+						field_size = sizeof(uint32_t);
+						value = *(uint32_t *)(data_ptr + previous_data_offset);
+					}
+
+					if (field_size > 0) {
+						PX4_INFO_RAW(" (0b");
+
+						for (int i = (field_size * 8) - 1; i >= 0; i--) {
+							PX4_INFO_RAW("%lu%s", (value >> i) & 1, ((unsigned)i < (field_size * 8) - 1 && i % 4 == 0 && i > 0) ? "'" : "");
+						}
+
+						PX4_INFO_RAW(")");
+					}
+
+				} else if (strcmp(c_type, "uint32_t") == 0 && strstr(field_name, "device_id") != nullptr) {
+					// Device ID
+					uint32_t device_id = *(uint32_t *)(data_ptr + previous_data_offset);
+					char device_id_buffer[80];
+					device::Device::device_id_print_buffer(device_id_buffer, sizeof(device_id_buffer), device_id);
+					PX4_INFO_RAW(" (%s)", device_id_buffer);
+				}
+
+			} else if (array_size == 4 && strcmp(c_type, "float") == 0 && (strcmp(field_name, "q") == 0
+					|| strncmp(field_name, "q_", 2) == 0)) {
+				// attitude
+				float *attitude = (float *)(data_ptr + previous_data_offset);
+				matrix::Eulerf euler{matrix::Quatf{attitude}};
+				PX4_INFO_RAW(" (Roll: %.1f deg, Pitch: %.1f deg, Yaw: %.1f deg)",
+					     (double)math::degrees(euler(0)), (double)math::degrees(euler(1)), (double)math::degrees(euler(2)));
+			}
+
+#pragma GCC diagnostic pop
+
+			PX4_INFO_RAW("\n");
+
+		} else {
+
+			// extract the topic name
+			char topic_name[80];
+			const size_t topic_name_len = array_size > 1 ? array_idx - format_idx - 1 : field_name_idx - format_idx - 1;
+
+			if (topic_name_len >= sizeof(topic_name)) {
+				PX4_ERR("topic name too long in %s (max: %u)", meta->o_name, (unsigned)sizeof(topic_name));
+				return;
+			}
+
+			memcpy(topic_name, meta->o_fields + format_idx, topic_name_len);
+			field_name[topic_name_len] = '\0';
+
+			// find the metadata
+			const orb_metadata *const *topics = orb_get_topics();
+			const orb_metadata *found_topic = nullptr;
+
+			for (size_t i = 0; i < orb_topics_count(); i++) {
+				if (strcmp(topics[i]->o_name, topic_name) == 0) {
+					found_topic = topics[i];
+					break;
+				}
+			}
+
+			if (!found_topic) {
+				PX4_ERR("Topic %s did not match any known topics", topic_name);
+				return;
+			}
+
+			// print recursively
+			for (int i = 0; i < array_size; ++i) {
+				PX4_INFO_RAW("  %s", field_name);
+
+				if (array_size > 1) {
+					PX4_INFO_RAW("[%i]", i);
+				}
+
+				PX4_INFO_RAW(" (%s):\n", topic_name);
+				orb_print_message_internal(found_topic, data_ptr + data_offset, false);
+				data_offset += found_topic->o_size;
+			}
+		}
+
+		format_idx = end_field_idx + 1;
+	}
+}

platforms/common/uORB/uORB.h

@@ -240,6 +240,14 @@ extern int	orb_get_interval(int handle, unsigned *interval) __EXPORT;
  */
 const char *orb_get_c_type(unsigned char short_type);
 
+/**
+ * Print a topic to console. Do not call this directly, use print_message() instead.
+ * @param meta orb topic metadata
+ * @param data expected to be aligned to the largest member
+ */
+void orb_print_message_internal(const struct orb_metadata *meta, const void *data, bool print_topic_name);
+
+
 __END_DECLS
 
 /* Diverse uORB header defines */ //XXX: move to better location

src/drivers/gps/gps.cpp

@@ -1045,7 +1045,7 @@ GPS::print_status()
 		PX4_INFO("rate publication:\t\t%6.2f Hz", (double)_rate);
 		PX4_INFO("rate RTCM injection:\t%6.2f Hz", (double)_rate_rtcm_injection);
 
-		print_message(_report_gps_pos);
+		print_message(ORB_ID(sensor_gps), _report_gps_pos);
 	}
 
 	if (_instance == Instance::Main && _secondary_instance.load()) {

src/drivers/px4io/px4io.cpp

@@ -1508,7 +1508,7 @@ int PX4IO::print_status()
 	uORB::SubscriptionData<px4io_status_s> status_sub{ORB_ID(px4io_status)};
 	status_sub.update();
 
-	print_message(status_sub.get());
+	print_message(ORB_ID(px4io_status), status_sub.get());
 
 	/* now clear alarms */
 	io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, 0x0000);

src/drivers/rc_input/RCInput.cpp

@@ -861,7 +861,7 @@ int RCInput::print_status()
 	perf_print_counter(_publish_interval_perf);
 
 	if (hrt_elapsed_time(&_rc_in.timestamp) < 1_s) {
-		print_message(_rc_in);
+		print_message(ORB_ID(input_rc), _rc_in);
 	}
 
 	return 0;

src/drivers/rpm/rpm_simulator/rpm_simulator.cpp

@@ -72,7 +72,7 @@ int rpm_simulator_main(int argc, char *argv[])
 
 	// Publish data and let the user know what was published
 	rpm_pub.publish(rpm);
-	print_message(rpm);
+	print_message(ORB_ID(rpm), rpm);
 
 	return 0;
 }

src/drivers/uavcan_v1/Subscribers/legacy/LegacyBatteryInfo.hpp

@@ -120,7 +120,7 @@ public:
 
 
 		_battery_status_pub.publish(bat_status);
-		print_message(bat_status);
+		print_message(ORB_ID(battery_status), bat_status);
 	};
 
 private:

src/modules/airship_att_control/airship_att_control_main.cpp

@@ -170,7 +170,7 @@ int AirshipAttitudeControl::print_status()
 
 	perf_print_counter(_loop_perf);
 
-	print_message(_actuators);
+	print_message(ORB_ID(actuator_controls), _actuators);
 
 	return 0;
 }

src/systemcmds/topic_listener/CMakeLists.txt

@@ -31,31 +31,11 @@
 #
 ############################################################################
 
-add_custom_command(OUTPUT listener_generated.cpp
-	COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_listener.py ${PX4_SOURCE_DIR} ${msg_files} > listener_generated.cpp
-	DEPENDS generate_listener.py ${msg_files}
-)
-
-add_custom_target(generate_topic_listener
-	DEPENDS
-		listener_generated.cpp
-		generate_listener.py
-)
-add_dependencies(prebuild_targets generate_topic_listener)
-
 px4_add_module(
 	MODULE systemcmds__topic_listener
 	MAIN listener
-	COMPILE_FLAGS
 	STACK_MAIN 4096
-	INCLUDES
-		${CMAKE_CURRENT_SOURCE_DIR}
-		${CMAKE_CURRENT_BINARY_DIR}
 	SRCS
 		listener_main.cpp
-		${CMAKE_CURRENT_BINARY_DIR}/listener_generated.cpp
-	DEPENDS
-		generate_topic_listener
-		uorb_msgs
 	)
 

src/systemcmds/topic_listener/generate_listener.py

@@ -1,135 +0,0 @@
-#!/usr/bin/python
-
-import glob
-import os
-import sys
-import re
-
-# This script is run from Build/<target>_default.build/$(PX4_BASE)/Firmware/src/systemcmds/topic_listener
-
-# argv[1] must be the full path of the top Firmware dir
-# argv[2] - argv[n] is the full list of msg files
-
-raw_messages = sys.argv[2:]
-
-messages = []
-topics = []
-message_elements = []
-
-for index,m in enumerate(raw_messages):
-	topic_list = []
-
-	msg_path = sys.argv[1]+ '/msg/' + m
-
-	if os.path.isfile(msg_path):
-		# first try opening file in msg/ directory
-		f = open(msg_path,'r')
-	else:
-		# otherwise try opening directly (could be an external module msg)
-		f = open(m,'r')
-
-	for line in f.readlines():
-		items = re.split('\s+', line.strip())
-
-		if '# TOPICS' == ' '.join(items[:2]):
-			for topic in items[2:]:
-				topic_list.append(topic)
-
-	f.close()
-
-	(m_head, m_tail) = os.path.split(m)
-	message = m_tail.split('.')[0]
-
-	if len(topic_list) == 0:
-		topic_list.append(message)
-
-	for topic in topic_list:
-		messages.append(message)
-		topics.append(topic)
-
-num_messages = len(messages);
-
-print("""
-
-/****************************************************************************
- *
- *   Copyright (c) 2015 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 topic_listener.cpp
- *
- * Autogenerated by Tools/generate_listener.py
- *
- * Tool for listening to topics when running flight stack on linux.
- */
-
-#include <drivers/drv_hrt.h>
-#include <px4_platform_common/app.h>
-#include <px4_platform_common/px4_config.h>
-#include <px4_platform_common/log.h>
-#include <uORB/uORB.h>
-#include <string.h>
-#include <stdint.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <inttypes.h>
-
-#include <topic_listener.hpp>
-#include <topic_listener_generated.hpp>
-
-""")
-for m in set(messages):
-	print("#include <uORB/topics/%s.h>" % m)
-
-print ("""
-\nvoid listener_generated(char * topic_name, int topic_instance, int topic_rate, int num_msgs) {
-""")
-
-print("""
-	unsigned topic_interval = 0;
-	if (topic_rate != 0) {
-		topic_interval = 1000 / topic_rate;
-	}
-""")
-
-for index, (m, t) in enumerate(zip(messages, topics)):
-	if index == 0:
-		print("\tif (strcmp(topic_name,\"%s\") == 0) {" % (t))
-	else:
-		print("\t} else if (strcmp(topic_name,\"%s\") == 0) {" % (t))
-	print("\t\tlistener(listener_print_topic<%s_s>, ORB_ID(%s), num_msgs, topic_instance, topic_interval);" % (m, t))
-
-print("\t} else {")
-print("\t\t PX4_INFO_RAW(\" Topic did not match any known topics\\n\");")
-print("\t}")
-
-print("}\n")

src/systemcmds/topic_listener/listener_main.cpp

@@ -42,8 +42,8 @@
 
 #include <poll.h>
 
+#include <uORB/topics/uORBTopics.hpp>
 #include "topic_listener.hpp"
-#include "topic_listener_generated.hpp"
 
 // Amount of time to wait when listening for a message, before giving up.
 static constexpr float MESSAGE_TIMEOUT_S = 2.0f;
@@ -52,7 +52,7 @@ 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, int topic_instance,
+void listener(const orb_id_t &id, unsigned num_msgs, int topic_instance,
 	      unsigned topic_interval)
 {
 
@@ -69,7 +69,7 @@ void listener(listener_print_topic_cb cb, const orb_id_t &id, unsigned num_msgs,
 		if (instances == 1) {
 			PX4_INFO_RAW("\nTOPIC: %s\n", id->o_name);
 			int sub = orb_subscribe(id);
-			cb(id, sub);
+			listener_print_topic(id, sub);
 			orb_unsubscribe(sub);
 
 		} else if (instances > 1) {
@@ -79,7 +79,7 @@ void listener(listener_print_topic_cb cb, const orb_id_t &id, unsigned num_msgs,
 				if (orb_exists(id, i) == PX4_OK) {
 					PX4_INFO_RAW("\nInstance %d:\n", i);
 					int sub = orb_subscribe_multi(id, i);
-					cb(id, sub);
+					listener_print_topic(id, sub);
 					orb_unsubscribe(sub);
 				}
 			}
@@ -141,7 +141,7 @@ void listener(listener_print_topic_cb cb, const orb_id_t &id, unsigned num_msgs,
 
 					PX4_INFO_RAW("\nTOPIC: %s instance %d #%d\n", id->o_name, topic_instance, msgs_received);
 
-					int ret = cb(id, sub);
+					int ret = listener_print_topic(id, sub);
 
 					if (ret != PX4_OK) {
 						PX4_ERR("listener callback failed (%i)", ret);
@@ -208,7 +208,29 @@ int listener_main(int argc, char *argv[])
 		}
 	}
 
-	listener_generated(topic_name, topic_instance, topic_rate, num_msgs);
+
+	unsigned topic_interval = 0;
+
+	if (topic_rate != 0) {
+		topic_interval = 1000 / topic_rate;
+	}
+
+	const orb_metadata *const *topics = orb_get_topics();
+	const orb_metadata *found_topic = nullptr;
+
+	for (size_t i = 0; i < orb_topics_count(); i++) {
+		if (strcmp(topics[i]->o_name, topic_name) == 0) {
+			found_topic = topics[i];
+		}
+	}
+
+	if (found_topic) {
+		listener(found_topic, num_msgs, topic_instance, topic_interval);
+
+	} else {
+		PX4_INFO_RAW("Topic %s did not match any known topics\n", topic_name);
+		return -1;
+	}
 
 	return 0;
 }

src/systemcmds/topic_listener/topic_listener.hpp

@@ -50,21 +50,24 @@
 #include <stdlib.h>
 #include <inttypes.h>
 
-typedef int(*listener_print_topic_cb)(const orb_id_t &orb_id, int subscription);
+inline int listener_print_topic(const orb_id_t &orb_id, int subscription)
-
-template <typename T>
-int listener_print_topic(const orb_id_t &orb_id, int subscription)
 {
-	T container;
+	static constexpr int max_size = 512;
+	alignas(8) char container[max_size];
+
+	if (orb_id->o_size > max_size) {
+		PX4_ERR("topic %s too large (%i > %i)", orb_id->o_name, orb_id->o_size, max_size);
+		return -1;
+	}
 
 	int ret = orb_copy(orb_id, subscription, &container);
 
 	if (ret == PX4_OK) {
-		print_message(container);
+		orb_print_message_internal(orb_id, &container, true);
 	}
 
 	return ret;
 }
 
-void listener(listener_print_topic_cb cb, const orb_id_t &id, unsigned num_msgs, int topic_instance,
+void listener(const orb_id_t &id, unsigned num_msgs, int topic_instance,
 	      unsigned topic_interval);

src/systemcmds/topic_listener/topic_listener_generated.hpp

@@ -1,36 +0,0 @@
-/****************************************************************************
- *
- *   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.
- *
- ****************************************************************************/
-
-#pragma once
-
-void listener_generated(char *topic_name, int topic_instance, int topic_rate, int num_msgs);