From 17916b7fdc91822f7414f1f2da748ab8bd622cad Mon Sep 17 00:00:00 2001 From: Konrad Date: Tue, 14 May 2024 13:37:54 +0200 Subject: [PATCH] uxcre_dds_client: use topic name as defined in the dds_topics.yaml to register stream --- src/modules/uxrce_dds_client/dds_topics.h.em | 6 ++++-- src/modules/uxrce_dds_client/utilities.hpp | 18 +++++++++++------- 2 files changed, 15 insertions(+), 9 deletions(-) diff --git a/src/modules/uxrce_dds_client/dds_topics.h.em b/src/modules/uxrce_dds_client/dds_topics.h.em index cade47b5fd..3c4311e6c3 100644 --- a/src/modules/uxrce_dds_client/dds_topics.h.em +++ b/src/modules/uxrce_dds_client/dds_topics.h.em @@ -43,6 +43,7 @@ struct SendSubscription { const struct orb_metadata *orb_meta; uxrObjectId data_writer; const char* dds_type_name; + const char* topic; uint32_t topic_size; UcdrSerializeMethod ucdr_serialize_method; }; @@ -54,6 +55,7 @@ struct SendTopicsSubs { { ORB_ID(@(pub['topic_simple'])), uxr_object_id(0, UXR_INVALID_ID), "@(pub['dds_type'])", + "@(pub['topic'])", ucdr_topic_size_@(pub['simple_base_type'])(), &ucdr_serialize_@(pub['simple_base_type']), }, @@ -96,7 +98,7 @@ void SendTopicsSubs::update(uxrSession *session, uxrStreamId reliable_out_stream orb_copy(send_subscriptions[idx].orb_meta, fds[idx].fd, &topic_data); 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].orb_meta->o_name, + 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].dds_type_name, send_subscriptions[idx].data_writer); } @@ -169,7 +171,7 @@ bool RcvTopicsPubs::init(uxrSession *session, uxrStreamId reliable_out_stream_id @[ for idx, sub in enumerate(subscriptions + subscriptions_multi)]@ { uint16_t queue_depth = orb_get_queue_size(ORB_ID(@(sub['simple_base_type']))) * 2; // use a bit larger queue size than internal - create_data_reader(session, reliable_out_stream_id, best_effort_in_stream_id, participant_id, @(idx), client_namespace, "@(sub['topic_simple'])", "@(sub['dds_type'])", queue_depth); + create_data_reader(session, reliable_out_stream_id, best_effort_in_stream_id, participant_id, @(idx), client_namespace, "@(sub['topic'])", "@(sub['dds_type'])", queue_depth); } @[ end for]@ diff --git a/src/modules/uxrce_dds_client/utilities.hpp b/src/modules/uxrce_dds_client/utilities.hpp index 5c7fb2e9dc..70886ec795 100644 --- a/src/modules/uxrce_dds_client/utilities.hpp +++ b/src/modules/uxrce_dds_client/utilities.hpp @@ -23,25 +23,29 @@ uxrObjectId topic_id_from_orb(ORB_ID orb_id, uint8_t instance = 0) return uxrObjectId{}; } -static bool generate_topic_name(char *topic, const char *client_namespace, const char *direction, const char *name) +static bool generate_topic_name(char *topic_name, const char *client_namespace, const char *topic) { + if (topic[0] == '/') { + topic++; + } + if (client_namespace != nullptr) { - int ret = snprintf(topic, TOPIC_NAME_SIZE, "rt/%s/fmu/%s/%s", client_namespace, direction, name); + int ret = snprintf(topic_name, TOPIC_NAME_SIZE, "rt/%s/%s", client_namespace, topic); return (ret > 0 && ret < TOPIC_NAME_SIZE); } - int ret = snprintf(topic, TOPIC_NAME_SIZE, "rt/fmu/%s/%s", direction, name); + int ret = snprintf(topic_name, TOPIC_NAME_SIZE, "rt/%s", topic); 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_name_simple, const char *type_name, + ORB_ID orb_id, const char *client_namespace, const char *topic, const char *type_name, uxrObjectId &datawriter_id) { // topic char topic_name[TOPIC_NAME_SIZE]; - if (!generate_topic_name(topic_name, client_namespace, "out", topic_name_simple)) { + if (!generate_topic_name(topic_name, client_namespace, topic)) { PX4_ERR("topic path too long"); return false; } @@ -87,13 +91,13 @@ static bool create_data_writer(uxrSession *session, uxrStreamId reliable_out_str } static bool create_data_reader(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId input_stream_id, - uxrObjectId participant_id, uint16_t index, const char *client_namespace, const char *topic_name_simple, + uxrObjectId participant_id, uint16_t index, const char *client_namespace, const char *topic, const char *type_name, uint16_t queue_depth) { // topic char topic_name[TOPIC_NAME_SIZE]; - if (!generate_topic_name(topic_name, client_namespace, "in", topic_name_simple)) { + if (!generate_topic_name(topic_name, client_namespace, topic)) { PX4_ERR("topic path too long"); return false; }