Revert "Revert "Store the subscriber, client, service and timer (#431)…

…" (#448)"

This reverts commit 168d75c.

rclcpp/CMakeLists.txt

@@ -125,6 +125,7 @@ ament_export_include_directories(include)
 ament_export_libraries(${PROJECT_NAME})
 
 if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
 

rclcpp/include/rclcpp/client.hpp

@@ -68,11 +68,11 @@ class ClientBase
   get_service_name() const;
 
   RCLCPP_PUBLIC
-  rcl_client_t *
+  std::shared_ptr<rcl_client_t>
   get_client_handle();
 
   RCLCPP_PUBLIC
-  const rcl_client_t *
+  std::shared_ptr<const rcl_client_t>
   get_client_handle() const;
 
   RCLCPP_PUBLIC
@@ -112,7 +112,7 @@ class ClientBase
   rclcpp::node_interfaces::NodeGraphInterface::WeakPtr node_graph_;
   std::shared_ptr<rcl_node_t> node_handle_;
 
-  rcl_client_t client_handle_ = rcl_get_zero_initialized_client();
+  std::shared_ptr<rcl_client_t> client_handle_;
   std::string service_name_;
 };
 
@@ -148,7 +148,7 @@ class Client : public ClientBase
     auto service_type_support_handle =
       get_service_type_support_handle<ServiceT>();
     rcl_ret_t ret = rcl_client_init(
-      &client_handle_,
+      client_handle_.get(),
       this->get_rcl_node_handle(),
       service_type_support_handle,
       service_name.c_str(),
@@ -170,11 +170,6 @@ class Client : public ClientBase
 
   virtual ~Client()
   {
-    if (rcl_client_fini(&client_handle_, this->get_rcl_node_handle()) != RCL_RET_OK) {
-      fprintf(stderr,
-        "Error in destruction of rcl client handle: %s\n", rcl_get_error_string_safe());
-      rcl_reset_error();
-    }
   }
 
   std::shared_ptr<void>
@@ -238,7 +233,7 @@ class Client : public ClientBase
   {
     std::lock_guard<std::mutex> lock(pending_requests_mutex_);
     int64_t sequence_number;
-    rcl_ret_t ret = rcl_send_request(get_client_handle(), request.get(), &sequence_number);
+    rcl_ret_t ret = rcl_send_request(get_client_handle().get(), request.get(), &sequence_number);
     if (RCL_RET_OK != ret) {
       rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send request");
     }

rclcpp/include/rclcpp/memory_strategy.hpp

@@ -83,14 +83,18 @@ class RCLCPP_PUBLIC MemoryStrategy
 
   static rclcpp::SubscriptionBase::SharedPtr
   get_subscription_by_handle(
-    const rcl_subscription_t * subscriber_handle,
+    std::shared_ptr<const rcl_subscription_t> subscriber_handle,
     const WeakNodeVector & weak_nodes);
 
   static rclcpp::ServiceBase::SharedPtr
-  get_service_by_handle(const rcl_service_t * service_handle, const WeakNodeVector & weak_nodes);
+  get_service_by_handle(
+    std::shared_ptr<const rcl_service_t> service_handle,
+    const WeakNodeVector & weak_nodes);
 
   static rclcpp::ClientBase::SharedPtr
-  get_client_by_handle(const rcl_client_t * client_handle, const WeakNodeVector & weak_nodes);
+  get_client_by_handle(
+    std::shared_ptr<const rcl_client_t> client_handle,
+    const WeakNodeVector & weak_nodes);
 
   static rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
   get_node_by_group(

rclcpp/include/rclcpp/service.hpp

@@ -21,6 +21,8 @@
 #include <sstream>
 #include <string>
 
+#include "rcutils/logging_macros.h"
+
 #include "rcl/error_handling.h"
 #include "rcl/service.h"
 
@@ -30,6 +32,7 @@
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/expand_topic_or_service_name.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rclcpp/logging.hpp"
 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"
 
@@ -58,11 +61,11 @@ class ServiceBase
   get_service_name();
 
   RCLCPP_PUBLIC
-  rcl_service_t *
+  std::shared_ptr<rcl_service_t>
   get_service_handle();
 
   RCLCPP_PUBLIC
-  const rcl_service_t *
+  std::shared_ptr<const rcl_service_t>
   get_service_handle() const;
 
   virtual std::shared_ptr<void> create_request() = 0;
@@ -84,7 +87,7 @@ class ServiceBase
 
   std::shared_ptr<rcl_node_t> node_handle_;
 
-  rcl_service_t * service_handle_ = nullptr;
+  std::shared_ptr<rcl_service_t> service_handle_;
   std::string service_name_;
   bool owns_rcl_handle_ = true;
 };
@@ -116,11 +119,22 @@ class Service : public ServiceBase
     auto service_type_support_handle = get_service_type_support_handle<ServiceT>();
 
     // rcl does the static memory allocation here
-    service_handle_ = new rcl_service_t;
-    *service_handle_ = rcl_get_zero_initialized_service();
+    service_handle_ = std::shared_ptr<rcl_service_t>(
+      new rcl_service_t, [ = ](rcl_service_t * service)
+      {
+        if (rcl_service_fini(service, node_handle_.get()) != RCL_RET_OK) {
+          RCLCPP_ERROR(
+            rclcpp::get_logger(rcl_node_get_name(node_handle.get())).get_child("rclcpp"),
+            "Error in destruction of rcl service handle: %s",
+            rcl_get_error_string_safe());
+          rcl_reset_error();
+        }
+        delete service;
+      });
+    *service_handle_.get() = rcl_get_zero_initialized_service();
 
     rcl_ret_t ret = rcl_service_init(
-      service_handle_,
+      service_handle_.get(),
       node_handle.get(),
       service_type_support_handle,
       service_name.c_str(),
@@ -141,6 +155,29 @@ class Service : public ServiceBase
     }
   }
 
+  Service(
+    std::shared_ptr<rcl_node_t> node_handle,
+    std::shared_ptr<rcl_service_t> service_handle,
+    AnyServiceCallback<ServiceT> any_callback)
+  : ServiceBase(node_handle),
+    any_callback_(any_callback)
+  {
+    // check if service handle was initialized
+    if (!rcl_service_is_valid(service_handle.get(), nullptr)) {
+      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
+      throw std::runtime_error(
+        std::string("rcl_service_t in constructor argument must be initialized beforehand."));
+      // *INDENT-ON*
+    }
+
+    const char * service_name = rcl_service_get_service_name(service_handle.get());
+    if (!service_name) {
+      throw std::runtime_error("failed to get service name");
+    }
+    service_handle_ = service_handle;
+    service_name_ = std::string(service_name);
+  }
+
   Service(
     std::shared_ptr<rcl_node_t> node_handle,
     rcl_service_t * service_handle,
@@ -149,10 +186,7 @@ class Service : public ServiceBase
     any_callback_(any_callback)
   {
     // check if service handle was initialized
-    // TODO(karsten1987): Take this verification
-    // directly in rcl_*_t
-    // see: https://github.com/ros2/rcl/issues/81
-    if (!service_handle->impl) {
+    if (!rcl_service_is_valid(service_handle, nullptr)) {
       // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
       throw std::runtime_error(
         std::string("rcl_service_t in constructor argument must be initialized beforehand."));
@@ -163,26 +197,17 @@ class Service : public ServiceBase
     if (!service_name) {
       throw std::runtime_error("failed to get service name");
     }
-    service_handle_ = service_handle;
     service_name_ = std::string(service_name);
-    owns_rcl_handle_ = false;
+
+    // In this case, rcl owns the service handle memory
+    service_handle_ = std::shared_ptr<rcl_service_t>(new rcl_service_t);
+    service_handle_->impl = service_handle->impl;
   }
 
   Service() = delete;
 
   virtual ~Service()
   {
-    // check if you have ownership of the handle
-    if (owns_rcl_handle_) {
-      if (rcl_service_fini(service_handle_, node_handle_.get()) != RCL_RET_OK) {
-        std::stringstream ss;
-        ss << "Error in destruction of rcl service_handle_ handle: " <<
-          rcl_get_error_string_safe() << '\n';
-        (std::cerr << ss.str()).flush();
-        rcl_reset_error();
-      }
-      delete service_handle_;
-    }
   }
 
   std::shared_ptr<void> create_request()
@@ -211,7 +236,7 @@ class Service : public ServiceBase
     std::shared_ptr<rmw_request_id_t> req_id,
     std::shared_ptr<typename ServiceT::Response> response)
   {
-    rcl_ret_t status = rcl_send_response(get_service_handle(), req_id.get(), response.get());
+    rcl_ret_t status = rcl_send_response(get_service_handle().get(), req_id.get(), response.get());
 
     if (status != RCL_RET_OK) {
       rclcpp::exceptions::throw_from_rcl_error(status, "failed to send response");

rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp

@@ -98,22 +98,22 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
   {
     for (size_t i = 0; i < wait_set->size_of_subscriptions; ++i) {
       if (!wait_set->subscriptions[i]) {
-        subscription_handles_[i] = nullptr;
+        subscription_handles_[i].reset();
       }
     }
     for (size_t i = 0; i < wait_set->size_of_services; ++i) {
       if (!wait_set->services[i]) {
-        service_handles_[i] = nullptr;
+        service_handles_[i].reset();
       }
     }
     for (size_t i = 0; i < wait_set->size_of_clients; ++i) {
       if (!wait_set->clients[i]) {
-        client_handles_[i] = nullptr;
+        client_handles_[i].reset();
       }
     }
     for (size_t i = 0; i < wait_set->size_of_timers; ++i) {
       if (!wait_set->timers[i]) {
-        timer_handles_[i] = nullptr;
+        timer_handles_[i].reset();
       }
     }
 
@@ -188,7 +188,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
   bool add_handles_to_wait_set(rcl_wait_set_t * wait_set)
   {
     for (auto subscription : subscription_handles_) {
-      if (rcl_wait_set_add_subscription(wait_set, subscription) != RCL_RET_OK) {
+      if (rcl_wait_set_add_subscription(wait_set, subscription.get()) != RCL_RET_OK) {
         RCUTILS_LOG_ERROR_NAMED(
           "rclcpp",
           "Couldn't add subscription to wait set: %s", rcl_get_error_string_safe());
@@ -197,7 +197,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
     }
 
     for (auto client : client_handles_) {
-      if (rcl_wait_set_add_client(wait_set, client) != RCL_RET_OK) {
+      if (rcl_wait_set_add_client(wait_set, client.get()) != RCL_RET_OK) {
         RCUTILS_LOG_ERROR_NAMED(
           "rclcpp",
           "Couldn't add client to wait set: %s", rcl_get_error_string_safe());
@@ -206,7 +206,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
     }
 
     for (auto service : service_handles_) {
-      if (rcl_wait_set_add_service(wait_set, service) != RCL_RET_OK) {
+      if (rcl_wait_set_add_service(wait_set, service.get()) != RCL_RET_OK) {
         RCUTILS_LOG_ERROR_NAMED(
           "rclcpp",
           "Couldn't add service to wait set: %s", rcl_get_error_string_safe());
@@ -215,7 +215,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
     }
 
     for (auto timer : timer_handles_) {
-      if (rcl_wait_set_add_timer(wait_set, timer) != RCL_RET_OK) {
+      if (rcl_wait_set_add_timer(wait_set, timer.get()) != RCL_RET_OK) {
         RCUTILS_LOG_ERROR_NAMED(
           "rclcpp",
           "Couldn't add timer to wait set: %s", rcl_get_error_string_safe());
@@ -391,10 +391,10 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
 
   VectorRebind<const rcl_guard_condition_t *> guard_conditions_;
 
-  VectorRebind<const rcl_subscription_t *> subscription_handles_;
-  VectorRebind<const rcl_service_t *> service_handles_;
-  VectorRebind<const rcl_client_t *> client_handles_;
-  VectorRebind<const rcl_timer_t *> timer_handles_;
+  VectorRebind<std::shared_ptr<const rcl_subscription_t>> subscription_handles_;
+  VectorRebind<std::shared_ptr<const rcl_service_t>> service_handles_;
+  VectorRebind<std::shared_ptr<const rcl_client_t>> client_handles_;
+  VectorRebind<std::shared_ptr<const rcl_timer_t>> timer_handles_;
 
   std::shared_ptr<ExecAlloc> executable_allocator_;
   std::shared_ptr<VoidAlloc> allocator_;

rclcpp/include/rclcpp/subscription.hpp

@@ -76,15 +76,15 @@ class SubscriptionBase
   get_topic_name() const;
 
   RCLCPP_PUBLIC
-  rcl_subscription_t *
+  std::shared_ptr<rcl_subscription_t>
   get_subscription_handle();
 
   RCLCPP_PUBLIC
-  const rcl_subscription_t *
+  const std::shared_ptr<rcl_subscription_t>
   get_subscription_handle() const;
 
   RCLCPP_PUBLIC
-  virtual const rcl_subscription_t *
+  virtual const std::shared_ptr<rcl_subscription_t>
   get_intra_process_subscription_handle() const;
 
   /// Borrow a new message.
@@ -110,8 +110,8 @@ class SubscriptionBase
     const rmw_message_info_t & message_info) = 0;
 
 protected:
-  rcl_subscription_t intra_process_subscription_handle_ = rcl_get_zero_initialized_subscription();
-  rcl_subscription_t subscription_handle_ = rcl_get_zero_initialized_subscription();
+  std::shared_ptr<rcl_subscription_t> intra_process_subscription_handle_;
+  std::shared_ptr<rcl_subscription_t> subscription_handle_;
   std::shared_ptr<rcl_node_t> node_handle_;
 
 private:
@@ -241,7 +241,7 @@ class Subscription : public SubscriptionBase
   {
     std::string intra_process_topic_name = std::string(get_topic_name()) + "/_intra";
     rcl_ret_t ret = rcl_subscription_init(
-      &intra_process_subscription_handle_,
+      intra_process_subscription_handle_.get(),
       node_handle_.get(),
       rclcpp::type_support::get_intra_process_message_msg_type_support(),
       intra_process_topic_name.c_str(),
@@ -266,13 +266,13 @@ class Subscription : public SubscriptionBase
   }
 
   /// Implemenation detail.
-  const rcl_subscription_t *
+  const std::shared_ptr<rcl_subscription_t>
   get_intra_process_subscription_handle() const
   {
     if (!get_intra_process_message_callback_) {
       return nullptr;
     }
-    return &intra_process_subscription_handle_;
+    return intra_process_subscription_handle_;
   }
 
 private:

rclcpp/include/rclcpp/timer.hpp

@@ -62,7 +62,7 @@ class TimerBase
   execute_callback() = 0;
 
   RCLCPP_PUBLIC
-  const rcl_timer_t *
+  std::shared_ptr<const rcl_timer_t>
   get_timer_handle();
 
   /// Check how long the timer has until its next scheduled callback.
@@ -85,7 +85,7 @@ class TimerBase
   bool is_ready();
 
 protected:
-  rcl_timer_t timer_handle_ = rcl_get_zero_initialized_timer();
+  std::shared_ptr<rcl_timer_t> timer_handle_;
 };
 
 
@@ -122,15 +122,12 @@ class GenericTimer : public TimerBase
   {
     // Stop the timer from running.
     cancel();
-    if (rcl_timer_fini(&timer_handle_) != RCL_RET_OK) {
-      fprintf(stderr, "Failed to clean up rcl timer handle: %s\n", rcl_get_error_string_safe());
-    }
   }
 
   void
   execute_callback()
   {
-    rcl_ret_t ret = rcl_timer_call(&timer_handle_);
+    rcl_ret_t ret = rcl_timer_call(timer_handle_.get());
     if (ret == RCL_RET_TIMER_CANCELED) {
       return;
     }