From 2d3e58d23f703ff7466c1f6c74b5483a972faa63 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Ricardo=20Gonz=C3=A1lez=20Moreno?= <correoricky@gmail.com>
Date: Thu, 7 Mar 2019 16:17:47 +0100
Subject: [PATCH] Fixed race condition between taking sample and updating
 counter.

---
 .../rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp      | 6 +++---
 rmw_fastrtps_shared_cpp/src/rmw_take.cpp                    | 4 ++--
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp
index f3938a146..22fcca141 100644
--- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp
+++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp
@@ -102,15 +102,15 @@ class SubListener : public eprosima::fastrtps::SubscriberListener
   }
 
   void
-  data_taken()
+  data_taken(eprosima::fastrtps::Subscriber * sub)
   {
     std::lock_guard<std::mutex> lock(internalMutex_);
 
     if (conditionMutex_ != nullptr) {
       std::unique_lock<std::mutex> clock(*conditionMutex_);
-      --data_;
+      data_ = sub->getUnreadCount();
     } else {
-      --data_;
+      data_ = sub->getUnreadCount();
     }
   }
 
diff --git a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp
index 8621e239b..873816cfc 100644
--- a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp
+++ b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp
@@ -67,7 +67,7 @@ _take(
   data.is_cdr_buffer = false;
   data.data = ros_message;
   if (info->subscriber_->takeNextData(&data, &sinfo)) {
-    info->listener_->data_taken();
+    info->listener_->data_taken(info->subscriber_);
 
     if (eprosima::fastrtps::rtps::ALIVE == sinfo.sampleKind) {
       if (message_info) {
@@ -140,7 +140,7 @@ _take_serialized_message(
   data.is_cdr_buffer = true;
   data.data = &buffer;
   if (info->subscriber_->takeNextData(&data, &sinfo)) {
-    info->listener_->data_taken();
+    info->listener_->data_taken(info->subscriber_);
 
     if (eprosima::fastrtps::rtps::ALIVE == sinfo.sampleKind) {
       auto buffer_size = static_cast<size_t>(buffer.getBufferSize());