Improve ignore_local_publications logic

Signed-off-by: Jose Antonio Moral <joseantoniomoralparras@gmail.com>
ros2 · Jun 1, 2021 · b3096b9 · b3096b9
1 parent 4e7fd18
commit b3096b9
Showing 1 changed file with 19 additions and 14 deletions.
diff --git a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp
@@ -75,24 +75,29 @@ _take(
   data.is_cdr_buffer = false;
   data.data = ros_message;
   data.impl = info->type_support_impl_;
-  if (info->data_reader_->take_next_sample(&data, &sinfo) == ReturnCode_t::RETCODE_OK) {
-    if (subscription->options.ignore_local_publications) {
-      auto sample_writer_guid =
-        eprosima::fastrtps::rtps::iHandle2GUID(sinfo.publication_handle);
 
-      if (sample_writer_guid.guidPrefix == info->data_reader_->guid().guidPrefix) {
-        // This is a local publication. Ignore it
-        return RMW_RET_OK;
+  while (0 < info->data_reader_->get_unread_count()) {
+    if (info->data_reader_->take_next_sample(&data, &sinfo) == ReturnCode_t::RETCODE_OK) {
+      // Update hasData from listener
+      info->listener_->update_has_data(info->data_reader_);
+
+      if (subscription->options.ignore_local_publications) {
+        auto sample_writer_guid =
+          eprosima::fastrtps::rtps::iHandle2GUID(sinfo.publication_handle);
+
+        if (sample_writer_guid.guidPrefix == info->data_reader_->guid().guidPrefix) {
+          // This is a local publication. Ignore it
+          continue;
+        }
       }
-    }
-    // Update hasData from listener
-    info->listener_->update_has_data(info->data_reader_);
 
-    if (sinfo.valid_data) {
-      if (message_info) {
-        _assign_message_info(identifier, message_info, &sinfo);
+      if (sinfo.valid_data) {
+        if (message_info) {
+          _assign_message_info(identifier, message_info, &sinfo);
+        }
+        *taken = true;
+        break;
       }
-      *taken = true;
     }
   }