Alway read status from entity to reset status change.

Signed-off-by: Miguel Company <MiguelCompany@eprosima.com>
ros2 · Apr 10, 2023 · dbe3bdf · dbe3bdf
1 parent 6a9caae
commit dbe3bdf
Show file tree

Hide file tree

Showing 2 changed files with 21 additions and 24 deletions.
diff --git 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
@@ -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;
       }