[#25513] Fixed flaky RPC reply loss by gating reply write on reader match

Signed-off-by: danipiza <dpizarrogallego@gmail.com>

Author: Danipiza

ddspipe_core/include/ddspipe_core/interface/IWriter.hpp

@@ -19,6 +19,7 @@
 #include <cpp_utils/ReturnCode.hpp>
 
 #include <ddspipe_core/interface/IRoutingData.hpp>
+#include <ddspipe_core/types/dds/GuidPrefix.hpp>
 
 namespace eprosima {
 namespace ddspipe {
@@ -94,6 +95,33 @@ class IWriter
     DDSPIPE_CORE_DllAPI
     virtual void update_topic_partitions(
             const std::map<std::string, std::string>& partition_name) = 0;
+
+    /**
+     * @brief Wait until this Writer has matched a Reader belonging to the participant identified by
+     * \c reader_participant_guid_prefix, or until \c timeout_ms milliseconds have elapsed.
+     *
+     * Needed for RPC reply routing: reply topics use VOLATILE durability, so a reply written before
+     * the destination client's reply reader is matched on the writer side is silently dropped. As
+     * ROS 2 service clients never retry, that hangs the client forever. Gating the reply write on
+     * this method closes the discovery race (a client's \c wait_for_service only proves the match
+     * from the reader side).
+     *
+     * @param reader_participant_guid_prefix  GuidPrefix of the participant whose reader must match.
+     * @param timeout_ms                      Maximum time to wait, in milliseconds.
+     *
+     * @return \c true if a matching reader is present within the timeout, \c false otherwise.
+     *
+     * @note Default implementation returns \c true: writers that do not track matching are assumed
+     *       ready and never block the caller.
+     */
+    virtual bool wait_reader_matched(
+            const types::GuidPrefix& reader_participant_guid_prefix,
+            const unsigned int timeout_ms) const noexcept
+    {
+        static_cast<void>(reader_participant_guid_prefix);
+        static_cast<void>(timeout_ms);
+        return true;
+    }
 };
 
 } /* namespace core */

ddspipe_core/src/cpp/communication/rpc/RpcBridge.cpp

@@ -33,6 +33,10 @@ namespace core {
 
 using namespace eprosima::ddspipe::core::types;
 
+//! Maximum time (ms) to wait for the destination reply reader to be matched before writing a reply.
+//! Only reached during the discovery race window; the wait returns immediately once matched.
+static constexpr unsigned int REPLY_MATCH_MAX_WAIT_MS = 1000;
+
 RpcBridge::RpcBridge(
         const RpcTopic& topic,
         const std::shared_ptr<ParticipantsDatabase>& participants_database,
@@ -405,6 +409,21 @@ void RpcBridge::transmit_(
                     rpc_data.write_params.set_level();
                     rpc_data.write_params.get_reference().related_sample_identity(registry_entry.second);
 
+                    // Reply topics use VOLATILE durability: a reply written before the destination
+                    // client's reply reader has been matched on this writer's side is silently
+                    // dropped. As ROS 2 service clients never retry a request, that would hang the
+                    // client forever. The client's own \c wait_for_service only guarantees the match
+                    // from the reader side, so wait (bounded) for the writer side to match as well
+                    // before writing, closing the discovery race.
+                    if (!reply_writers_[registry_entry.first]->wait_reader_matched(
+                                registry_entry.second.writer_guid().guidPrefix, REPLY_MATCH_MAX_WAIT_MS))
+                    {
+                        EPROSIMA_LOG_WARNING(DDSPIPE_RPCBRIDGE,
+                                "RpcBridge for service " << rpc_topic_ << " writing reply towards participant "
+                                << registry_entry.second.writer_guid().guidPrefix <<
+                                " whose reply reader is not matched yet; reply may be lost.");
+                    }
+
                     ret = reply_writers_[registry_entry.first]->write(*data);
 
                     if (ret != utils::ReturnCode::RETCODE_OK)

ddspipe_participants/include/ddspipe_participants/writer/rtps/CommonWriter.hpp

@@ -15,6 +15,9 @@
 #pragma once
 
 #include <atomic>
+#include <condition_variable>
+#include <mutex>
+#include <set>
 
 #include <cpp_utils/pool/IPool.hpp>
 #include <cpp_utils/ReturnCode.hpp>
@@ -140,6 +143,16 @@ class CommonWriter : public BaseWriter, public fastdds::rtps::WriterListener
             fastdds::rtps::RTPSWriter*,
             eprosima::fastdds::dds::PolicyMask qos) noexcept override;
 
+    /**
+     * @brief Wait (bounded) until a Reader belonging to \c reader_participant_guid_prefix is matched.
+     *
+     * @note Used by RPC bridges to avoid dropping VOLATILE replies on the writer-side discovery race.
+     */
+    DDSPIPE_PARTICIPANTS_DllAPI
+    bool wait_reader_matched(
+            const core::types::GuidPrefix& reader_participant_guid_prefix,
+            const unsigned int timeout_ms) const noexcept override;
+
     /////////////////////
     // STATIC ATTRIBUTES
     /////////////////////
@@ -313,6 +326,15 @@ class CommonWriter : public BaseWriter, public fastdds::rtps::WriterListener
 
     //! Pool Configuration to create the internal History.
     utils::PoolConfiguration pool_configuration_;
+
+    //! GUIDs of the Readers (external to this Participant) currently matched with this Writer
+    std::set<fastdds::rtps::GUID_t> matched_readers_;
+
+    //! Protects \c matched_readers_ and coordinates \c wait_reader_matched
+    mutable std::mutex matched_readers_mutex_;
+
+    //! Notified whenever \c matched_readers_ changes so \c wait_reader_matched can wake up
+    mutable std::condition_variable matched_readers_cv_;
 };
 
 } /* namespace rtps */

ddspipe_participants/src/cpp/writer/rtps/CommonWriter.cpp

@@ -116,6 +116,19 @@ void CommonWriter::on_writer_matched(
 {
     if (!come_from_this_participant_(info.remoteEndpointGuid))
     {
+        {
+            std::lock_guard<std::mutex> lock(matched_readers_mutex_);
+            if (info.status == fastdds::rtps::MatchingStatus::MATCHED_MATCHING)
+            {
+                matched_readers_.insert(info.remoteEndpointGuid);
+            }
+            else
+            {
+                matched_readers_.erase(info.remoteEndpointGuid);
+            }
+        }
+        matched_readers_cv_.notify_all();
+
         if (info.status == fastdds::rtps::MatchingStatus::MATCHED_MATCHING)
         {
             EPROSIMA_LOG_INFO(DDSPIPE_RTPS_COMMONWRITER_LISTENER,
@@ -131,6 +144,28 @@ void CommonWriter::on_writer_matched(
     }
 }
 
+bool CommonWriter::wait_reader_matched(
+        const core::types::GuidPrefix& reader_participant_guid_prefix,
+        const unsigned int timeout_ms) const noexcept
+{
+    std::unique_lock<std::mutex> lock(matched_readers_mutex_);
+
+    auto reader_from_participant_matched = [this, &reader_participant_guid_prefix]() -> bool
+            {
+                for (const auto& reader_guid : matched_readers_)
+                {
+                    if (reader_guid.guidPrefix == reader_participant_guid_prefix)
+                    {
+                        return true;
+                    }
+                }
+                return false;
+            };
+
+    return matched_readers_cv_.wait_for(
+        lock, std::chrono::milliseconds(timeout_ms), reader_from_participant_matched);
+}
+
 void CommonWriter::on_writer_change_received_by_all(
         fastdds::rtps::RTPSWriter* /*writer*/,
         fastdds::rtps::CacheChange_t* change)