Feature: expose event callback setter in subscription, service, client and timer (backport #1496)

rclpy/src/rclpy/client.cpp

@@ -23,20 +23,27 @@
 
 #include <memory>
 #include <string>
+#include <utility>
 
 #include "client.hpp"
 #include "clock.hpp"
 #include "exceptions.hpp"
 #include "node.hpp"
 #include "python_allocator.hpp"
 #include "utils.hpp"
+#include "events_executor/rcl_support.hpp"
 
 namespace rclpy
 {
+using events_executor::RclEventCallbackTrampoline;
 
 void
 Client::destroy()
 {
+  try {
+    clear_on_new_response_callback();
+  } catch (RCLError) {
+  }
   rcl_client_.reset();
   node_.destroy();
 }
@@ -181,6 +188,41 @@ Client::get_logger_name() const
   return node_logger_name;
 }
 
+void
+Client::set_callback(
+  rcl_event_callback_t callback,
+  const void * user_data)
+{
+  rcl_ret_t ret = rcl_client_set_on_new_response_callback(
+    rcl_client_.get(),
+    callback,
+    user_data);
+
+  if (RCL_RET_OK != ret) {
+    throw RCLError(std::string("Failed to set the on new response callback for client: ") +
+      rcl_get_error_string().str);
+  }
+}
+
+void
+Client::set_on_new_response_callback(std::function<void(size_t)> callback)
+{
+  clear_on_new_response_callback();
+  on_new_response_callback_ = std::move(callback);
+  set_callback(
+    RclEventCallbackTrampoline,
+    static_cast<const void *>(&on_new_response_callback_));
+}
+
+void
+Client::clear_on_new_response_callback()
+{
+  if (on_new_response_callback_) {
+    set_callback(nullptr, nullptr);
+    on_new_response_callback_ = nullptr;
+  }
+}
+
 void
 define_client(py::object module)
 {
@@ -208,6 +250,10 @@ define_client(py::object module)
     "Configure whether introspection is enabled")
   .def(
     "get_logger_name", &Client::get_logger_name,
-    "Get the name of the logger associated with the node of the client.");
+    "Get the name of the logger associated with the node of the client.")
+  .def(
+    "set_on_new_response_callback", &Client::set_on_new_response_callback,
+    py::arg("callback"))
+  .def("clear_on_new_response_callback", &Client::clear_on_new_response_callback);
 }
 }  // namespace rclpy

rclpy/src/rclpy/client.hpp

@@ -16,6 +16,7 @@
 #define RCLPY__CLIENT_HPP_
 
 #include <pybind11/pybind11.h>
+#include <pybind11/functional.h>
 
 #include <rcl/client.h>
 #include <rcl/service_introspection.h>
@@ -118,10 +119,20 @@ class Client : public Destroyable, public std::enable_shared_from_this<Client>
   const char *
   get_logger_name() const;
 
+  void
+  set_on_new_response_callback(std::function<void(size_t)> callback);
+
+  void
+  clear_on_new_response_callback();
+
 private:
   Node node_;
+  std::function<void(size_t)> on_new_response_callback_{nullptr};
   std::shared_ptr<rcl_client_t> rcl_client_;
   rosidl_service_type_support_t * srv_type_;
+
+  void
+  set_callback(rcl_event_callback_t callback, const void * user_data);
 };
 
 /// Define a pybind11 wrapper for an rclpy::Client

rclpy/src/rclpy/events_executor/rcl_support.cpp

@@ -26,7 +26,13 @@ namespace events_executor
 extern "C" void RclEventCallbackTrampoline(const void * user_data, size_t number_of_events)
 {
   const auto cb = reinterpret_cast<const std::function<void(size_t)> *>(user_data);
-  (*cb)(number_of_events);
+  try {
+    (*cb)(number_of_events);
+  } catch (const std::exception & e) {
+    // Catch and print any exception to avoid propagation to c code
+    std::fprintf(stderr, "%s\n", e.what());
+    std::terminate();
+  }
 }
 
 RclCallbackManager::RclCallbackManager(EventsQueue * events_queue)

rclpy/src/rclpy/service.cpp

@@ -22,19 +22,26 @@
 
 #include <memory>
 #include <string>
+#include <utility>
 
 #include "clock.hpp"
 #include "exceptions.hpp"
 #include "node.hpp"
 #include "service.hpp"
 #include "utils.hpp"
+#include "events_executor/rcl_support.hpp"
 
 namespace rclpy
 {
+using events_executor::RclEventCallbackTrampoline;
 
 void
 Service::destroy()
 {
+  try {
+    clear_on_new_request_callback();
+  } catch (RCLError) {
+  }
   rcl_service_.reset();
   node_.destroy();
 }
@@ -184,6 +191,41 @@ Service::configure_introspection(
   }
 }
 
+void
+Service::set_callback(
+  rcl_event_callback_t callback,
+  const void * user_data)
+{
+  rcl_ret_t ret = rcl_service_set_on_new_request_callback(
+    rcl_service_.get(),
+    callback,
+    user_data);
+
+  if (RCL_RET_OK != ret) {
+    throw RCLError(std::string("Failed to set the on new request callback for service: ") +
+      rcl_get_error_string().str);
+  }
+}
+
+void
+Service::set_on_new_request_callback(std::function<void(size_t)> callback)
+{
+  clear_on_new_request_callback();
+  on_new_request_callback_ = std::move(callback);
+  set_callback(
+    RclEventCallbackTrampoline,
+    static_cast<const void *>(&on_new_request_callback_));
+}
+
+void
+Service::clear_on_new_request_callback()
+{
+  if (on_new_request_callback_) {
+    set_callback(nullptr, nullptr);
+    on_new_request_callback_ = nullptr;
+  }
+}
+
 void
 define_service(py::object module)
 {
@@ -211,6 +253,10 @@ define_service(py::object module)
     "Configure whether introspection is enabled")
   .def(
     "get_logger_name", &Service::get_logger_name,
-    "Get the name of the logger associated with the node of the service.");
+    "Get the name of the logger associated with the node of the service.")
+  .def(
+    "set_on_new_request_callback", &Service::set_on_new_request_callback,
+    py::arg("callback"))
+  .def("clear_on_new_request_callback", &Service::clear_on_new_request_callback);
 }
 }  // namespace rclpy

rclpy/src/rclpy/service.hpp

@@ -16,6 +16,7 @@
 #define RCLPY__SERVICE_HPP_
 
 #include <pybind11/pybind11.h>
+#include <pybind11/functional.h>
 
 #include <rcl/service.h>
 #include <rcl/service_introspection.h>
@@ -123,10 +124,20 @@ class Service : public Destroyable, public std::enable_shared_from_this<Service>
   void
   destroy() override;
 
+  void
+  set_on_new_request_callback(std::function<void(size_t)> callback);
+
+  void
+  clear_on_new_request_callback();
+
 private:
   Node node_;
+  std::function<void(size_t)> on_new_request_callback_{nullptr};
   std::shared_ptr<rcl_service_t> rcl_service_;
   rosidl_service_type_support_t * srv_type_;
+
+  void
+  set_callback(rcl_event_callback_t callback, const void * user_data);
 };
 
 /// Define a pybind11 wrapper for an rclpy::Service

rclpy/src/rclpy/subscription.cpp

@@ -24,17 +24,21 @@
 #include <memory>
 #include <stdexcept>
 #include <string>
+#include <utility>
 
 #include "exceptions.hpp"
 #include "node.hpp"
 #include "serialization.hpp"
 #include "subscription.hpp"
 #include "utils.hpp"
+#include "events_executor/rcl_support.hpp"
 
 using pybind11::literals::operator""_a;
 
 namespace rclpy
 {
+using events_executor::RclEventCallbackTrampoline;
+
 Subscription::Subscription(
   Node & node, py::object pymsg_type, std::string topic,
   py::object pyqos_profile)
@@ -87,6 +91,10 @@ Subscription::Subscription(
 
 void Subscription::destroy()
 {
+  try {
+    clear_on_new_message_callback();
+  } catch (RCLError) {
+  }
   rcl_subscription_.reset();
   node_.destroy();
 }
@@ -181,6 +189,41 @@ Subscription::get_publisher_count() const
   return count;
 }
 
+void
+Subscription::set_callback(
+  rcl_event_callback_t callback,
+  const void * user_data)
+{
+  rcl_ret_t ret = rcl_subscription_set_on_new_message_callback(
+    rcl_subscription_.get(),
+    callback,
+    user_data);
+
+  if (RCL_RET_OK != ret) {
+    throw RCLError(std::string("Failed to set the on new message callback for subscription: ") +
+      rcl_get_error_string().str);
+  }
+}
+
+void
+Subscription::set_on_new_message_callback(std::function<void(size_t)> callback)
+{
+  clear_on_new_message_callback();
+  on_new_message_callback_ = std::move(callback);
+  set_callback(
+    RclEventCallbackTrampoline,
+    static_cast<const void *>(&on_new_message_callback_));
+}
+
+void
+Subscription::clear_on_new_message_callback()
+{
+  if (on_new_message_callback_) {
+    set_callback(nullptr, nullptr);
+    on_new_message_callback_ = nullptr;
+  }
+}
+
 void
 define_subscription(py::object module)
 {
@@ -202,6 +245,10 @@ define_subscription(py::object module)
     "Return the resolved topic name of a subscription.")
   .def(
     "get_publisher_count", &Subscription::get_publisher_count,
-    "Count the publishers from a subscription.");
+    "Count the publishers from a subscription.")
+  .def(
+    "set_on_new_message_callback", &Subscription::set_on_new_message_callback,
+    py::arg("callback"))
+  .def("clear_on_new_message_callback", &Subscription::clear_on_new_message_callback);
 }
 }  // namespace rclpy

rclpy/src/rclpy/subscription.hpp

@@ -16,6 +16,7 @@
 #define RCLPY__SUBSCRIPTION_HPP_
 
 #include <pybind11/pybind11.h>
+#include <pybind11/functional.h>
 
 #include <rcl/subscription.h>
 
@@ -105,9 +106,19 @@ class Subscription : public Destroyable, public std::enable_shared_from_this<Sub
   void
   destroy() override;
 
+  void
+  set_on_new_message_callback(std::function<void(size_t)> callback);
+
+  void
+  clear_on_new_message_callback();
+
 private:
   Node node_;
+  std::function<void(size_t)> on_new_message_callback_{nullptr};
   std::shared_ptr<rcl_subscription_t> rcl_subscription_;
+
+  void
+  set_callback(rcl_event_callback_t callback, const void * user_data);
 };
 /// Define a pybind11 wrapper for an rclpy::Subscription
 void define_subscription(py::object module);