Fix assertion on OutputTrafficManager (#5704) (#5711)

* Fix assertion on `OutputTrafficManager` (#5704)

* Refs #22930. Add entry when missing on `set_statistics_message_data`.

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

* Refs #22930. Create sender resources on stateless readers.

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

* Refs #22930. Similar changes on writers.

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

* Refs #22930: Test TCP with best effort reader

Signed-off-by: cferreiragonz <[email protected]>

* Refs #22930: Uncrustify

Signed-off-by: cferreiragonz <[email protected]>

---------

Signed-off-by: Miguel Company <[email protected]>
Signed-off-by: cferreiragonz <[email protected]>
Co-authored-by: cferreiragonz <[email protected]>
(cherry picked from commit 777a71b)

# Conflicts:
#	src/cpp/rtps/reader/StatelessReader.cpp
#	src/cpp/rtps/writer/StatelessWriter.cpp

* Fix conflicts

---------

Co-authored-by: Miguel Company <[email protected]>

Author: mergify[bot]

src/cpp/rtps/participant/RTPSParticipantImpl.cpp

@@ -1991,6 +1991,28 @@ void RTPSParticipantImpl::createSenderResources(
     m_network_Factory.build_send_resources(send_resource_list_, locator_selector_entry);
 }
 
+void RTPSParticipantImpl::createSenderResources(
+        const RemoteLocatorList& locator_list,
+        const EndpointAttributes& param)
+{
+    using network::external_locators::filter_remote_locators;
+
+    LocatorSelectorEntry entry(locator_list.unicast.size(), locator_list.multicast.size());
+    entry.multicast = locator_list.multicast;
+    entry.unicast = locator_list.unicast;
+    filter_remote_locators(entry, param.external_unicast_locators, param.ignore_non_matching_locators);
+
+    std::lock_guard<std::timed_mutex> lock(m_send_resources_mutex_);
+    for (const Locator_t& locator : entry.unicast)
+    {
+        m_network_Factory.build_send_resources(send_resource_list_, locator);
+    }
+    for (const Locator_t& locator : entry.multicast)
+    {
+        m_network_Factory.build_send_resources(send_resource_list_, locator);
+    }
+}
+
 bool RTPSParticipantImpl::deleteUserEndpoint(
         const GUID_t& endpoint)
 {

src/cpp/rtps/participant/RTPSParticipantImpl.hpp

@@ -115,6 +115,7 @@ namespace rtps {
 
 struct PublicationBuiltinTopicData;
 struct TopicDescription;
+struct RemoteLocatorList;
 class RTPSParticipant;
 class RTPSParticipantListener;
 class BuiltinProtocols;
@@ -1045,6 +1046,10 @@ class RTPSParticipantImpl
     void createSenderResources(
             const Locator_t& locator);
 
+    void createSenderResources(
+            const RemoteLocatorList& locator_list,
+            const EndpointAttributes& param);
+
     /**
      * Creates sender resources for the given locator selector entry by calling the NetworkFactory's
      * build_send_resources method.

src/cpp/rtps/reader/StatelessReader.cpp

@@ -107,6 +107,9 @@ bool StatelessReader::matched_writer_add_edp(
         std::unique_lock<RecursiveTimedMutex> guard(mp_mutex);
         listener = listener_;
 
+        bool is_same_process = RTPSDomainImpl::should_intraprocess_between(m_guid, wdata.guid());
+        bool is_datasharing = is_datasharing_compatible_with(wdata);
+
         for (RemoteWriterInfo_t& writer : matched_writers_)
         {
             if (writer.guid == wdata.guid())
@@ -121,6 +124,11 @@ bool StatelessReader::matched_writer_add_edp(
                 }
                 writer.ownership_strength = wdata.m_qos.m_ownershipStrength.value;
 
+                if (!is_same_process && !is_datasharing)
+                {
+                    mp_RTPSParticipant->createSenderResources(wdata.remote_locators(), m_att);
+                }
+
                 if (nullptr != listener)
                 {
                     // call the listener without the lock taken
@@ -144,9 +152,6 @@ bool StatelessReader::matched_writer_add_edp(
             }
         }
 
-        bool is_same_process = RTPSDomainImpl::should_intraprocess_between(m_guid, wdata.guid());
-        bool is_datasharing = is_datasharing_compatible_with(wdata);
-
         RemoteWriterInfo_t info;
         info.guid = wdata.guid();
         info.persistence_guid = wdata.persistence_guid();
@@ -195,6 +200,11 @@ bool StatelessReader::matched_writer_add_edp(
             // this has to be done after the writer is added to the matched_writers or the processing may fail
             datasharing_listener_->notify(false);
         }
+
+        if (!is_same_process && !is_datasharing)
+        {
+            mp_RTPSParticipant->createSenderResources(wdata.remote_locators(), m_att);
+        }
     }
 
     if (liveliness_lease_duration_ < dds::c_TimeInfinite)

src/cpp/rtps/writer/StatefulWriter.cpp

@@ -983,6 +983,7 @@ bool StatefulWriter::matched_reader_add_edp(
                         m_att.external_unicast_locators, m_att.ignore_non_matching_locators);
                         filter_remote_locators(*reader->async_locator_selector_entry(),
                         m_att.external_unicast_locators, m_att.ignore_non_matching_locators);
+                        mp_RTPSParticipant->createSenderResources(rdata.remote_locators(), m_att);
                         update_reader_info(locator_selector_general_, true);
                         update_reader_info(locator_selector_async_, true);
                     }
@@ -1069,6 +1070,7 @@ bool StatefulWriter::matched_reader_add_edp(
         }
     }
 
+    mp_RTPSParticipant->createSenderResources(rdata.remote_locators(), m_att);
     update_reader_info(locator_selector_general_, true);
     update_reader_info(locator_selector_async_, true);
 

src/cpp/rtps/writer/StatelessWriter.cpp

@@ -455,6 +455,7 @@ bool StatelessWriter::matched_reader_add_edp(
                     {
                         filter_remote_locators(*reader.general_locator_selector_entry(),
                         m_att.external_unicast_locators, m_att.ignore_non_matching_locators);
+                        mp_RTPSParticipant->createSenderResources(data.remote_locators(), m_att);
                         update_reader_info(true);
                     }
                     return true;
@@ -543,14 +544,7 @@ bool StatelessWriter::matched_reader_add_edp(
     }
 
     // Create sender resources for the case when we send to a single reader
-    locator_selector_.locator_selector.reset(false);
-    locator_selector_.locator_selector.enable(data.guid());
-    mp_RTPSParticipant->network_factory().select_locators(locator_selector_.locator_selector);
-    RTPSParticipantImpl* part = mp_RTPSParticipant;
-    locator_selector_.locator_selector.for_each([part](const Locator_t& loc)
-            {
-                part->createSenderResources(loc);
-            });
+    mp_RTPSParticipant->createSenderResources(data.remote_locators(), m_att);
 
     // Create sender resources for the case when we send to all readers
     update_reader_info(true);

src/cpp/statistics/rtps/messages/OutputTrafficManager.hpp

@@ -25,6 +25,7 @@
 #include <utility>
 
 #include <fastdds/config.hpp>
+#include <fastdds/dds/log/Log.hpp>
 #include <fastdds/rtps/common/Locator.hpp>
 
 #include <statistics/rtps/messages/RTPSStatisticsMessages.hpp>
@@ -83,7 +84,12 @@ class OutputTrafficManager
                     return locator == entry.first;
                 };
         auto it = std::find_if(collection_.begin(), collection_.end(), search);
-        assert(it != collection_.end());
+        if (it == collection_.end())
+        {
+            EPROSIMA_LOG_ERROR(RTPS_OUT,
+                    "Locator '" << locator << "' not found in collection. Adding entry.");
+            it = collection_.insert(it, entry_type(locator, value_type{}));
+        }
         set_statistics_submessage_from_transport(locator, send_buffer, total_bytes, it->second);
 #endif // FASTDDS_STATISTICS
     }

test/blackbox/common/BlackboxTestsTransportTCP.cpp

@@ -1506,6 +1506,57 @@ TEST_P(TransportTCP, tcp_unique_network_flows_communication)
     EXPECT_TRUE(writer.waitForAllAcked(std::chrono::seconds(30)));
 }
 
+/**
+ * This verifies that a best effort reader is capable of creating resources when a new locator
+ * is received along a Data(W) in order to start communication. This will ensure the creation a new connect channel.
+ * The reader must have the lowest listening port to force the participant to create the channel.
+ */
+TEST_P(TransportTCP, best_effort_reader_tcp_resources_creation)
+{
+    PubSubWriter<HelloWorldPubSubType> writer(TEST_TOPIC_NAME);
+    PubSubReader<HelloWorldPubSubType> reader(TEST_TOPIC_NAME);
+
+    // Large data setup is reused to enable UDP for multicast and TCP for data.
+    // However, the metatraffic unicast needs to be replaced for UDP to ensure that the TCP
+    // locator is not announced in the Data(P) (In large data the metatraffic unicast is TCP).
+    LocatorList metatraffic_unicast;
+    eprosima::fastdds::rtps::Locator_t udp_locator;
+    udp_locator.kind = LOCATOR_KIND_UDPv4;
+    eprosima::fastdds::rtps::IPLocator::setIPv4(udp_locator, "127.0.0.1");
+    metatraffic_unicast.push_back(udp_locator);
+
+    // Writer with highest listening port will wait for connection
+    writer.setup_large_data_tcp(use_ipv6, global_port + 1)
+            .metatraffic_unicast_locator_list(metatraffic_unicast)
+            .init();
+
+    // Reader with lowest listening port to force the connection channel creation
+    reader.setup_large_data_tcp(use_ipv6, global_port)
+            .reliability(eprosima::fastdds::dds::ReliabilityQosPolicyKind::BEST_EFFORT_RELIABILITY_QOS)
+            .metatraffic_unicast_locator_list(metatraffic_unicast)
+            .init();
+
+    ASSERT_TRUE(writer.isInitialized());
+    ASSERT_TRUE(reader.isInitialized());
+
+    writer.wait_discovery(std::chrono::seconds(5));
+    reader.wait_discovery(std::chrono::seconds(5));
+
+    ASSERT_EQ(writer.get_matched(), 1u);
+    ASSERT_EQ(reader.get_matched(), 1u);
+
+    // Although participants have matched, the TCP connection might not be established yet.
+    // This active wait ensures the connection had time to be established before sending non-reliable samples.
+    std::this_thread::sleep_for(std::chrono::seconds(3));
+
+    auto data = default_helloworld_data_generator();
+    reader.startReception(data);
+    writer.send(data);
+    ASSERT_TRUE(data.empty());
+
+    reader.block_for_all();
+}
+
 #ifdef INSTANTIATE_TEST_SUITE_P
 #define GTEST_INSTANTIATE_TEST_MACRO(x, y, z, w) INSTANTIATE_TEST_SUITE_P(x, y, z, w)
 #else