Adding logic to support original writer forwarding (#163)

* Adding logic to support original writer forwarding

Signed-off-by: Emilio Cuesta <emiliocuesta@eprosima.com>

* Uncrustify

Signed-off-by: Emilio Cuesta <emiliocuesta@eprosima.com>

* Applying review

Signed-off-by: Emilio Cuesta <emiliocuesta@eprosima.com>

* Refs 23573, adapting changes to new original writer info struct

Signed-off-by: Emilio Cuesta <emiliocuesta@eprosima.com>

---------

Signed-off-by: Emilio Cuesta <emiliocuesta@eprosima.com>

Author: emiliocuestaf

ddspipe_core/include/ddspipe_core/types/data/RtpsPayloadData.hpp

@@ -17,6 +17,7 @@
 
 #include <fastdds/rtps/common/SerializedPayload.hpp>
 #include <fastdds/rtps/common/SequenceNumber.hpp>
+#include <fastdds/rtps/common/OriginalWriterInfo.hpp>
 
 #include <ddspipe_core/library/library_dll.h>
 #include <ddspipe_core/types/dds/Payload.hpp>
@@ -85,6 +86,9 @@ struct RtpsPayloadData : public core::IRoutingData
 
     //! Id of the participant from which the Reader has received the data.
     core::types::ParticipantId participant_receiver{};
+
+    //! Guid of the original entity that transmitted the data for the first time
+    eprosima::fastdds::rtps::OriginalWriterInfo original_writer_info{};
 };
 
 /**

ddspipe_participants/src/cpp/reader/dds/CommonReader.cpp

@@ -302,6 +302,20 @@ void CommonReader::fill_received_data_(
     // Store the new data that has arrived in the Track data
     // Get the writer guid
     data_to_fill.source_guid = detail::guid_from_instance_handle(info.publication_handle);
+
+    auto original_source_guid = info.original_writer_info.original_writer_guid();
+    if (original_source_guid == fastdds::rtps::GUID_t::unknown())
+    {
+        // If the original source guid is unknown, this is the first time a router receives this data
+        // and the current source is the original
+        data_to_fill.original_writer_info.original_writer_guid(data_to_fill.source_guid);
+    }
+    else
+    {
+        // If the original source guid is known, set it
+        data_to_fill.original_writer_info = info.original_writer_info;
+    }
+
     // Get source timestamp
     data_to_fill.source_timestamp = info.source_timestamp;
     // Get Participant receiver

ddspipe_participants/src/cpp/reader/rtps/CommonReader.cpp

@@ -131,7 +131,8 @@ void CommonReader::internal_entities_creation_(
                       " for Simple RTPSReader in Participant " << participant_id_);
     }
 
-    EPROSIMA_LOG_INFO(DDSPIPE_RTPS_READER, "New CommonReader created in Participant " << participant_id_ << " for topic " <<
+    EPROSIMA_LOG_INFO(DDSPIPE_RTPS_READER,
+            "New CommonReader created in Participant " << participant_id_ << " for topic " <<
             topic_ << " with guid " << rtps_reader_->getGuid());
 }
 
@@ -205,6 +206,20 @@ void CommonReader::fill_received_data_(
     // Store the new data that has arrived in the Track data
     // Get the writer guid
     data_to_fill.source_guid = received_change.writerGUID;
+
+    auto original_source_guid = received_change.write_params.original_writer_info().original_writer_guid();
+    if (original_source_guid == fastdds::rtps::GUID_t::unknown())
+    {
+        // If the original source guid is unknown, this is the first time a router receives this data
+        // and the current source is the original
+        data_to_fill.original_writer_info.original_writer_guid(data_to_fill.source_guid);
+    }
+    else
+    {
+        // If the original source guid is known, set it
+        data_to_fill.original_writer_info = received_change.write_params.original_writer_info();
+    }
+
     // Get source timestamp
     data_to_fill.source_timestamp = received_change.sourceTimestamp;
     // Get Participant receiver
@@ -454,7 +469,8 @@ void CommonReader::on_reader_matched(
         if (info.status == fastdds::rtps::MatchingStatus::MATCHED_MATCHING)
         {
             EPROSIMA_LOG_INFO(DDSPIPE_RTPS_COMMONREADER_LISTENER,
-                    "Reader " << *this << " in topic " << topic_.serialize() << " matched with a new Writer with guid " <<
+                    "Reader " << *this << " in topic " << topic_.serialize() <<
+                    " matched with a new Writer with guid " <<
                     info.remoteEndpointGuid);
         }
         else

ddspipe_participants/src/cpp/writer/dds/CommonWriter.cpp

@@ -215,6 +215,9 @@ utils::ReturnCode CommonWriter::fill_to_send_data_(
     // Set source time stamp to be the original one
     to_send_params.source_timestamp(data.source_timestamp);
 
+    // Set original writer guid
+    to_send_params.original_writer_info(data.original_writer_info);
+
     return utils::ReturnCode::RETCODE_OK;
 }
 

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

@@ -251,6 +251,9 @@ utils::ReturnCode CommonWriter::fill_to_send_data_(
     // Set source time stamp to be the original one
     to_send_params.source_timestamp(data.source_timestamp);
 
+    // Set original writer guid
+    to_send_params.original_writer_info(data.original_writer_info);
+
     return utils::ReturnCode::RETCODE_OK;
 }