uorb: add message format compatibility check

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

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;