allow to set ipc sub callback from regular subscription

Signed-off-by: Alberto Soragna <[email protected]>

Author: Alberto Soragna

rclcpp/include/rclcpp/subscription.hpp

@@ -343,11 +343,6 @@ class Subscription : public SubscriptionBase
     message_memory_strategy_;
   /// Component which computes and publishes topic statistics for this subscriber
   SubscriptionTopicStatisticsSharedPtr subscription_topic_statistics_{nullptr};
-  using SubscriptionIntraProcessT = rclcpp::experimental::SubscriptionIntraProcess<
-    CallbackMessageT,
-    AllocatorT,
-    typename MessageUniquePtr::deleter_type>;
-  std::shared_ptr<SubscriptionIntraProcessT> subscription_intra_process_;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/subscription_base.hpp

@@ -346,6 +346,55 @@ class SubscriptionBase : public std::enable_shared_from_this<SubscriptionBase>
     on_new_message_callback_ = nullptr;
   }
 
+  /// Set a callback to be called when each new intra-process message is received.
+  /**
+   * The callback receives a size_t which is the number of messages received
+   * since the last time this callback was called.
+   * Normally this is 1, but can be > 1 if messages were received before any
+   * callback was set.
+   *
+   * Calling it again will clear any previously set callback.
+   *
+   * This function is thread-safe.
+   *
+   * If you want more information available in the callback, like the subscription
+   * or other information, you may use a lambda with captures or std::bind.
+   *
+   * \sa rclcpp::SubscriptionIntraProcessBase::set_on_ready_callback
+   *
+   * \param[in] callback functor to be called when a new message is received
+   */
+  void
+  set_on_new_intra_process_message_callback(std::function<void(size_t)> callback)
+  {
+    if (!use_intra_process_) {
+      RCLCPP_WARN(
+        rclcpp::get_logger("rclcpp"),
+        "Calling set_on_new_intra_process_message_callback for subscription with IPC disabled");
+      return;
+    }
+
+    // The on_ready_callback signature has an extra `int` argument used to disambiguate between 
+    // possible different entities within a generic waitable.
+    // We hide that detail to users of this method.
+    std::function<void(size_t, int)> new_callback = std::bind(callback, std::placeholders::_1);
+    subscription_intra_process_->set_on_ready_callback(new_callback);
+  }
+
+  /// Unset the callback registered for new intra-process messages, if any.
+  void
+  clear_on_new_intra_process_message_callback()
+  {
+    if (!use_intra_process_) {
+      RCLCPP_WARN(
+        rclcpp::get_logger("rclcpp"),
+        "Calling clear_on_new_intra_process_message_callback for subscription with IPC disabled");
+      return;
+    }
+
+    subscription_intra_process_->clear_on_ready_callback();
+  }
+
 protected:
   template<typename EventCallbackT>
   void
@@ -386,6 +435,7 @@ class SubscriptionBase : public std::enable_shared_from_this<SubscriptionBase>
   bool use_intra_process_;
   IntraProcessManagerWeakPtr weak_ipm_;
   uint64_t intra_process_subscription_id_;
+  std::shared_ptr<SubscriptionIntraProcessBase> subscription_intra_process_;
 
 private:
   RCLCPP_DISABLE_COPY(SubscriptionBase)

rclcpp/test/rclcpp/test_subscription.cpp

@@ -444,7 +444,7 @@ TEST_F(TestSubscription, handle_loaned_message) {
    Testing on_new_message callbacks.
  */
 TEST_F(TestSubscription, on_new_message_callback) {
-  initialize();
+  initialize(rclcpp::NodeOptions().use_intra_process_comms(false));
   using test_msgs::msg::Empty;
 
   auto do_nothing = [](std::shared_ptr<const test_msgs::msg::Empty>) {FAIL();};
@@ -510,6 +510,76 @@ TEST_F(TestSubscription, on_new_message_callback) {
   EXPECT_THROW(sub->set_on_new_message_callback(invalid_cb), std::invalid_argument);
 }
 
+/*
+   Testing on_new_intra_process_message callbacks.
+ */
+TEST_F(TestSubscription, on_new_intra_process_message_callback) {
+  initialize(rclcpp::NodeOptions().use_intra_process_comms(true));
+  using test_msgs::msg::Empty;
+
+  auto do_nothing = [](std::shared_ptr<const test_msgs::msg::Empty>) {FAIL();};
+  auto sub = node->create_subscription<test_msgs::msg::Empty>("~/test_take", 1, do_nothing);
+
+  std::atomic<size_t> c1 {0};
+  auto increase_c1_cb = [&c1](size_t count_msgs) {c1 += count_msgs;};
+  sub->set_on_new_intra_process_message_callback(increase_c1_cb);
+
+  auto pub = node->create_publisher<test_msgs::msg::Empty>("~/test_take", 1);
+  {
+    test_msgs::msg::Empty msg;
+    pub->publish(msg);
+  }
+
+  auto start = std::chrono::steady_clock::now();
+  do {
+    std::this_thread::sleep_for(100ms);
+  } while (c1 == 0 && std::chrono::steady_clock::now() - start < 10s);
+
+  EXPECT_EQ(c1.load(), 1u);
+
+  std::atomic<size_t> c2 {0};
+  auto increase_c2_cb = [&c2](size_t count_msgs) {c2 += count_msgs;};
+  sub->set_on_new_intra_process_message_callback(increase_c2_cb);
+
+  {
+    test_msgs::msg::Empty msg;
+    pub->publish(msg);
+  }
+
+  start = std::chrono::steady_clock::now();
+  do {
+    std::this_thread::sleep_for(100ms);
+  } while (c2 == 0 && std::chrono::steady_clock::now() - start < 10s);
+
+  EXPECT_EQ(c1.load(), 1u);
+  EXPECT_EQ(c2.load(), 1u);
+
+  sub->clear_on_new_intra_process_message_callback();
+
+  {
+    test_msgs::msg::Empty msg;
+    pub->publish(msg);
+    pub->publish(msg);
+    pub->publish(msg);
+  }
+
+  std::atomic<size_t> c3 {0};
+  auto increase_c3_cb = [&c3](size_t count_msgs) {c3 += count_msgs;};
+  sub->set_on_new_intra_process_message_callback(increase_c3_cb);
+
+  start = std::chrono::steady_clock::now();
+  do {
+    std::this_thread::sleep_for(100ms);
+  } while (c3 == 0 && std::chrono::steady_clock::now() - start < 10s);
+
+  EXPECT_EQ(c1.load(), 1u);
+  EXPECT_EQ(c2.load(), 1u);
+  EXPECT_EQ(c3.load(), 3u);
+
+  std::function<void(size_t)> invalid_cb = nullptr;
+  EXPECT_THROW(sub->set_on_new_intra_process_message_callback(invalid_cb), std::invalid_argument);
+}
+
 /*
    Testing subscription with intraprocess enabled and invalid QoS
  */