From d0f7b7d8fcb905fe7fad54f2cb2bdfc3204c2fe5 Mon Sep 17 00:00:00 2001 From: yannickpoffet Date: Fri, 3 Jul 2026 21:14:40 +0200 Subject: [PATCH] feat(uxrce_dds_client): drain / unlimited rate options for topic bridging (#27688) * feat(uxrce_dds_client): add "drain" and "unlimited" rate options for dds_topics bridging Drain option allows to specify that a queued uORB topic being bridged should have its queue drained completely at every rate interval, instead of sending only the latest message. This is useful to prevent jitter and delay being added on topics subject to burst and irregular publishing patterns. The "unlimited" rate option allows to specify that a uORB topic being bridged should be sent as fast as possible, without any rate limiting. This is useful for reducing latency on topics where no message loss is acceptable and where the uORB topic is being published at a high rate. * fix(dds_topics): remove "drain" option and safe guard against inf loop Drain option is removed to be coupled with rate_limit = unlimited. The loop is gated to the size of the uorb queue to avoid infinite loops if publisher is faster that this loop. * fix(dds_topics): simplification recommended in review --- src/modules/uxrce_dds_client/dds_topics.h.em | 67 ++++++++++++------- .../uxrce_dds_client/generate_dds_topics.py | 20 ++++++ 2 files changed, 63 insertions(+), 24 deletions(-) diff --git a/src/modules/uxrce_dds_client/dds_topics.h.em b/src/modules/uxrce_dds_client/dds_topics.h.em index 8cb5e244ba1..c7f1931302c 100644 --- a/src/modules/uxrce_dds_client/dds_topics.h.em +++ b/src/modules/uxrce_dds_client/dds_topics.h.em @@ -83,7 +83,7 @@ struct SendTopicsSubs { get_message_version<@(pub['simple_base_type'])_s>(), ucdr_topic_size_@(pub['simple_base_type'])(), &ucdr_serialize_@(pub['simple_base_type']), - static_cast((@(pub.get('rate_limit', 0)) > 0) ? (1e3 / @(pub.get('rate_limit', 1e3))) : UXRCE_DEFAULT_POLL_INTERVAL_MS), + @(pub['publish_interval_ms'] if pub['publish_interval_ms'] is not None else 'UXRCE_DEFAULT_POLL_INTERVAL_MS'), // ms; 0 = unlimited (rate limit disabled, queue drained) @(pub['instance']) }, @[ end for]@ @@ -135,38 +135,57 @@ void SendTopicsSubs::update(uxrSession *session, uxrStreamId reliable_out_stream for (unsigned idx = 0; idx < sizeof(send_subscriptions)/sizeof(send_subscriptions[0]); ++idx) { if (fds[idx].revents & POLLIN) { - // Topic updated, copy data and send - orb_copy(send_subscriptions[idx].orb_meta, fds[idx].fd, &topic_data); + // Topics with an unlimited rate (publish_interval_ms == 0) drain the whole + // uORB queue in one pass: the interval is already 0, so orb_check gates only + // on the message generation and every queued sample is forwarded, so bursts + // (e.g. CAN frames) are not dropped. The pass is bounded by the topic's queue + // depth so a publisher producing faster than we drain cannot spin this loop. + // Rate-limited topics forward only the latest sample per poll wakeup, + // respecting the configured interval. + const bool drain_queue = (send_subscriptions[idx].publish_interval_ms == 0); + unsigned remaining = drain_queue ? send_subscriptions[idx].orb_meta->o_queue : 1; + bool updated = true; - if (send_subscriptions[idx].data_writer.id != UXR_INVALID_ID) { + while (updated) { + // Topic updated, copy data and send + orb_copy(send_subscriptions[idx].orb_meta, fds[idx].fd, &topic_data); - ucdrBuffer ub; - uint32_t topic_size = send_subscriptions[idx].topic_size; - uint16_t req_id = uxr_prepare_output_stream(session, best_effort_stream_id, send_subscriptions[idx].data_writer, &ub, - topic_size); + if (send_subscriptions[idx].data_writer.id != UXR_INVALID_ID) { - if (req_id == UXR_INVALID_REQUEST_ID) { - // The best-effort output buffer can fill up if multiple topics update at once. - // Flush once to free space and retry. - uxr_flash_output_streams(session); - needs_flush = false; - req_id = uxr_prepare_output_stream(session, best_effort_stream_id, send_subscriptions[idx].data_writer, &ub, - topic_size); - } + ucdrBuffer ub; + uint32_t topic_size = send_subscriptions[idx].topic_size; + uint16_t req_id = uxr_prepare_output_stream(session, best_effort_stream_id, send_subscriptions[idx].data_writer, &ub, + topic_size); - if (req_id != UXR_INVALID_REQUEST_ID) { - send_subscriptions[idx].ucdr_serialize_method(&topic_data, ub, time_offset_us); - needs_flush = true; - num_payload_sent += topic_size; + if (req_id == UXR_INVALID_REQUEST_ID) { + // The best-effort output buffer can fill up if multiple topics update at once. + // Flush once to free space and retry. + uxr_flash_output_streams(session); + needs_flush = false; + req_id = uxr_prepare_output_stream(session, best_effort_stream_id, send_subscriptions[idx].data_writer, &ub, + topic_size); + } + + if (req_id != UXR_INVALID_REQUEST_ID) { + send_subscriptions[idx].ucdr_serialize_method(&topic_data, ub, time_offset_us); + needs_flush = true; + num_payload_sent += topic_size; + + } else { + //PX4_ERR("Error uxr_prepare_output_stream UXR_INVALID_REQUEST_ID %s", send_subscriptions[idx].subscription.get_topic()->o_name); + } } else { - //PX4_ERR("Error uxr_prepare_output_stream UXR_INVALID_REQUEST_ID %s", send_subscriptions[idx].subscription.get_topic()->o_name); + //PX4_ERR("Error UXR_INVALID_ID %s", send_subscriptions[idx].subscription.get_topic()->o_name); } - } else { - //PX4_ERR("Error UXR_INVALID_ID %s", send_subscriptions[idx].subscription.get_topic()->o_name); - } + if (--remaining == 0) { + // Latest sample only; leave the rest of the queue for the next cycle. + break; + } + orb_check(fds[idx].fd, &updated); + } } } diff --git a/src/modules/uxrce_dds_client/generate_dds_topics.py b/src/modules/uxrce_dds_client/generate_dds_topics.py index 64cf9575279..d40e785a95d 100644 --- a/src/modules/uxrce_dds_client/generate_dds_topics.py +++ b/src/modules/uxrce_dds_client/generate_dds_topics.py @@ -102,6 +102,26 @@ def process_message_type(msg_type): # topic_simple: eg vehicle_status msg_type['topic_simple'] = msg_type['topic'].split('/')[-1] + # publish_interval_ms: maps the optional rate_limit (Hz) to the orb_set_interval + # gate, which is expressed in integer milliseconds. An unlimited rate (0) also + # makes the bridge drain the whole uORB queue each poll wakeup instead of + # forwarding only the latest sample (see SendTopicsSubs::update). + # absent -> None: template uses UXRCE_DEFAULT_POLL_INTERVAL_MS + # 0 or 'unlimited'/'off' -> 0: rate limiting disabled, drain the whole queue + # N > 0 -> round(1000/N), clamped to a >=1ms floor (anything finer + # than 1ms cannot be expressed and must use 'unlimited') + rate = msg_type.get('rate_limit', None) + if rate is None: + msg_type['publish_interval_ms'] = None + elif (isinstance(rate, str) and rate.strip().lower() in ('unlimited', 'off', 'none')) \ + or (isinstance(rate, (int, float)) and rate <= 0): + msg_type['publish_interval_ms'] = 0 + elif isinstance(rate, (int, float)): + msg_type['publish_interval_ms'] = max(1, round(1000.0 / float(rate))) + else: + raise ValueError(f"invalid rate_limit {rate!r} for topic {msg_type['topic']}: " + "expected a positive number, 0, or 'unlimited'") + # Optional per-publisher QoS options from YAML 'options:' field. # Converts e.g. {cc: block, express: true} -> "cc=block,express=true" opts = msg_type.get('options', None)