From 370e5b4a57574be14d88d46b22dab247c1bb9163 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 10 Sep 2024 10:12:53 +0200 Subject: [PATCH] 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 | 2 ++ msg/VehicleGlobalPosition.msg | 1 + msg/VehicleLocalPosition.msg | 1 + src/modules/uxrce_dds_client/dds_topics.h.em | 22 ++++++++++++++++++++ src/modules/uxrce_dds_client/utilities.hpp | 21 ++++++++++++++----- 5 files changed, 42 insertions(+), 5 deletions(-) diff --git a/msg/VehicleAttitude.msg b/msg/VehicleAttitude.msg index 99e6f25c2e..32bddc9337 100644 --- a/msg/VehicleAttitude.msg +++ b/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) diff --git a/msg/VehicleGlobalPosition.msg b/msg/VehicleGlobalPosition.msg index e37155fe25..4cad859134 100644 --- a/msg/VehicleGlobalPosition.msg +++ b/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) diff --git a/msg/VehicleLocalPosition.msg b/msg/VehicleLocalPosition.msg index 0e74ac0f4b..61a06e9428 100644 --- a/msg/VehicleLocalPosition.msg +++ b/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) diff --git a/src/modules/uxrce_dds_client/dds_topics.h.em b/src/modules/uxrce_dds_client/dds_topics.h.em index 3c4311e6c3..a3d3a45162 100644 --- a/src/modules/uxrce_dds_client/dds_topics.h.em +++ b/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 +class MessageVersionHelper +{ + template + static constexpr uint32_t get(decltype(&C::MESSAGE_VERSION)) { return C::MESSAGE_VERSION; } + template + static constexpr uint32_t get(...) { return 0; } +public: + static constexpr uint32_t m = get(0); +}; + +template +static constexpr uint32_t get_message_version() { + return MessageVersionHelper::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(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); } diff --git a/src/modules/uxrce_dds_client/utilities.hpp b/src/modules/uxrce_dds_client/utilities.hpp index 70886ec795..dd029bf864 100644 --- a/src/modules/uxrce_dds_client/utilities.hpp +++ b/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; }