From 64257391c68b29f01f997ffc2bee3f0d589451ae Mon Sep 17 00:00:00 2001 From: Beniamino Pozzan Date: Mon, 13 Nov 2023 00:41:24 +0000 Subject: [PATCH] [uxrce_dds_client] Allow for arbitrary topic instances to be bridged Signed-off-by: Beniamino Pozzan --- src/modules/uxrce_dds_client/dds_topics.h.em | 11 +++++-- .../uxrce_dds_client/generate_dds_topics.py | 8 +++++ src/modules/uxrce_dds_client/utilities.hpp | 29 ++++++++++++++----- 3 files changed, 39 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 c664d87a9f..c21c1111de 100644 --- a/src/modules/uxrce_dds_client/dds_topics.h.em +++ b/src/modules/uxrce_dds_client/dds_topics.h.em @@ -41,6 +41,7 @@ static_assert(sizeof(@(pub['simple_base_type'])_s) <= max_topic_size, "topic too struct SendSubscription { const struct orb_metadata *orb_meta; + const int8_t instance; uxrObjectId data_writer; const char* dds_type_name; uint32_t topic_size; @@ -52,6 +53,7 @@ struct SendTopicsSubs { SendSubscription send_subscriptions[@(len(publications))] = { @[ for pub in publications]@ { ORB_ID(@(pub['topic_simple'])), + @(pub['instance']), uxr_object_id(0, UXR_INVALID_ID), "@(pub['dds_type'])", ucdr_topic_size_@(pub['simple_base_type'])(), @@ -71,7 +73,12 @@ struct SendTopicsSubs { void SendTopicsSubs::init() { for (unsigned idx = 0; idx < sizeof(send_subscriptions)/sizeof(send_subscriptions[0]); ++idx) { - fds[idx].fd = orb_subscribe(send_subscriptions[idx].orb_meta); + if (send_subscriptions[idx].instance == -1){ + fds[idx].fd = orb_subscribe(send_subscriptions[idx].orb_meta); + } + else{ + fds[idx].fd = orb_subscribe_multi(send_subscriptions[idx].orb_meta, send_subscriptions[idx].instance); + } fds[idx].events = POLLIN; orb_set_interval(fds[idx].fd, UXRCE_DEFAULT_POLL_RATE); } @@ -97,7 +104,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].orb_meta->o_name, - send_subscriptions[idx].dds_type_name, send_subscriptions[idx].data_writer); + send_subscriptions[idx].instance, send_subscriptions[idx].dds_type_name, send_subscriptions[idx].data_writer); } if (send_subscriptions[idx].data_writer.id != UXR_INVALID_ID) { diff --git a/src/modules/uxrce_dds_client/generate_dds_topics.py b/src/modules/uxrce_dds_client/generate_dds_topics.py index 4052958df8..d16197da7d 100644 --- a/src/modules/uxrce_dds_client/generate_dds_topics.py +++ b/src/modules/uxrce_dds_client/generate_dds_topics.py @@ -98,6 +98,14 @@ def process_message_type(msg_type): msg_type['dds_type'] = msg_type['type'].replace("::msg::", "::msg::dds_::") + "_" # topic_simple: eg vehicle_status msg_type['topic_simple'] = msg_type['topic'].split('/')[-1] + # topic instance + if 'instance' in msg_type: + # if instance is given, check if it it a non negative integer + if not (type(msg_type['instance']) is int and msg_type['instance'] >= 0) : + raise TypeError("`instance` must be a non negative integer") + else: + # if instance is not given, + msg_type['instance'] = -1 pubs_not_empty = msg_map['publications'] is not None if pubs_not_empty: diff --git a/src/modules/uxrce_dds_client/utilities.hpp b/src/modules/uxrce_dds_client/utilities.hpp index 5c7fb2e9dc..5869dc4e26 100644 --- a/src/modules/uxrce_dds_client/utilities.hpp +++ b/src/modules/uxrce_dds_client/utilities.hpp @@ -23,30 +23,44 @@ 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, const char *client_namespace, const char *direction, const char *name, + const int8_t uorb_topic_instance) { if (client_namespace != nullptr) { - int ret = snprintf(topic, TOPIC_NAME_SIZE, "rt/%s/fmu/%s/%s", client_namespace, direction, name); + if (uorb_topic_instance == -1) { + int ret = snprintf(topic, TOPIC_NAME_SIZE, "rt/%s/fmu/%s/%s", client_namespace, direction, name); + return (ret > 0 && ret < TOPIC_NAME_SIZE); + } + + int ret = snprintf(topic, TOPIC_NAME_SIZE, "rt/%s/fmu/%s/%s%d", client_namespace, direction, name, uorb_topic_instance); + return (ret > 0 && ret < TOPIC_NAME_SIZE); + + + } + + if (uorb_topic_instance == -1) { + int ret = snprintf(topic, TOPIC_NAME_SIZE, "rt/fmu/%s/%s", direction, name); return (ret > 0 && ret < TOPIC_NAME_SIZE); } - int ret = snprintf(topic, TOPIC_NAME_SIZE, "rt/fmu/%s/%s", direction, name); + int ret = snprintf(topic, TOPIC_NAME_SIZE, "rt/fmu/%s/%s%d", direction, name, uorb_topic_instance); 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_name_simple, const int8_t uorb_topic_instance, + 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, "out", topic_name_simple, uorb_topic_instance)) { PX4_ERR("topic path too long"); return false; } - uxrObjectId topic_id = topic_id_from_orb(orb_id); + uxrObjectId topic_id = topic_id_from_orb(orb_id, (uorb_topic_instance == -1 ? 0 : uorb_topic_instance)); uint16_t topic_req = uxr_buffer_create_topic_bin(session, reliable_out_stream_id, topic_id, participant_id, topic_name, type_name, UXR_REPLACE); @@ -92,8 +106,9 @@ static bool create_data_reader(uxrSession *session, uxrStreamId reliable_out_str { // topic char topic_name[TOPIC_NAME_SIZE]; + const int8_t no_instance = -1; - if (!generate_topic_name(topic_name, client_namespace, "in", topic_name_simple)) { + if (!generate_topic_name(topic_name, client_namespace, "in", topic_name_simple, no_instance)) { PX4_ERR("topic path too long"); return false; }