expose qos listener APIs from pub and sub; add unit-tests

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

Author: alsora

rclcpp/include/rclcpp/publisher_base.hpp

@@ -23,6 +23,8 @@
 #include <memory>
 #include <sstream>
 #include <string>
+#include <unordered_map>
+#include <utility>
 #include <vector>
 
 #include "rcl/publisher.h"
@@ -109,9 +111,10 @@ class PublisherBase : public std::enable_shared_from_this<PublisherBase>
   get_publisher_handle() const;
 
   /// Get all the QoS event handlers associated with this publisher.
-  /** \return The vector of QoS event handlers. */
+  /** \return The map of QoS event handlers. */
   RCLCPP_PUBLIC
-  const std::vector<std::shared_ptr<rclcpp::QOSEventHandlerBase>> &
+  const
+  std::unordered_map<rcl_publisher_event_type_t, std::shared_ptr<rclcpp::QOSEventHandlerBase>> &
   get_event_handlers() const;
 
   /// Get subscription count
@@ -193,6 +196,71 @@ class PublisherBase : public std::enable_shared_from_this<PublisherBase>
     uint64_t intra_process_publisher_id,
     IntraProcessManagerSharedPtr ipm);
 
+  /// Set a callback to be called when each new qos event instance occurs.
+  /**
+   * The callback receives a size_t which is the number of events that occurred
+   * since the last time this callback was called.
+   * Normally this is 1, but can be > 1 if events occurred before any
+   * callback was set.
+   *
+   * Since this callback is called from the middleware, you should aim to make
+   * it fast and not blocking.
+   * If you need to do a lot of work or wait for some other event, you should
+   * spin it off to another thread, otherwise you risk blocking the middleware.
+   *
+   * Calling it again will clear any previously set callback.
+   *
+   * An exception will be thrown if the callback is not callable.
+   *
+   * This function is thread-safe.
+   *
+   * If you want more information available in the callback, like the qos event
+   * or other information, you may use a lambda with captures or std::bind.
+   *
+   * \sa rclcpp::QOSEventHandlerBase::set_on_ready_callback
+   *
+   * \param[in] callback functor to be called when a new event occurs
+   * \param[in] event_type identifier for the qos event we want to attach the callback to
+   */
+  void
+  set_on_new_qos_event_callback(
+    std::function<void(size_t)> callback,
+    rcl_publisher_event_type_t event_type)
+  {
+    if (event_handlers_.count(event_type) == 0) {
+      RCLCPP_WARN(
+        rclcpp::get_logger("rclcpp"),
+        "Calling set_on_new_qos_event_callback for non registered event_type");
+      return;
+    }
+
+    if (!callback) {
+      throw std::invalid_argument(
+              "The callback passed to set_on_new_qos_event_callback "
+              "is not callable.");
+    }
+
+    // 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);
+    event_handlers_[event_type]->set_on_ready_callback(new_callback);
+  }
+
+  /// Unset the callback registered for new qos events, if any.
+  void
+  clear_on_new_qos_event_callback(rcl_publisher_event_type_t event_type)
+  {
+    if (event_handlers_.count(event_type) == 0) {
+      RCLCPP_WARN(
+        rclcpp::get_logger("rclcpp"),
+        "Calling clear_on_new_qos_event_callback for non registered event_type");
+      return;
+    }
+
+    event_handlers_[event_type]->clear_on_ready_callback();
+  }
+
 protected:
   template<typename EventCallbackT>
   void
@@ -206,7 +274,7 @@ class PublisherBase : public std::enable_shared_from_this<PublisherBase>
       rcl_publisher_event_init,
       publisher_handle_,
       event_type);
-    event_handlers_.emplace_back(handler);
+    event_handlers_.insert(std::make_pair(event_type, handler));
   }
 
   RCLCPP_PUBLIC
@@ -216,7 +284,8 @@ class PublisherBase : public std::enable_shared_from_this<PublisherBase>
 
   std::shared_ptr<rcl_publisher_t> publisher_handle_;
 
-  std::vector<std::shared_ptr<rclcpp::QOSEventHandlerBase>> event_handlers_;
+  std::unordered_map<rcl_publisher_event_type_t,
+    std::shared_ptr<rclcpp::QOSEventHandlerBase>> event_handlers_;
 
   using IntraProcessManagerWeakPtr =
     std::weak_ptr<rclcpp::experimental::IntraProcessManager>;

rclcpp/include/rclcpp/subscription_base.hpp

@@ -98,9 +98,10 @@ class SubscriptionBase : public std::enable_shared_from_this<SubscriptionBase>
   get_subscription_handle() const;
 
   /// Get all the QoS event handlers associated with this subscription.
-  /** \return The vector of QoS event handlers. */
+  /** \return The map of QoS event handlers. */
   RCLCPP_PUBLIC
-  const std::vector<std::shared_ptr<rclcpp::QOSEventHandlerBase>> &
+  const
+  std::unordered_map<rcl_subscription_event_type_t, std::shared_ptr<rclcpp::QOSEventHandlerBase>> &
   get_event_handlers() const;
 
   /// Get the actual QoS settings, after the defaults have been determined.
@@ -401,6 +402,71 @@ class SubscriptionBase : public std::enable_shared_from_this<SubscriptionBase>
     subscription_intra_process_->clear_on_ready_callback();
   }
 
+  /// Set a callback to be called when each new qos event instance occurs.
+  /**
+   * The callback receives a size_t which is the number of events that occurred
+   * since the last time this callback was called.
+   * Normally this is 1, but can be > 1 if events occurred before any
+   * callback was set.
+   *
+   * Since this callback is called from the middleware, you should aim to make
+   * it fast and not blocking.
+   * If you need to do a lot of work or wait for some other event, you should
+   * spin it off to another thread, otherwise you risk blocking the middleware.
+   *
+   * Calling it again will clear any previously set callback.
+   *
+   * An exception will be thrown if the callback is not callable.
+   *
+   * This function is thread-safe.
+   *
+   * If you want more information available in the callback, like the qos event
+   * or other information, you may use a lambda with captures or std::bind.
+   *
+   * \sa rclcpp::QOSEventHandlerBase::set_on_ready_callback
+   *
+   * \param[in] callback functor to be called when a new event occurs
+   * \param[in] event_type identifier for the qos event we want to attach the callback to
+   */
+  void
+  set_on_new_qos_event_callback(
+    std::function<void(size_t)> callback,
+    rcl_subscription_event_type_t event_type)
+  {
+    if (event_handlers_.count(event_type) == 0) {
+      RCLCPP_WARN(
+        rclcpp::get_logger("rclcpp"),
+        "Calling set_on_new_qos_event_callback for non registered event_type");
+      return;
+    }
+
+    if (!callback) {
+      throw std::invalid_argument(
+              "The callback passed to set_on_new_qos_event_callback "
+              "is not callable.");
+    }
+
+    // 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);
+    event_handlers_[event_type]->set_on_ready_callback(new_callback);
+  }
+
+  /// Unset the callback registered for new qos events, if any.
+  void
+  clear_on_new_qos_event_callback(rcl_subscription_event_type_t event_type)
+  {
+    if (event_handlers_.count(event_type) == 0) {
+      RCLCPP_WARN(
+        rclcpp::get_logger("rclcpp"),
+        "Calling clear_on_new_qos_event_callback for non registered event_type");
+      return;
+    }
+
+    event_handlers_[event_type]->clear_on_ready_callback();
+  }
+
 protected:
   template<typename EventCallbackT>
   void
@@ -415,7 +481,7 @@ class SubscriptionBase : public std::enable_shared_from_this<SubscriptionBase>
       get_subscription_handle(),
       event_type);
     qos_events_in_use_by_wait_set_.insert(std::make_pair(handler.get(), false));
-    event_handlers_.emplace_back(handler);
+    event_handlers_.insert(std::make_pair(event_type, handler));
   }
 
   RCLCPP_PUBLIC
@@ -436,7 +502,8 @@ class SubscriptionBase : public std::enable_shared_from_this<SubscriptionBase>
   std::shared_ptr<rcl_subscription_t> intra_process_subscription_handle_;
   rclcpp::Logger node_logger_;
 
-  std::vector<std::shared_ptr<rclcpp::QOSEventHandlerBase>> event_handlers_;
+  std::unordered_map<rcl_subscription_event_type_t,
+    std::shared_ptr<rclcpp::QOSEventHandlerBase>> event_handlers_;
 
   bool use_intra_process_;
   IntraProcessManagerWeakPtr weak_ipm_;

rclcpp/include/rclcpp/wait_set_template.hpp

@@ -155,7 +155,8 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli
           this->storage_add_subscription(std::move(local_subscription));
         }
         if (mask.include_events) {
-          for (auto event : inner_subscription->get_event_handlers()) {
+          for (auto key_event_pair : inner_subscription->get_event_handlers()) {
+            auto event = key_event_pair.second;
             auto local_subscription = inner_subscription;
             bool already_in_use =
             local_subscription->exchange_in_use_by_wait_set_state(event.get(), true);
@@ -225,7 +226,8 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli
           this->storage_remove_subscription(std::move(local_subscription));
         }
         if (mask.include_events) {
-          for (auto event : inner_subscription->get_event_handlers()) {
+          for (auto key_event_pair : inner_subscription->get_event_handlers()) {
+            auto event = key_event_pair.second;
             auto local_subscription = inner_subscription;
             local_subscription->exchange_in_use_by_wait_set_state(event.get(), false);
             this->storage_remove_waitable(std::move(event));

rclcpp/src/rclcpp/node_interfaces/node_topics.cpp

@@ -55,7 +55,8 @@ NodeTopics::add_publisher(
     callback_group = node_base_->get_default_callback_group();
   }
 
-  for (auto & publisher_event : publisher->get_event_handlers()) {
+  for (auto & key_event_pair : publisher->get_event_handlers()) {
+    auto publisher_event = key_event_pair.second;
     callback_group->add_waitable(publisher_event);
   }
 
@@ -97,7 +98,8 @@ NodeTopics::add_subscription(
 
   callback_group->add_subscription(subscription);
 
-  for (auto & subscription_event : subscription->get_event_handlers()) {
+  for (auto & key_event_pair : subscription->get_event_handlers()) {
+    auto subscription_event = key_event_pair.second;
     callback_group->add_waitable(subscription_event);
   }
 

rclcpp/src/rclcpp/publisher_base.cpp

@@ -22,6 +22,7 @@
 #include <mutex>
 #include <sstream>
 #include <string>
+#include <unordered_map>
 #include <vector>
 
 #include "rcutils/logging_macros.h"
@@ -153,7 +154,8 @@ PublisherBase::get_publisher_handle() const
   return publisher_handle_;
 }
 
-const std::vector<std::shared_ptr<rclcpp::QOSEventHandlerBase>> &
+const
+std::unordered_map<rcl_publisher_event_type_t, std::shared_ptr<rclcpp::QOSEventHandlerBase>> &
 PublisherBase::get_event_handlers() const
 {
   return event_handlers_;

rclcpp/src/rclcpp/subscription_base.cpp

@@ -17,6 +17,7 @@
 #include <cstdio>
 #include <memory>
 #include <string>
+#include <unordered_map>
 #include <vector>
 
 #include "rclcpp/exceptions.hpp"
@@ -116,7 +117,8 @@ SubscriptionBase::get_subscription_handle() const
   return subscription_handle_;
 }
 
-const std::vector<std::shared_ptr<rclcpp::QOSEventHandlerBase>> &
+const
+std::unordered_map<rcl_subscription_event_type_t, std::shared_ptr<rclcpp::QOSEventHandlerBase>> &
 SubscriptionBase::get_event_handlers() const
 {
   return event_handlers_;
@@ -282,7 +284,8 @@ SubscriptionBase::exchange_in_use_by_wait_set_state(
   if (get_intra_process_waitable().get() == pointer_to_subscription_part) {
     return intra_process_subscription_waitable_in_use_by_wait_set_.exchange(in_use_state);
   }
-  for (const auto & qos_event : event_handlers_) {
+  for (const auto & key_event_pair : event_handlers_) {
+    auto qos_event = key_event_pair.second;
     if (qos_event.get() == pointer_to_subscription_part) {
       return qos_events_in_use_by_wait_set_[qos_event.get()].exchange(in_use_state);
     }

rclcpp/test/rclcpp/test_publisher.cpp

@@ -501,7 +501,8 @@ TEST_F(TestPublisher, run_event_handlers) {
   initialize();
   auto publisher = node->create_publisher<test_msgs::msg::Empty>("topic", 10);
 
-  for (const auto & handler : publisher->get_event_handlers()) {
+  for (const auto & key_event_pair : publisher->get_event_handlers()) {
+    auto handler = key_event_pair.second;
     std::shared_ptr<void> data = handler->take_data();
     handler->execute(data);
   }

rclcpp/test/rclcpp/test_qos_event.cpp

@@ -27,6 +27,8 @@
 
 #include "../mocking_utils/patch.hpp"
 
+using namespace std::chrono_literals;
+
 class TestQosEvent : public ::testing::Test
 {
 protected:
@@ -325,3 +327,67 @@ TEST_F(TestQosEvent, add_to_wait_set) {
     EXPECT_THROW(handler.add_to_wait_set(&wait_set), rclcpp::exceptions::RCLError);
   }
 }
+
+TEST_F(TestQosEvent, test_on_new_event_callback)
+{
+  rclcpp::QoS qos_profile_publisher(10);
+  qos_profile_publisher.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE);
+  auto publisher = node->create_publisher<test_msgs::msg::Empty>(
+    topic_name, qos_profile_publisher);
+
+  std::atomic<size_t> c1 {0};
+  auto increase_c1_cb = [&c1](size_t count_events) {c1 += count_events;};
+  publisher->set_on_new_qos_event_callback(increase_c1_cb, RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS);
+
+  rclcpp::QoS qos_profile_subscription(10);
+  qos_profile_subscription.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
+  auto subscription = node->create_subscription<test_msgs::msg::Empty>(
+    topic_name, qos_profile_subscription, message_callback);
+
+  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, 1u);
+
+  std::atomic<size_t> c2 {0};
+  auto increase_c2_cb = [&c2](size_t count_events) {c2 += count_events;};
+  subscription->set_on_new_qos_event_callback(
+    increase_c2_cb,
+    RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS);
+
+  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, 1u);
+  EXPECT_EQ(c2, 1u);
+
+  auto publisher2 = node->create_publisher<test_msgs::msg::Empty>(
+    topic_name, qos_profile_publisher);
+
+  start = std::chrono::steady_clock::now();
+  do {
+    std::this_thread::sleep_for(100ms);
+  } while (c2 == 1u && std::chrono::steady_clock::now() - start < 10s);
+
+  EXPECT_EQ(c1, 1u);
+  EXPECT_EQ(c2, 2u);
+
+  publisher->clear_on_new_qos_event_callback(RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS);
+
+  auto subscription2 = node->create_subscription<test_msgs::msg::Empty>(
+    topic_name, qos_profile_subscription, message_callback);
+
+  publisher->set_on_new_qos_event_callback(increase_c1_cb, RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS);
+
+  start = std::chrono::steady_clock::now();
+  do {
+    std::this_thread::sleep_for(100ms);
+  } while (c1 == 1 && std::chrono::steady_clock::now() - start < 10s);
+
+  EXPECT_EQ(c1, 2u);
+  EXPECT_EQ(c2, 2u);
+}