update listener logic for accurate counting (#262)

ros2 · Mar 15, 2019 · 94af1d2 · 94af1d2
1 parent ea4fb7c
commit 94af1d2
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 14 deletions.
diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp
@@ -47,8 +47,8 @@ typedef struct CustomClientInfo
   eprosima::fastrtps::Participant * participant_;
   const char * typesupport_identifier_;
   ClientPubListener * pub_listener_;
-  uint32_t response_subscriber_matched_count_;
-  uint32_t request_publisher_matched_count_;
+  std::atomic_uint32_t response_subscriber_matched_count_;
+  std::atomic_uint32_t request_publisher_matched_count_;
 } CustomClientInfo;
 
 typedef struct CustomClientResponse
@@ -152,15 +152,18 @@ class ClientListener : public eprosima::fastrtps::SubscriberListener
     eprosima::fastrtps::Subscriber * sub,
     eprosima::fastrtps::rtps::MatchingInfo & matchingInfo)
   {
-    if (info_ == nullptr || sub == nullptr) {
+    (void)sub;
+    if (info_ == nullptr) {
       return;
     }
-
-    if (matchingInfo.status == eprosima::fastrtps::rtps::MATCHED_MATCHING) {
-      info_->response_subscriber_matched_count_++;
+    if (eprosima::fastrtps::rtps::MATCHED_MATCHING == matchingInfo.status) {
+      publishers_.insert(matchingInfo.remoteEndpointGuid);
+    } else if (eprosima::fastrtps::rtps::REMOVED_MATCHING == matchingInfo.status) {
+      publishers_.erase(matchingInfo.remoteEndpointGuid);
     } else {
-      info_->response_subscriber_matched_count_--;
+      return;
     }
+    info_->response_subscriber_matched_count_.store(publishers_.size());
   }
 
 private:
@@ -170,6 +173,7 @@ class ClientListener : public eprosima::fastrtps::SubscriberListener
   std::atomic_bool list_has_data_;
   std::mutex * conditionMutex_;
   std::condition_variable * conditionVariable_;
+  std::set<eprosima::fastrtps::rtps::GUID_t> publishers_;
 };
 
 class ClientPubListener : public eprosima::fastrtps::PublisherListener
@@ -184,19 +188,23 @@ class ClientPubListener : public eprosima::fastrtps::PublisherListener
     eprosima::fastrtps::Publisher * pub,
     eprosima::fastrtps::rtps::MatchingInfo & matchingInfo)
   {
-    if (info_ == nullptr || pub == nullptr) {
+    (void) pub;
+    if (info_ == nullptr) {
       return;
     }
-
-    if (matchingInfo.status == eprosima::fastrtps::rtps::MATCHED_MATCHING) {
-      info_->request_publisher_matched_count_++;
+    if (eprosima::fastrtps::rtps::MATCHED_MATCHING == matchingInfo.status) {
+      subscriptions_.insert(matchingInfo.remoteEndpointGuid);
+    } else if (eprosima::fastrtps::rtps::REMOVED_MATCHING == matchingInfo.status) {
+      subscriptions_.erase(matchingInfo.remoteEndpointGuid);
     } else {
-      info_->request_publisher_matched_count_--;
+      return;
     }
+    info_->request_publisher_matched_count_.store(subscriptions_.size());
   }
 
 private:
   CustomClientInfo * info_;
+  std::set<eprosima::fastrtps::rtps::GUID_t> subscriptions_;
 };
 
 #endif  // RMW_FASTRTPS_SHARED_CPP__CUSTOM_CLIENT_INFO_HPP_
diff --git a/rmw_fastrtps_shared_cpp/src/rmw_service_server_is_available.cpp b/rmw_fastrtps_shared_cpp/src/rmw_service_server_is_available.cpp
@@ -104,11 +104,11 @@ __rmw_service_server_is_available(
     return RMW_RET_OK;
   }
 
-  if (0 == client_info->request_publisher_matched_count_) {
+  if (0 == client_info->request_publisher_matched_count_.load()) {
     // not ready
     return RMW_RET_OK;
   }
-  if (0 == client_info->response_subscriber_matched_count_) {
+  if (0 == client_info->response_subscriber_matched_count_.load()) {
     // not ready
     return RMW_RET_OK;
   }