Add RMW listener APIs

Signed-off-by: Alberto Soragna <asoragna@irobot.com>

rclcpp/include/rclcpp/client.hpp

@@ -150,6 +150,12 @@ class ClientBase
   bool
   exchange_in_use_by_wait_set_state(bool in_use_state);
 
+  RCLCPP_PUBLIC
+  void
+  set_listener_callback(
+    rmw_listener_callback_t callback,
+    const void * user_data) const;
+
 protected:
   RCLCPP_DISABLE_COPY(ClientBase)
 

rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp

@@ -74,6 +74,12 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable
   rmw_qos_profile_t
   get_actual_qos() const;
 
+  RCLCPP_PUBLIC
+  void
+  set_listener_callback(
+    rmw_listener_callback_t callback,
+    const void * user_data) const override;
+
 protected:
   std::recursive_mutex reentrant_mutex_;
   rcl_guard_condition_t gc_;

rclcpp/include/rclcpp/qos_event.hpp

@@ -102,6 +102,12 @@ class QOSEventHandlerBase : public Waitable
   bool
   is_ready(rcl_wait_set_t * wait_set) override;
 
+  RCLCPP_PUBLIC
+  void
+  set_listener_callback(
+    rmw_listener_callback_t callback,
+    const void * user_data) const override;
+
 protected:
   rcl_event_t event_handle_;
   size_t wait_set_event_index_;

rclcpp/include/rclcpp/service.hpp

@@ -121,6 +121,12 @@ class ServiceBase
   bool
   exchange_in_use_by_wait_set_state(bool in_use_state);
 
+  RCLCPP_PUBLIC
+  void
+  set_listener_callback(
+    rmw_listener_callback_t callback,
+    const void * user_data) const;
+
 protected:
   RCLCPP_DISABLE_COPY(ServiceBase)
 

rclcpp/include/rclcpp/subscription_base.hpp

@@ -263,6 +263,12 @@ class SubscriptionBase : public std::enable_shared_from_this<SubscriptionBase>
   bool
   exchange_in_use_by_wait_set_state(void * pointer_to_subscription_part, bool in_use_state);
 
+  RCLCPP_PUBLIC
+  void
+  set_listener_callback(
+    rmw_listener_callback_t callback,
+    const void * user_data) const;
+
 protected:
   template<typename EventCallbackT>
   void

rclcpp/include/rclcpp/waitable.hpp

@@ -203,6 +203,13 @@ class Waitable
   bool
   exchange_in_use_by_wait_set_state(bool in_use_state);
 
+  RCLCPP_PUBLIC
+  virtual
+  void
+  set_listener_callback(
+    rmw_listener_callback_t callback,
+    const void * user_data) const;
+
 private:
   std::atomic<bool> in_use_by_wait_set_{false};
 };  // class Waitable

rclcpp/src/rclcpp/client.cpp

@@ -198,3 +198,18 @@ ClientBase::exchange_in_use_by_wait_set_state(bool in_use_state)
 {
   return in_use_by_wait_set_.exchange(in_use_state);
 }
+
+void
+ClientBase::set_listener_callback(
+  rmw_listener_callback_t callback,
+  const void * user_data) const
+{
+  rcl_ret_t ret = rcl_client_set_listener_callback(
+    client_handle_.get(),
+    callback,
+    user_data);
+
+  if (RCL_RET_OK != ret) {
+    throw std::runtime_error("Couldn't set listener callback to client");
+  }
+}

rclcpp/src/rclcpp/qos_event.cpp

@@ -68,4 +68,20 @@ QOSEventHandlerBase::is_ready(rcl_wait_set_t * wait_set)
   return wait_set->events[wait_set_event_index_] == &event_handle_;
 }
 
+void
+QOSEventHandlerBase::set_listener_callback(
+  rmw_listener_callback_t callback,
+  const void * user_data) const
+{
+  rcl_ret_t ret = rcl_event_set_listener_callback(
+    &event_handle_,
+    callback,
+    user_data,
+    true /* Use previous events */);
+
+  if (RCL_RET_OK != ret) {
+    throw std::runtime_error("Couldn't set listener callback to QOSEventHandlerBase");
+  }
+}
+
 }  // namespace rclcpp

rclcpp/src/rclcpp/service.cpp

@@ -84,3 +84,18 @@ ServiceBase::exchange_in_use_by_wait_set_state(bool in_use_state)
 {
   return in_use_by_wait_set_.exchange(in_use_state);
 }
+
+void
+ServiceBase::set_listener_callback(
+  rmw_listener_callback_t callback,
+  const void * user_data) const
+{
+  rcl_ret_t ret = rcl_service_set_listener_callback(
+    service_handle_.get(),
+    callback,
+    user_data);
+
+  if (RCL_RET_OK != ret) {
+    throw std::runtime_error("Couldn't set listener callback to service");
+  }
+}

rclcpp/src/rclcpp/subscription_base.cpp

@@ -288,3 +288,18 @@ SubscriptionBase::exchange_in_use_by_wait_set_state(
   }
   throw std::runtime_error("given pointer_to_subscription_part does not match any part");
 }
+
+void
+SubscriptionBase::set_listener_callback(
+  rmw_listener_callback_t callback,
+  const void * user_data) const
+{
+  rcl_ret_t ret = rcl_subscription_set_listener_callback(
+    subscription_handle_.get(),
+    callback,
+    user_data);
+
+  if (RCL_RET_OK != ret) {
+    throw std::runtime_error("Couldn't set listener callback to subscription");
+  }
+}

rclcpp/src/rclcpp/subscription_intra_process_base.cpp

@@ -36,3 +36,19 @@ SubscriptionIntraProcessBase::get_actual_qos() const
 {
   return qos_profile_;
 }
+
+void
+SubscriptionIntraProcessBase::set_listener_callback(
+  rmw_listener_callback_t callback,
+  const void * user_data) const
+{
+  rcl_ret_t ret = rcl_guard_condition_set_listener_callback(
+    &gc_,
+    callback,
+    user_data,
+    true /*Use previous events*/);
+
+  if (RCL_RET_OK != ret) {
+    throw std::runtime_error("Couldn't set guard condition listener callback");
+  }
+}

rclcpp/src/rclcpp/waitable.cpp

@@ -12,6 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <stdexcept>
+
 #include "rclcpp/waitable.hpp"
 
 using rclcpp::Waitable;
@@ -57,3 +59,15 @@ Waitable::exchange_in_use_by_wait_set_state(bool in_use_state)
 {
   return in_use_by_wait_set_.exchange(in_use_state);
 }
+
+void
+Waitable::set_listener_callback(
+  rmw_listener_callback_t callback,
+  const void * user_data) const
+{
+  (void)callback;
+  (void)user_data;
+
+  throw std::runtime_error(
+    "Custom waitables should override set_listener_callback() if they want to use RMW listeners");
+}