From dbe3bdf4407cfd635cdd5195f125d76349c4eb34 Mon Sep 17 00:00:00 2001
From: Miguel Company <MiguelCompany@eprosima.com>
Date: Mon, 10 Apr 2023 07:28:11 +0200
Subject: [PATCH] Alway read status from entity to reset status change.

Signed-off-by: Miguel Company <MiguelCompany@eprosima.com>
---
 .../src/custom_publisher_info.cpp             | 22 ++++++++----------
 .../src/custom_subscriber_info.cpp            | 23 +++++++++----------
 2 files changed, 21 insertions(+), 24 deletions(-)

diff --git a/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp
index 499f264c1..ba41fe9b0 100644
--- a/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp
+++ b/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp
@@ -171,20 +171,18 @@ bool RMWPublisherEvent::take_event(
       {
         auto rmw_data = static_cast<rmw_matched_status_t *>(event_info);
 
+        eprosima::fastdds::dds::PublicationMatchedStatus matched_status;
+        publisher_info_->data_writer_->get_publication_matched_status(matched_status);
+
+        rmw_data->total_count = static_cast<size_t>(matched_status.total_count);
+        rmw_data->current_count = static_cast<size_t>(matched_status.current_count);
+        rmw_data->total_count_change = static_cast<size_t>(matched_status.total_count_change);
+        rmw_data->current_count_change = matched_status.current_count_change;
+
         if (matched_changes_) {
-          rmw_data->total_count = static_cast<size_t>(matched_status_.total_count);
-          rmw_data->total_count_change = static_cast<size_t>(matched_status_.total_count_change);
-          rmw_data->current_count = static_cast<size_t>(matched_status_.current_count);
-          rmw_data->current_count_change = matched_status_.current_count_change;
+          rmw_data->total_count_change += static_cast<size_t>(matched_status_.total_count_change);
+          rmw_data->current_count_change += matched_status_.current_count_change;
           matched_changes_ = false;
-        } else {
-          eprosima::fastdds::dds::PublicationMatchedStatus matched_status;
-          publisher_info_->data_writer_->get_publication_matched_status(matched_status);
-
-          rmw_data->total_count = static_cast<size_t>(matched_status.total_count);
-          rmw_data->total_count_change = static_cast<size_t>(matched_status.total_count_change);
-          rmw_data->current_count = static_cast<size_t>(matched_status.current_count);
-          rmw_data->current_count_change = matched_status.current_count_change;
         }
 
         matched_status_.total_count_change = 0;
diff --git a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp
index 0ff7ace6c..5eb367d02 100644
--- a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp
+++ b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp
@@ -206,21 +206,20 @@ bool RMWSubscriptionEvent::take_event(
       {
         auto rmw_data = static_cast<rmw_matched_status_t *>(event_info);
 
+        eprosima::fastdds::dds::SubscriptionMatchedStatus matched_status;
+        subscriber_info_->data_reader_->get_subscription_matched_status(matched_status);
+
+        rmw_data->total_count = static_cast<size_t>(matched_status.total_count);
+        rmw_data->total_count_change = static_cast<size_t>(matched_status.total_count_change);
+        rmw_data->current_count = static_cast<size_t>(matched_status.current_count);
+        rmw_data->current_count_change = matched_status.current_count_change;
+
         if (matched_changes_) {
-          rmw_data->total_count = static_cast<size_t>(matched_status_.total_count);
-          rmw_data->total_count_change = static_cast<size_t>(matched_status_.total_count_change);
-          rmw_data->current_count = static_cast<size_t>(matched_status_.current_count);
-          rmw_data->current_count_change = matched_status_.current_count_change;
+          rmw_data->total_count_change += static_cast<size_t>(matched_status_.total_count_change);
+          rmw_data->current_count_change += matched_status_.current_count_change;
           matched_changes_ = false;
-        } else {
-          eprosima::fastdds::dds::SubscriptionMatchedStatus matched_status;
-          subscriber_info_->data_reader_->get_subscription_matched_status(matched_status);
-
-          rmw_data->total_count = static_cast<size_t>(matched_status.total_count);
-          rmw_data->total_count_change = static_cast<size_t>(matched_status.total_count_change);
-          rmw_data->current_count = static_cast<size_t>(matched_status.current_count);
-          rmw_data->current_count_change = matched_status.current_count_change;
         }
+
         matched_status_.total_count_change = 0;
         matched_status_.current_count_change = 0;
       }