TEST: add DDS message versioning

This adds "_v" + string(T::MESSAGE_VERSION) to the ROS topic if the message
contains a MESSAGE_VERSION field.

msg/VehicleAttitude.msg

@@ -1,6 +1,8 @@
 # This is similar to the mavlink message ATTITUDE_QUATERNION, but for onboard use
 # The quaternion uses the Hamilton convention, and the order is q(w, x, y, z)
 
+uint32 MESSAGE_VERSION = 1
+
 uint64 timestamp                # time since system start (microseconds)
 
 uint64 timestamp_sample         # the timestamp of the raw data (microseconds)

msg/VehicleGlobalPosition.msg

@@ -4,6 +4,7 @@
 # estimator, which will take more sources of information into account than just GPS,
 # e.g. control inputs of the vehicle in a Kalman-filter implementation.
 #
+uint32 MESSAGE_VERSION = 1
 
 uint64 timestamp		# time since system start (microseconds)
 uint64 timestamp_sample         # the timestamp of the raw data (microseconds)

msg/VehicleLocalPosition.msg

@@ -1,5 +1,6 @@
 # Fused local position in NED.
 # The coordinate system origin is the vehicle position at the time when the EKF2-module was started.
+uint32 MESSAGE_VERSION = 1
 
 uint64 timestamp			# time since system start (microseconds)
 uint64 timestamp_sample                 # the timestamp of the raw data (microseconds)

src/modules/uxrce_dds_client/dds_topics.h.em

@@ -39,11 +39,31 @@ static constexpr int max_topic_size = 512;
 static_assert(sizeof(@(pub['simple_base_type'])_s) <= max_topic_size, "topic too large, increase max_topic_size");
 @[    end for]@
 
+
+
+// SFINAE to use R::MESSAGE_VERSION if it exists, and 0 otherwise
+template <typename R>
+class MessageVersionHelper
+{
+	template <typename C>
+	static constexpr uint32_t get(decltype(&C::MESSAGE_VERSION)) { return C::MESSAGE_VERSION; }
+	template <typename C>
+	static constexpr uint32_t get(...) { return 0; }
+public:
+	static constexpr uint32_t m = get<R>(0);
+};
+
+template <typename T>
+static constexpr uint32_t get_message_version() {
+	return MessageVersionHelper<T>::m;
+}
+
 struct SendSubscription {
 	const struct orb_metadata *orb_meta;
 	uxrObjectId data_writer;
 	const char* dds_type_name;
 	const char* topic;
+	uint32_t message_version;
 	uint32_t topic_size;
 	UcdrSerializeMethod ucdr_serialize_method;
 };
@@ -56,6 +76,7 @@ struct SendTopicsSubs {
 			  uxr_object_id(0, UXR_INVALID_ID),
 			  "@(pub['dds_type'])",
 			  "@(pub['topic'])",
+			  get_message_version<@(pub['simple_base_type'])_s>(),
 			  ucdr_topic_size_@(pub['simple_base_type'])(),
 			  &ucdr_serialize_@(pub['simple_base_type']),
 			},
@@ -99,6 +120,7 @@ void SendTopicsSubs::update(uxrSession *session, uxrStreamId reliable_out_stream
 			if (send_subscriptions[idx].data_writer.id == UXR_INVALID_ID) {
 				// data writer not created yet
 				create_data_writer(session, reliable_out_stream_id, participant_id, static_cast<ORB_ID>(send_subscriptions[idx].orb_meta->o_id), client_namespace, send_subscriptions[idx].topic,
+								   send_subscriptions[idx].message_version,
 								   send_subscriptions[idx].dds_type_name, send_subscriptions[idx].data_writer);
 			}
 

src/modules/uxrce_dds_client/utilities.hpp

@@ -23,29 +23,40 @@ uxrObjectId topic_id_from_orb(ORB_ID orb_id, uint8_t instance = 0)
 	return uxrObjectId{};
 }
 
-static bool generate_topic_name(char *topic_name, const char *client_namespace, const char *topic)
+static bool generate_topic_name(char *topic_name, const char *client_namespace, const char *topic,
+				uint32_t message_version = 0)
 {
 	if (topic[0] == '/') {
 		topic++;
 	}
 
+	char version[16];
+
+	if (message_version != 0) {
+		snprintf(version, sizeof(version), "_v%u", message_version);
+		version[sizeof(version) - 1] = '\0';
+
+	} else {
+		version[0] = '\0';
+	}
+
 	if (client_namespace != nullptr) {
-		int ret = snprintf(topic_name, TOPIC_NAME_SIZE, "rt/%s/%s", client_namespace, topic);
+		int ret = snprintf(topic_name, TOPIC_NAME_SIZE, "rt/%s/%s%s", client_namespace, topic, version);
 		return (ret > 0 && ret < TOPIC_NAME_SIZE);
 	}
 
-	int ret = snprintf(topic_name, TOPIC_NAME_SIZE, "rt/%s", topic);
+	int ret = snprintf(topic_name, TOPIC_NAME_SIZE, "rt/%s%s", topic, version);
 	return (ret > 0 && ret < TOPIC_NAME_SIZE);
 }
 
 static bool create_data_writer(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrObjectId participant_id,
-			       ORB_ID orb_id, const char *client_namespace, const char *topic, const char *type_name,
+			       ORB_ID orb_id, const char *client_namespace, const char *topic, uint32_t message_version, const char *type_name,
 			       uxrObjectId &datawriter_id)
 {
 	// topic
 	char topic_name[TOPIC_NAME_SIZE];
 
-	if (!generate_topic_name(topic_name, client_namespace, topic)) {
+	if (!generate_topic_name(topic_name, client_namespace, topic, message_version)) {
 		PX4_ERR("topic path too long");
 		return false;
 	}