Execute intra-process subscription callbacks in a separate thread rat…

…her than in the executor

rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp

@@ -90,6 +90,13 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase
       throw std::runtime_error("SubscriptionIntraProcess init error initializing guard condition");
     }
 
+    wait_set_ = rcl_get_zero_initialized_wait_set();
+    ret = rcl_wait_set_init(&wait_set_, 0, 1, 0, 0, 0, 0, context->get_rcl_context().get(), rcl_get_default_allocator());
+
+    if (RCL_RET_OK != ret) {
+      throw std::runtime_error("SubscriptionIntraProcess init error initializing wait set");
+    }
+
     TRACEPOINT(
       rclcpp_subscription_callback_added,
       (const void *)this,
@@ -134,10 +141,33 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase
     return buffer_->use_take_shared_method();
   }
 
+  void consume_messages_task() override
+  {
+    rcl_ret_t ret;
+    do {
+      ret = rcl_wait_set_clear(&wait_set_);
+      ret = rcl_wait_set_add_guard_condition(&wait_set_, &gc_, NULL);
+
+      // Wait until guard condition is triggered
+      ret = rcl_wait(&wait_set_, -1);
+
+      // If guard condition is triggered, check buffer for data
+      while (is_ready(&wait_set_)) {
+          // Process the message
+          execute();
+      }
+    } while (rclcpp::ok());
+
+    ret = rcl_wait_set_fini(&wait_set_);
+    (void)ret;
+  }
+
 private:
   void
   trigger_guard_condition()
   {
+    // Publisher pushed message into the buffer.
+    // Notify subscription context
     rcl_ret_t ret = rcl_trigger_guard_condition(&gc_);
     (void)ret;
   }

rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp

@@ -47,7 +47,7 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable
 
   RCLCPP_PUBLIC
   size_t
-  get_number_of_ready_guard_conditions() {return 1;}
+  get_number_of_ready_guard_conditions() {return 0;} // QUICK HACK to avoid allocating space in the executor
 
   RCLCPP_PUBLIC
   bool
@@ -70,9 +70,13 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable
   rmw_qos_profile_t
   get_actual_qos() const;
 
+  virtual void
+  consume_messages_task() = 0;
+
 protected:
   std::recursive_mutex reentrant_mutex_;
   rcl_guard_condition_t gc_;
+  rcl_wait_set_t wait_set_;
 
 private:
   virtual void

rclcpp/include/rclcpp/subscription_base.hpp

@@ -190,8 +190,8 @@ class SubscriptionBase : public std::enable_shared_from_this<SubscriptionBase>
 
   /// Return the waitable for intra-process, or nullptr if intra-process is not setup.
   RCLCPP_PUBLIC
-  rclcpp::Waitable::SharedPtr
-  get_intra_process_waitable() const;
+  rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr
+  get_intra_process_subscription() const;
 
 protected:
   template<typename EventCallbackT>

rclcpp/src/rclcpp/node_interfaces/node_topics.cpp

@@ -99,10 +99,20 @@ NodeTopics::add_subscription(
     callback_group->add_waitable(subscription_event);
   }
 
-  auto intra_process_waitable = subscription->get_intra_process_waitable();
-  if (nullptr != intra_process_waitable) {
-    // Add to the callback group to be notified about intra-process msgs.
-    callback_group->add_waitable(intra_process_waitable);
+  auto intra_process_subscription = subscription->get_intra_process_subscription();
+
+  if (nullptr != intra_process_subscription) {
+    using rclcpp::experimental::SubscriptionIntraProcessBase;
+
+    // TODO:
+    // - the thread should be explicitly created by the user
+    // - this should be part of the intra-process executor
+    //Start thread which executes subscription callback whenever an intra-process message is received
+    std::thread subscription_thread = std::thread(
+      &SubscriptionIntraProcessBase::consume_messages_task,
+      intra_process_subscription);
+
+    subscription_thread.detach();
   }
 
   // Notify the executor that a new subscription was created using the parent Node.

rclcpp/src/rclcpp/subscription_base.cpp

@@ -176,8 +176,8 @@ SubscriptionBase::can_loan_messages() const
   return rcl_subscription_can_loan_messages(subscription_handle_.get());
 }
 
-rclcpp::Waitable::SharedPtr
-SubscriptionBase::get_intra_process_waitable() const
+rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr
+SubscriptionBase::get_intra_process_subscription() const
 {
   // If not using intra process, shortcut to nullptr.
   if (!use_intra_process_) {
@@ -187,7 +187,7 @@ SubscriptionBase::get_intra_process_waitable() const
   auto ipm = weak_ipm_.lock();
   if (!ipm) {
     throw std::runtime_error(
-            "SubscriptionBase::get_intra_process_waitable() called "
+            "SubscriptionBase::get_intra_process_subscription() called "
             "after destruction of intra process manager");
   }
 

rclcpp/src/rclcpp/subscription_intra_process_base.cpp

@@ -19,10 +19,13 @@ using rclcpp::experimental::SubscriptionIntraProcessBase;
 bool
 SubscriptionIntraProcessBase::add_to_wait_set(rcl_wait_set_t * wait_set)
 {
-  std::lock_guard<std::recursive_mutex> lock(reentrant_mutex_);
+  // QUICK HACK: commented out to avoid adding this to the executor wait set
+  (void)wait_set;
+  //std::lock_guard<std::recursive_mutex> lock(reentrant_mutex_);
 
-  rcl_ret_t ret = rcl_wait_set_add_guard_condition(wait_set, &gc_, NULL);
-  return RCL_RET_OK == ret;
+  //rcl_ret_t ret = rcl_wait_set_add_guard_condition(wait_set, &gc_, NULL);
+  //return RCL_RET_OK == ret;
+  return true;
 }
 
 const char *