uorb: add message format compatibility check

This can be used by DDS/ROS 2 to check for matching message definitions.

Tools/msg/px_generate_uorb_topic_helper.py

@@ -171,6 +171,61 @@ def get_children_fields(base_type, search_path):
     return spec_temp.parsed_fields()
 
 
+def get_message_fields_str_for_message_hash(msg_fields, search_path):
+    """
+    Get all fields (including for nested types) in the form of:
+'''
+uint64 timestamp
+uint8 esc_count
+uint8 esc_online_flags
+EscReport[8] esc
+uint64 timestamp
+uint32 esc_errorcount
+int32 esc_rpm
+float32 esc_voltage
+uint16 failures
+int8 esc_power
+'''
+    """
+    all_fields_str = ''
+    for field in msg_fields:
+        if field.is_header:
+            continue
+
+        type_name = field.type
+        # detect embedded types
+        sl_pos = type_name.find('/')
+        if sl_pos >= 0:
+            type_name = type_name[sl_pos + 1:]
+
+        all_fields_str += type_name + ' ' + field.name + '\n'
+
+        if sl_pos >= 0: # nested type, add all nested fields
+            children_fields = get_children_fields(field.base_type, search_path)
+            all_fields_str += get_message_fields_str_for_message_hash(children_fields, search_path)
+
+    return all_fields_str
+
+
+def hash_32_fnv1a(data: str):
+    hash_val = 0x811c9dc5
+    prime = 0x1000193
+    for i in range(len(data)):
+        value = ord(data[i])
+        hash_val = hash_val ^ value
+        hash_val *= prime
+        hash_val &= 0xffffffff
+    return hash_val
+
+
+def get_message_hash(msg_fields, search_path):
+    """
+    Get a 32 bit message hash over all fields
+    """
+    all_fields_str = get_message_fields_str_for_message_hash(msg_fields, search_path)
+    return hash_32_fnv1a(all_fields_str)
+
+
 def add_padding_bytes(fields, search_path):
     """
     Add padding fields before the embedded types, at the end and calculate the

Tools/msg/templates/uorb/msg.cpp.em

@@ -58,6 +58,7 @@ from px_generate_uorb_topic_helper import * # this is in Tools/
 
 uorb_struct = '%s_s'%name_snake_case
 
+message_hash = get_message_hash(spec.parsed_fields(), search_path)
 sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True)
 struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path)
 }@
@@ -74,7 +75,7 @@ struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path)
 
 @[for topic in topics]@
 static_assert(static_cast<orb_id_size_t>(ORB_ID::@topic) == @(all_topics.index(topic)), "ORB_ID index mismatch");
-ORB_DEFINE(@topic, struct @uorb_struct, @(struct_size-padding_end_size), static_cast<orb_id_size_t>(ORB_ID::@topic));
+ORB_DEFINE(@topic, struct @uorb_struct, @(struct_size-padding_end_size), @(message_hash)u, static_cast<orb_id_size_t>(ORB_ID::@topic));
 @[end for]
 
 void print_message(const orb_metadata *meta, const @uorb_struct& message)

msg/CMakeLists.txt

@@ -133,6 +133,8 @@ set(msg_files
 	ManualControlSwitches.msg
 	MavlinkLog.msg
 	MavlinkTunnel.msg
+	MessageFormatRequest.msg
+	MessageFormatResponse.msg
 	Mission.msg
 	MissionResult.msg
 	MountOrientation.msg

msg/MessageFormatRequest.msg

@@ -0,0 +1,10 @@
+uint64 timestamp # time since system start (microseconds)
+
+# Request to PX4 to get the hash of a message, to check for message compatibility
+
+uint16 LATEST_PROTOCOL_VERSION = 1 # Current version of this protocol. Increase this whenever the MessageFormatRequest or MessageFormatResponse changes.
+
+uint16 protocol_version           # Must be set to LATEST_PROTOCOL_VERSION. Do not change this field, it must be the first field after the timestamp
+
+char[50] topic_name  # E.g. /fmu/in/vehicle_command
+

msg/MessageFormatResponse.msg

@@ -0,0 +1,11 @@
+uint64 timestamp # time since system start (microseconds)
+
+# Response from PX4 with the format of a message
+
+uint16 protocol_version           # Must be set to LATEST_PROTOCOL_VERSION. Do not change this field, it must be the first field after the timestamp
+
+char[50] topic_name  # E.g. /fmu/in/vehicle_command
+
+bool success
+uint32 message_hash # hash over all message fields
+

platforms/common/uORB/uORB.h

@@ -51,6 +51,7 @@ struct orb_metadata {
 	const char    *o_name;              /**< unique object name */
 	const uint16_t o_size;              /**< object size */
 	const uint16_t o_size_no_padding;   /**< object size w/o padding at the end (for logger) */
+	uint32_t message_hash;	/**< Hash over all fields for message compatibility checks */
 	orb_id_size_t  o_id;                /**< ORB_ID enum */
 };
 
@@ -99,13 +100,15 @@ typedef const struct orb_metadata *orb_id_t;
  * @param _name		The name of the topic.
  * @param _struct	The structure the topic provides.
  * @param _size_no_padding	Struct size w/o padding at the end
+ * @param _message_hash	32 bit message hash over all fields
  * @param _orb_id_enum	ORB ID enum e.g.: ORB_ID::vehicle_status
  */
-#define ORB_DEFINE(_name, _struct, _size_no_padding, _orb_id_enum)		\
+#define ORB_DEFINE(_name, _struct, _size_no_padding, _message_hash, _orb_id_enum)		\
 	const struct orb_metadata __orb_##_name = {	\
 		#_name,					\
 		sizeof(_struct),		\
 		_size_no_padding,			\
+		_message_hash,				\
 		_orb_id_enum				\
 	}; struct hack
 

src/modules/uxrce_dds_client/dds_topics.yaml

@@ -23,6 +23,9 @@ publications:
   - topic: /fmu/out/manual_control_setpoint
     type: px4_msgs::msg::ManualControlSetpoint
 
+  - topic: /fmu/out/message_format_response
+    type: px4_msgs::msg::MessageFormatResponse
+
   - topic: /fmu/out/position_setpoint_triplet
     type: px4_msgs::msg::PositionSetpointTriplet
 
@@ -75,6 +78,9 @@ subscriptions:
   - topic: /fmu/in/arming_check_reply
     type: px4_msgs::msg::ArmingCheckReply
 
+  - topic: /fmu/in/message_format_request
+    type: px4_msgs::msg::MessageFormatRequest
+
   - topic: /fmu/in/mode_completed
     type: px4_msgs::msg::ModeCompleted
 

src/modules/uxrce_dds_client/uxrce_dds_client.cpp

@@ -188,6 +188,67 @@ UxrceddsClient::~UxrceddsClient()
 	}
 }
 
+static void fillMessageFormatResponse(const message_format_request_s &message_format_request,
+				      message_format_response_s &message_format_response)
+{
+	message_format_response.protocol_version = message_format_request_s::LATEST_PROTOCOL_VERSION;
+	message_format_response.success = false;
+
+	if (message_format_request.protocol_version == message_format_request_s::LATEST_PROTOCOL_VERSION) {
+		static_assert(sizeof(message_format_request.topic_name) == sizeof(message_format_response.topic_name), "size mismatch");
+		memcpy(message_format_response.topic_name, message_format_request.topic_name,
+		       sizeof(message_format_response.topic_name));
+
+		// Get the topic name by searching for the last '/'
+		int idx_last_slash = -1;
+		bool found_null = false;
+
+		for (int i = 0; i < (int)sizeof(message_format_request.topic_name); ++i) {
+			if (message_format_request.topic_name[i] == 0) {
+				found_null = true;
+				break;
+			}
+
+			if (message_format_request.topic_name[i] == '/') {
+				idx_last_slash = i;
+			}
+		}
+
+		if (found_null && idx_last_slash != -1) {
+			const char *topic_name = message_format_request.topic_name + idx_last_slash + 1;
+			// Find the format
+			const orb_metadata *const *topics = orb_get_topics();
+			const orb_metadata *topic_meta{nullptr};
+
+			for (size_t i = 0; i < orb_topics_count(); i++) {
+				if (strcmp(topic_name, topics[i]->o_name) == 0) {
+					topic_meta = topics[i];
+					break;
+				}
+			}
+
+			if (topic_meta) {
+				message_format_response.message_hash = topic_meta->message_hash;
+				// The topic type is already checked by DDS
+				message_format_response.success = true;
+			}
+		}
+	}
+
+	message_format_response.timestamp = hrt_absolute_time();
+}
+
+void UxrceddsClient::handleMessageFormatRequest()
+{
+	message_format_request_s message_format_request;
+
+	if (_message_format_request_sub.update(&message_format_request)) {
+		message_format_response_s message_format_response;
+		fillMessageFormatResponse(message_format_request, message_format_response);
+		_message_format_response_pub.publish(message_format_response);
+	}
+}
+
 void UxrceddsClient::run()
 {
 	if (!_comm) {
@@ -405,6 +466,8 @@ void UxrceddsClient::run()
 				}
 			}
 
+			handleMessageFormatRequest();
+
 			// Check for a ping response
 			/* PONG_IN_SESSION_STATUS */
 			if (session.on_pong_flag == 1) {

src/modules/uxrce_dds_client/uxrce_dds_client.h

@@ -38,6 +38,10 @@
 
 #include <src/modules/uxrce_dds_client/dds_topics.h>
 
+#include <uORB/topics/message_format_request.h>
+#include <uORB/topics/message_format_response.h>
+#include <uORB/Subscription.hpp>
+
 #include <lib/timesync/Timesync.hpp>
 
 #include "srv_base.h"
@@ -106,6 +110,11 @@ public:
 private:
 	int setBaudrate(int fd, unsigned baud);
 
+	void handleMessageFormatRequest();
+
+	uORB::Publication<message_format_response_s> _message_format_response_pub{ORB_ID(message_format_response)};
+	uORB::Subscription _message_format_request_sub{ORB_ID(message_format_request)};
+
 	const bool _localhost_only;
 	const bool _custom_participant;
 	const char *_client_namespace;