Fix related_sample_identity handling in RPC entities (#5760)

* Refs #23048. Fix includes in ReplierImpl.

Signed-off-by: Miguel Company <[email protected]>

* Refs #23048. Factor-out code to fill `related_sample_identity`.

Signed-off-by: Miguel Company <[email protected]>

* Refs #23048. Only update `related_sample_identity` when necessary.

Signed-off-by: Miguel Company <[email protected]>

* Refs #23048. Doxygen.

Signed-off-by: Miguel Company <[email protected]>

* Refs #23048. Similar changes in RequesterImpl.

Signed-off-by: Miguel Company <[email protected]>

* Refs #23048. Add Blackbox tests.

Signed-off-by: Miguel Company <[email protected]>

* Refs #23048. Uncrustify.

Signed-off-by: Miguel Company <[email protected]>

* Refs #23048. Fix includes in `RequesterImpl`.

Signed-off-by: Miguel Company <[email protected]>

---------

Signed-off-by: Miguel Company <[email protected]>

Author: MiguelCompany

src/cpp/fastdds/rpc/ReplierImpl.cpp

@@ -12,18 +12,60 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include "ReplierImpl.hpp"
+
+#include <string>
+
+#include <fastdds/dds/core/detail/DDSReturnCode.hpp>
+#include <fastdds/dds/core/LoanableCollection.hpp>
+#include <fastdds/dds/core/LoanableSequence.hpp>
 #include <fastdds/dds/core/status/StatusMask.hpp>
+#include <fastdds/dds/domain/qos/ReplierQos.hpp>
 #include <fastdds/dds/log/Log.hpp>
+#include <fastdds/dds/rpc/RequestInfo.hpp>
+#include <fastdds/rtps/common/SampleIdentity.hpp>
 #include <fastdds/rtps/common/WriteParams.hpp>
 
-#include "ReplierImpl.hpp"
 #include "ServiceImpl.hpp"
 
 namespace eprosima {
 namespace fastdds {
 namespace dds {
 namespace rpc {
 
+/**
+ * @brief Fills the related sample identity of the request.
+ *
+ * This will fill the related sample identity of the request with values taken from the sample identity.
+ * Values different from unknown are preserved.
+ *
+ * @param info [in,out] The request information to update.
+ */
+static void fill_related_sample_identity(
+        RequestInfo& info)
+{
+    // When sending a reply, the code here expects that related_sample_identity
+    // has the sample_identity of the corresponding request.
+
+    static const rtps::SampleIdentity unknown_identity = rtps::SampleIdentity::unknown();
+
+    // If the related guid is unknown, we consider that the request is not related to a previous one,
+    // so we set the related sample identity to the received sample identity
+    if (unknown_identity.writer_guid() == info.related_sample_identity.writer_guid())
+    {
+        info.related_sample_identity = info.sample_identity;
+        return;
+    }
+
+    // There is a special case where only the related guid is set.
+    // This is used in ROS 2 to convey the GUID of the reply reader.
+    // In this case we just set the sequence number of the related sample identity
+    if (unknown_identity.sequence_number() == info.related_sample_identity.sequence_number())
+    {
+        info.related_sample_identity.sequence_number() = info.sample_identity.sequence_number();
+    }
+}
+
 ReplierImpl::ReplierImpl(
         ServiceImpl* service,
         const ReplierQos& qos)
@@ -79,8 +121,7 @@ ReturnCode_t ReplierImpl::take_request(
     }
 
     retcode = replier_reader_->take_next_sample(data, &info);
-    // Related sample identity is stored in sample_indentity member of info. Change it to related_sample_identity
-    info.related_sample_identity = info.sample_identity;
+    fill_related_sample_identity(info);
 
     return retcode;
 }
@@ -104,7 +145,7 @@ ReturnCode_t ReplierImpl::take_request(
     // Fill related_sample_identity attribute
     for (LoanableCollection::size_type i = 0; i < info.length(); ++i)
     {
-        info[i].related_sample_identity = info[i].sample_identity;
+        fill_related_sample_identity(info[i]);
     }
 
     return retcode;

src/cpp/fastdds/rpc/RequesterImpl.cpp

@@ -12,11 +12,21 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include "RequesterImpl.hpp"
+
+#include <string>
+
+#include <fastdds/dds/core/detail/DDSReturnCode.hpp>
+#include <fastdds/dds/core/LoanableCollection.hpp>
+#include <fastdds/dds/core/LoanableSequence.hpp>
 #include <fastdds/dds/core/status/StatusMask.hpp>
+#include <fastdds/dds/domain/qos/RequesterQos.hpp>
 #include <fastdds/dds/log/Log.hpp>
+#include <fastdds/dds/rpc/RequestInfo.hpp>
+#include <fastdds/rtps/common/Guid.hpp>
+#include <fastdds/rtps/common/SequenceNumber.hpp>
 #include <fastdds/rtps/common/WriteParams.hpp>
 
-#include "RequesterImpl.hpp"
 #include "ServiceImpl.hpp"
 
 namespace eprosima {
@@ -59,11 +69,24 @@ ReturnCode_t RequesterImpl::send_request(
     }
 
     rtps::WriteParams wparams;
+    wparams.related_sample_identity(info.related_sample_identity);
     ReturnCode_t ret = requester_writer_->write(data, wparams);
     if (RETCODE_OK == ret)
     {
         // Fill RequestInfo's related sample identity with the information expected for the corresponding reply
-        info.related_sample_identity = wparams.related_sample_identity();
+        info.sample_identity = wparams.sample_identity();
+
+        // Set the full related sample identity when the writer guid is unknown
+        if (rtps::GUID_t::unknown() == info.related_sample_identity.writer_guid())
+        {
+            info.related_sample_identity = wparams.related_sample_identity();
+        }
+
+        // Set the sequence number of the related sample identity when it is unknown
+        if (rtps::SequenceNumber_t::unknown() == info.related_sample_identity.sequence_number())
+        {
+            info.related_sample_identity.sequence_number() = wparams.related_sample_identity().sequence_number();
+        }
     }
     return ret;
 }

test/blackbox/api/dds-pim/ReqRepHelloWorldRequester.cpp

@@ -189,6 +189,37 @@ void ReqRepHelloWorldRequester::send(
     ASSERT_NE(related_sample_identity_.sequence_number(), SequenceNumber_t());
 }
 
+void ReqRepHelloWorldRequester::send(
+        const uint16_t number,
+        const eprosima::fastdds::rtps::SampleIdentity& related_sample_identity)
+{
+    RequestInfo info;
+    info.related_sample_identity = related_sample_identity;
+    HelloWorld hello;
+    hello.index(number);
+    hello.message("HelloWorld");
+
+    {
+        std::unique_lock<std::mutex> lock(mutex_);
+        current_number_ = number;
+    }
+
+    ASSERT_EQ(requester_->send_request((void*)&hello, info), RETCODE_OK);
+    related_sample_identity_ = info.related_sample_identity;
+
+    ASSERT_NE(related_sample_identity_.sequence_number(), SequenceNumber_t());
+
+    if (eprosima::fastdds::rtps::GUID_t::unknown() != related_sample_identity.writer_guid())
+    {
+        ASSERT_EQ(related_sample_identity_.writer_guid(), related_sample_identity.writer_guid());
+    }
+
+    if (eprosima::fastdds::rtps::SequenceNumber_t::unknown() != related_sample_identity.sequence_number())
+    {
+        ASSERT_EQ(related_sample_identity_.sequence_number(), related_sample_identity.sequence_number());
+    }
+}
+
 const Duration_t ReqRepHelloWorldRequester::datawriter_latency_budget_duration() const
 {
     return requester_->get_requester_writer()->get_qos().latency_budget().duration;
@@ -199,6 +230,16 @@ const Duration_t ReqRepHelloWorldRequester::datareader_latency_budget_duration()
     return requester_->get_requester_reader()->get_qos().latency_budget().duration;
 }
 
+const eprosima::fastdds::rtps::GUID_t& ReqRepHelloWorldRequester::get_reader_guid() const
+{
+    return requester_->get_requester_reader()->guid();
+}
+
+const eprosima::fastdds::rtps::SampleIdentity& ReqRepHelloWorldRequester::get_last_related_sample_identity() const
+{
+    return related_sample_identity_;
+}
+
 void ReqRepHelloWorldRequester::init_processing_thread()
 {
     wait_set_.attach_condition(stop_processing_thread_);

test/blackbox/api/dds-pim/ReqRepHelloWorldRequester.hpp

@@ -32,6 +32,7 @@
 #include <fastdds/dds/rpc/Requester.hpp>
 #include <fastdds/dds/rpc/RequestInfo.hpp>
 #include <fastdds/dds/rpc/Service.hpp>
+#include <fastdds/rtps/common/SampleIdentity.hpp>
 
 #include "../../common/BlackboxTests.hpp"
 
@@ -80,13 +81,21 @@ class ReqRepHelloWorldRequester
     void send(
             const uint16_t number);
 
+    void send(
+            const uint16_t number,
+            const eprosima::fastdds::rtps::SampleIdentity& related_sample_identity);
+
     const eprosima::fastdds::dds::Duration_t datawriter_latency_budget_duration() const;
 
     const eprosima::fastdds::dds::Duration_t datareader_latency_budget_duration() const;
 
     eprosima::fastdds::dds::RequesterQos create_requester_qos(
             bool volatile_durability_qos = false);
 
+    const eprosima::fastdds::rtps::GUID_t& get_reader_guid() const;
+
+    const eprosima::fastdds::rtps::SampleIdentity& get_last_related_sample_identity() const;
+
 private:
 
     ReqRepHelloWorldRequester& operator =(

test/blackbox/common/BlackboxTestsPubSubBasic.cpp

@@ -284,6 +284,62 @@ TEST_P(PubSubBasic, ReqRepAsReliableHelloworld)
     }
 }
 
+TEST_P(PubSubBasic, ReqRepAsReliableHelloworldReaderGUID)
+{
+    ReqRepHelloWorldRequester requester;
+    ReqRepHelloWorldReplier replier;
+    const uint16_t nmsgs = 10;
+
+    requester.init();
+
+    ASSERT_TRUE(requester.isInitialized());
+
+    replier.init();
+
+    requester.wait_discovery();
+    replier.wait_discovery();
+
+    ASSERT_TRUE(replier.isInitialized());
+
+    for (uint16_t count = 0; count < nmsgs; ++count)
+    {
+        eprosima::fastdds::rtps::SampleIdentity related_sample_identity{};
+        related_sample_identity.writer_guid(requester.get_reader_guid());
+        requester.send(count, related_sample_identity);
+        requester.block(std::chrono::seconds(5));
+    }
+}
+
+TEST_P(PubSubBasic, ReqRepAsReliableHelloworldConsecutive)
+{
+    ReqRepHelloWorldRequester requester;
+    ReqRepHelloWorldReplier replier;
+    const uint16_t nmsgs = 10;
+
+    requester.init();
+
+    ASSERT_TRUE(requester.isInitialized());
+
+    replier.init();
+
+    requester.wait_discovery();
+    replier.wait_discovery();
+
+    ASSERT_TRUE(replier.isInitialized());
+
+    requester.send(0);
+    requester.block(std::chrono::seconds(5));
+
+    eprosima::fastdds::rtps::SampleIdentity related_sample_identity{};
+    related_sample_identity = requester.get_last_related_sample_identity();
+
+    for (uint16_t count = 1; count < nmsgs; ++count)
+    {
+        requester.send(count, related_sample_identity);
+        requester.block(std::chrono::seconds(5));
+    }
+}
+
 TEST_P(PubSubBasic, PubSubAsReliableData64kb)
 {
     PubSubReader<Data64kbPubSubType> reader(TEST_TOPIC_NAME);