From 6fcfd5fac186cf607a464c8dd3db068cb9d58b8c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 18 Jan 2024 13:12:28 +0100 Subject: [PATCH] uxrce_dds_client: immediately create data writers on startup There is some race condition where in rare cases the topic publication right after creating the writer did not get received on the ROS side. This happens even with reliable QoS & reliable transport. --- src/modules/uxrce_dds_client/dds_topics.h.em | 26 +++++++++++-------- .../uxrce_dds_client/uxrce_dds_client.cpp | 13 ++++------ .../uxrce_dds_client/uxrce_dds_client.h | 1 - 3 files changed, 20 insertions(+), 20 deletions(-) diff --git a/src/modules/uxrce_dds_client/dds_topics.h.em b/src/modules/uxrce_dds_client/dds_topics.h.em index 81eadef1bb..deba362668 100644 --- a/src/modules/uxrce_dds_client/dds_topics.h.em +++ b/src/modules/uxrce_dds_client/dds_topics.h.em @@ -87,17 +87,27 @@ struct SendTopicsSubs { uint32_t num_payload_sent{}; - void init(); + bool init(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId reliable_in_stream_id, uxrStreamId best_effort_in_stream_id, uxrObjectId participant_id, const char *client_namespace); void update(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId best_effort_stream_id, uxrObjectId participant_id, const char *client_namespace); void reset(); }; -void SendTopicsSubs::init() { +bool SendTopicsSubs::init(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId reliable_in_stream_id, uxrStreamId best_effort_in_stream_id, uxrObjectId participant_id, const char *client_namespace) { + bool ret = true; for (unsigned idx = 0; idx < sizeof(send_subscriptions)/sizeof(send_subscriptions[0]); ++idx) { - fds[idx].fd = orb_subscribe(send_subscriptions[idx].orb_meta); - fds[idx].events = POLLIN; - orb_set_interval(fds[idx].fd, UXRCE_DEFAULT_POLL_RATE); + if (fds[idx].events == 0) { + fds[idx].fd = orb_subscribe(send_subscriptions[idx].orb_meta); + fds[idx].events = POLLIN; + orb_set_interval(fds[idx].fd, UXRCE_DEFAULT_POLL_RATE); + } + + if (!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)) { + ret = false; + } } + return ret; } void SendTopicsSubs::reset() { @@ -119,12 +129,6 @@ void SendTopicsSubs::update(uxrSession *session, uxrStreamId reliable_out_stream if (fds[idx].revents & POLLIN) { // Topic updated, copy data and send 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].topic, - send_subscriptions[idx].message_version, - 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/uxrce_dds_client.cpp b/src/modules/uxrce_dds_client/uxrce_dds_client.cpp index b550912791..9cc9bd516b 100644 --- a/src/modules/uxrce_dds_client/uxrce_dds_client.cpp +++ b/src/modules/uxrce_dds_client/uxrce_dds_client.cpp @@ -366,6 +366,11 @@ bool UxrceddsClient::setupSession(uxrSession *session) return false; } + if (!_subs->init(session, _reliable_out, reliable_in, best_effort_in, _participant_id, _client_namespace)) { + PX4_ERR("subs init failed"); + return false; + } + // create VehicleCommand replier if (_num_of_repliers < MAX_NUM_REPLIERS) { if (add_replier(new VehicleCommandSrv(session, _reliable_out, reliable_in, _participant_id, _client_namespace, @@ -388,11 +393,6 @@ void UxrceddsClient::deleteSession(uxrSession *session) _session_created = false; } - if (_subs_initialized) { - _subs->reset(); - _subs_initialized = false; - } - _last_payload_tx_rate = 0; _timesync.reset_filter(); } @@ -650,9 +650,6 @@ void UxrceddsClient::run() int poll_error_counter = 0; resetConnectivityCounters(); - _subs->init(); - _subs_initialized = true; - while (!should_exit() && _connected) { perf_begin(_loop_perf); perf_count(_loop_interval_perf); diff --git a/src/modules/uxrce_dds_client/uxrce_dds_client.h b/src/modules/uxrce_dds_client/uxrce_dds_client.h index 823e242f9d..7b5208a80e 100644 --- a/src/modules/uxrce_dds_client/uxrce_dds_client.h +++ b/src/modules/uxrce_dds_client/uxrce_dds_client.h @@ -197,7 +197,6 @@ private: bool _connected{false}; bool _session_created{false}; bool _timesync_converged{false}; - bool _subs_initialized{false}; Timesync _timesync{timesync_status_s::SOURCE_PROTOCOL_DDS};