Fixed race condition between taking sample and updating counter. (#264)

Author: richiware

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();
     }
   }
 

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());