[backport foxy] Fix trying to get topic data that was already removed (…

…#421)

* Fix trying to get topic data that was already removed. (#417)

* Fix trying to get topic data that was already removed.

Signed-off-by: Chen.Lihui <lihui.chen@sony.com>

* Revert "Fix trying to get topic data that was already removed."

This reverts commit 08632e8.

Signed-off-by: Chen.Lihui <lihui.chen@sony.com>

* Updated based on suggestions

Signed-off-by: Chen.Lihui <lihui.chen@sony.com>
Signed-off-by: Miguel Company <MiguelCompany@eprosima.com>
Signed-off-by: Dirk Thomas <dirk-thomas@users.noreply.github.com>

* revert rename of function

Signed-off-by: Dirk Thomas <dirk-thomas@users.noreply.github.com>

Co-authored-by: Chen Lihui <lihui.chen@sony.com>

rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp

@@ -67,34 +67,23 @@ class SubListener : public EventListenerInterface, public eprosima::fastrtps::Su
   // SubscriberListener implementation
   void
   onSubscriptionMatched(
-    eprosima::fastrtps::Subscriber * /*sub*/, eprosima::fastrtps::rtps::MatchingInfo & info) final
+    eprosima::fastrtps::Subscriber * sub, eprosima::fastrtps::rtps::MatchingInfo & info) final
   {
-    std::lock_guard<std::mutex> lock(internalMutex_);
-    if (eprosima::fastrtps::rtps::MATCHED_MATCHING == info.status) {
-      publishers_.insert(info.remoteEndpointGuid);
-    } else if (eprosima::fastrtps::rtps::REMOVED_MATCHING == info.status) {
-      publishers_.erase(info.remoteEndpointGuid);
+    {
+      std::lock_guard<std::mutex> lock(internalMutex_);
+      if (eprosima::fastrtps::rtps::MATCHED_MATCHING == info.status) {
+        publishers_.insert(info.remoteEndpointGuid);
+      } else if (eprosima::fastrtps::rtps::REMOVED_MATCHING == info.status) {
+        publishers_.erase(info.remoteEndpointGuid);
+      }
     }
+    data_taken(sub);
   }
 
   void
   onNewDataMessage(eprosima::fastrtps::Subscriber * sub) final
   {
-    // Make sure to call into Fast-RTPS before taking the lock to avoid an
-    // ABBA deadlock between internalMutex_ and mutexes inside of Fast-RTPS.
-#if FASTRTPS_VERSION_MAJOR == 1 && FASTRTPS_VERSION_MINOR < 9
-    uint64_t unread_count = sub->getUnreadCount();
-#else
-    uint64_t unread_count = sub->get_unread_count();
-#endif
-
-    std::lock_guard<std::mutex> lock(internalMutex_);
-
-    // the change to liveliness_lost_count_ needs to be mutually exclusive with
-    // rmw_wait() which checks hasEvent() and decides if wait() needs to be called
-    ConditionalScopedLock clock(conditionMutex_, conditionVariable_);
-
-    data_.store(unread_count, std::memory_order_relaxed);
+    data_taken(sub);
   }
 
   RMW_FASTRTPS_SHARED_CPP_PUBLIC
@@ -153,7 +142,7 @@ class SubListener : public EventListenerInterface, public eprosima::fastrtps::Su
 #endif
 
     std::lock_guard<std::mutex> lock(internalMutex_);
-    ConditionalScopedLock clock(conditionMutex_);
+    ConditionalScopedLock clock(conditionMutex_, conditionVariable_);
     data_.store(unread_count, std::memory_order_relaxed);
   }