shortcircuit events executor callback invocation in rclcpp for ipc subscriptions

Author: Alberto Soragna

rclcpp/include/rclcpp/executors/events_executor_notify_waitable.hpp

@@ -59,7 +59,7 @@ class EventsExecutorNotifyWaitable final : public EventWaitable
   void
   set_events_executor_callback(
     const rclcpp::executors::EventsExecutor * executor,
-    rmw_listener_cb_t executor_callback) const override
+    rmw_listener_cb_t executor_callback) override
   {
     for (auto gc : notify_guard_conditions_) {
       rcl_ret_t ret = rcl_guard_condition_set_listener_callback(

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

@@ -101,7 +101,14 @@ class RingBufferImplementation : public BufferImplementationBase<BufferT>
     return size_ == capacity_;
   }
 
-  void clear() {}
+  void clear()
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    ring_buffer_ = std::vector<BufferT>(capacity_);
+    write_index_ = capacity_ - 1;
+    read_index_ = 0;
+    size_ = 0;
+  }
 
 private:
   size_t capacity_;

rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp

@@ -118,14 +118,26 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase
   provide_intra_process_message(ConstMessageSharedPtr message)
   {
     buffer_->add_shared(std::move(message));
-    trigger_guard_condition();
+
+    std::lock_guard<std::mutex> lock(executor_callback_mutex_);
+    if (executor_callback_) {
+      invoke_executor_callback();
+    } else {
+      trigger_guard_condition();
+    }
   }
 
   void
   provide_intra_process_message(MessageUniquePtr message)
   {
     buffer_->add_unique(std::move(message));
-    trigger_guard_condition();
+
+    std::lock_guard<std::mutex> lock(executor_callback_mutex_);
+    if (executor_callback_) {
+      invoke_executor_callback();
+    } else {
+      trigger_guard_condition();
+    }
   }
 
   bool
@@ -134,6 +146,18 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase
     return buffer_->use_take_shared_method();
   }
 
+  void
+  set_events_executor_callback(
+    const rclcpp::executors::EventsExecutor * executor,
+    rmw_listener_cb_t executor_callback) override
+  {
+    std::lock_guard<std::mutex> lock(executor_callback_mutex_);
+    executor_ = executor;
+    executor_callback_ = executor_callback;
+    // Buffer must be cleared under the executor callback lock to make sure that other threads wait for this
+    buffer_->clear();
+  }
+
 private:
   void
   trigger_guard_condition()
@@ -142,6 +166,17 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase
     (void)ret;
   }
 
+  void
+  invoke_executor_callback()
+  {
+    static const rmw_listener_event_t this_event = {
+      .entity = this,
+      .type = WAITABLE_EVENT
+    };
+
+    executor_callback_(executor_, this_event);
+  }
+
   template<typename T>
   typename std::enable_if<std::is_same<T, rcl_serialized_message_t>::value, void>::type
   execute_impl()

rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp

@@ -74,12 +74,16 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable
   void
   set_events_executor_callback(
     const rclcpp::executors::EventsExecutor * executor,
-    rmw_listener_cb_t executor_callback) const override;
+    rmw_listener_cb_t executor_callback) override;
 
 protected:
   std::recursive_mutex reentrant_mutex_;
   rcl_guard_condition_t gc_;
 
+  const rclcpp::executors::EventsExecutor * executor_;
+  rmw_listener_cb_t executor_callback_;
+  std::mutex executor_callback_mutex_;
+
 private:
   virtual void
   trigger_guard_condition() = 0;

rclcpp/include/rclcpp/qos_event.hpp

@@ -107,7 +107,7 @@ class QOSEventHandlerBase : public Waitable
   void
   set_events_executor_callback(
     const rclcpp::executors::EventsExecutor * executor,
-    rmw_listener_cb_t executor_callback) const override;
+    rmw_listener_cb_t executor_callback) override;
 
 protected:
   rcl_event_t event_handle_;

rclcpp/include/rclcpp/waitable.hpp

@@ -175,7 +175,7 @@ class Waitable
   void
   set_events_executor_callback(
     const rclcpp::executors::EventsExecutor * executor,
-    rmw_listener_cb_t executor_callback) const;
+    rmw_listener_cb_t executor_callback);
 
 private:
   std::atomic<bool> in_use_by_wait_set_{false};

rclcpp/src/rclcpp/qos_event.cpp

@@ -71,7 +71,7 @@ QOSEventHandlerBase::is_ready(rcl_wait_set_t * wait_set)
 void
 QOSEventHandlerBase::set_events_executor_callback(
   const rclcpp::executors::EventsExecutor * executor,
-  rmw_listener_cb_t executor_callback) const
+  rmw_listener_cb_t executor_callback)
 {
   rcl_ret_t ret = rcl_event_set_listener_callback(
     &event_handle_,

rclcpp/src/rclcpp/subscription_intra_process_base.cpp

@@ -37,20 +37,12 @@ SubscriptionIntraProcessBase::get_actual_qos() const
   return qos_profile_;
 }
 
-
 void
 SubscriptionIntraProcessBase::set_events_executor_callback(
   const rclcpp::executors::EventsExecutor * executor,
-  rmw_listener_cb_t executor_callback) const
+  rmw_listener_cb_t executor_callback)
 {
-  rcl_ret_t ret = rcl_guard_condition_set_listener_callback(
-    &gc_,
-    executor_callback,
-    executor,
-    this,
-    true /*Use previous events*/);
-
-  if (RCL_RET_OK != ret) {
-    throw std::runtime_error("Couldn't set guard condition callback");
-  }
-}
+    (void)executor;
+    (void)executor_callback;
+    assert(0);
+}

rclcpp/src/rclcpp/waitable.cpp

@@ -61,7 +61,7 @@ Waitable::exchange_in_use_by_wait_set_state(bool in_use_state)
 void
 Waitable::set_events_executor_callback(
   const rclcpp::executors::EventsExecutor * executor,
-  rmw_listener_cb_t executor_callback) const
+  rmw_listener_cb_t executor_callback)
 {
   (void)executor;
   (void)executor_callback;