Fix error when SubscriptionIntraProcess is being executed more than once by the MultiThreadedExecutor

rclcpp/include/rclcpp/experimental/buffers/buffer_implementation_base.hpp

@@ -35,6 +35,15 @@ class BufferImplementationBase
   virtual bool has_data() const = 0;
 };
 
+/// Thrown when trying to dequeue data from an emty buffer
+class BufferEmptyError : public std::runtime_error
+{
+public:
+  BufferEmptyError()
+  : std::runtime_error("trying to dequeue from an empty buffer") {}
+};
+
+
 }  // namespace buffers
 }  // namespace experimental
 }  // namespace rclcpp

rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp

@@ -84,14 +84,14 @@ class RingBufferImplementation : public BufferImplementationBase<BufferT>
    * This member function is thread-safe.
    *
    * \return the element that is being removed from the ring buffer
+   * \throw BufferEmptyError if the buffer is empty
    */
   BufferT dequeue()
   {
     std::lock_guard<std::mutex> lock(mutex_);
 
     if (!has_data_()) {
-      RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Calling dequeue on empty intra-process buffer");
-      throw std::runtime_error("Calling dequeue on empty intra-process buffer");
+      throw BufferEmptyError();
     }
 
     auto request = std::move(ring_buffer_[read_index_]);

rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp

@@ -25,6 +25,7 @@
 #include "rcl/error_handling.h"
 
 #include "rclcpp/any_subscription_callback.hpp"
+#include "rclcpp/experimental/buffers/buffer_implementation_base.hpp"
 #include "rclcpp/experimental/buffers/intra_process_buffer.hpp"
 #include "rclcpp/experimental/create_intra_process_buffer.hpp"
 #include "rclcpp/experimental/subscription_intra_process_base.hpp"
@@ -159,10 +160,20 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase
 
     if (any_callback_.use_take_shared_method()) {
       ConstMessageSharedPtr msg = buffer_->consume_shared();
-      any_callback_.dispatch_intra_process(msg, msg_info);
+      try {
+        any_callback_.dispatch_intra_process(msg, msg_info);
+      } catch (buffers::BufferEmptyError & e) {
+        // Ignore this error.
+        // The multithreaded executor might have scheduled this waitable for execution more than
+        // once.
+      }
     } else {
       MessageUniquePtr msg = buffer_->consume_unique();
-      any_callback_.dispatch_intra_process(std::move(msg), msg_info);
+      try {
+        any_callback_.dispatch_intra_process(std::move(msg), msg_info);
+      } catch (buffers::BufferEmptyError & e) {
+        // ignore
+      }
     }
   }