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

This reverts commit 3652646.

rclcpp/CMakeLists.txt

@@ -125,7 +125,6 @@ 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
-  std::shared_ptr<rcl_client_t>
+  rcl_client_t *
   get_client_handle();
 
   RCLCPP_PUBLIC
-  std::shared_ptr<const rcl_client_t>
+  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_;
 
-  std::shared_ptr<rcl_client_t> client_handle_;
+  rcl_client_t client_handle_ = rcl_get_zero_initialized_client();
   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_.get(),
+      &client_handle_,
       this->get_rcl_node_handle(),
       service_type_support_handle,
       service_name.c_str(),
@@ -170,6 +170,11 @@ 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>
@@ -233,7 +238,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().get(), request.get(), &sequence_number);
+    rcl_ret_t ret = rcl_send_request(get_client_handle(), 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,18 +83,14 @@ class RCLCPP_PUBLIC MemoryStrategy
 
   static rclcpp::SubscriptionBase::SharedPtr
   get_subscription_by_handle(
-    std::shared_ptr<const rcl_subscription_t> subscriber_handle,
+    const rcl_subscription_t * subscriber_handle,
     const WeakNodeVector & weak_nodes);
 
   static rclcpp::ServiceBase::SharedPtr
-  get_service_by_handle(
-    std::shared_ptr<const rcl_service_t> service_handle,
-    const WeakNodeVector & weak_nodes);
+  get_service_by_handle(const rcl_service_t * service_handle, const WeakNodeVector & weak_nodes);
 
   static rclcpp::ClientBase::SharedPtr
-  get_client_by_handle(
-    std::shared_ptr<const rcl_client_t> client_handle,
-    const WeakNodeVector & weak_nodes);
+  get_client_by_handle(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,8 +21,6 @@
 #include <sstream>
 #include <string>
 
-#include "rcutils/logging_macros.h"
-
 #include "rcl/error_handling.h"
 #include "rcl/service.h"
 
@@ -32,7 +30,6 @@
 #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"
 
@@ -61,11 +58,11 @@ class ServiceBase
   get_service_name();
 
   RCLCPP_PUBLIC
-  std::shared_ptr<rcl_service_t>
+  rcl_service_t *
   get_service_handle();
 
   RCLCPP_PUBLIC
-  std::shared_ptr<const rcl_service_t>
+  const rcl_service_t *
   get_service_handle() const;
 
   virtual std::shared_ptr<void> create_request() = 0;
@@ -87,7 +84,7 @@ class ServiceBase
 
   std::shared_ptr<rcl_node_t> node_handle_;
 
-  std::shared_ptr<rcl_service_t> service_handle_;
+  rcl_service_t * service_handle_ = nullptr;
   std::string service_name_;
   bool owns_rcl_handle_ = true;
 };
@@ -119,22 +116,11 @@ class Service : public ServiceBase
     auto service_type_support_handle = get_service_type_support_handle<ServiceT>();
 
     // rcl does the static memory allocation here
-    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();
+    service_handle_ = new rcl_service_t;
+    *service_handle_ = rcl_get_zero_initialized_service();
 
     rcl_ret_t ret = rcl_service_init(
-      service_handle_.get(),
+      service_handle_,
       node_handle.get(),
       service_type_support_handle,
       service_name.c_str(),
@@ -155,29 +141,6 @@ 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,
@@ -186,7 +149,10 @@ class Service : public ServiceBase
     any_callback_(any_callback)
   {
     // check if service handle was initialized
-    if (!rcl_service_is_valid(service_handle, nullptr)) {
+    // TODO(karsten1987): Take this verification
+    // directly in rcl_*_t
+    // see: https://github.com/ros2/rcl/issues/81
+    if (!service_handle->impl) {
       // *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."));
@@ -197,17 +163,26 @@ 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);
-
-    // 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;
+    owns_rcl_handle_ = false;
   }
 
   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()
@@ -236,7 +211,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().get(), req_id.get(), response.get());
+    rcl_ret_t status = rcl_send_response(get_service_handle(), 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].reset();
+        subscription_handles_[i] = nullptr;
       }
     }
     for (size_t i = 0; i < wait_set->size_of_services; ++i) {
       if (!wait_set->services[i]) {
-        service_handles_[i].reset();
+        service_handles_[i] = nullptr;
       }
     }
     for (size_t i = 0; i < wait_set->size_of_clients; ++i) {
       if (!wait_set->clients[i]) {
-        client_handles_[i].reset();
+        client_handles_[i] = nullptr;
       }
     }
     for (size_t i = 0; i < wait_set->size_of_timers; ++i) {
       if (!wait_set->timers[i]) {
-        timer_handles_[i].reset();
+        timer_handles_[i] = nullptr;
       }
     }
 
@@ -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.get()) != RCL_RET_OK) {
+      if (rcl_wait_set_add_subscription(wait_set, subscription) != 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.get()) != RCL_RET_OK) {
+      if (rcl_wait_set_add_client(wait_set, client) != 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.get()) != RCL_RET_OK) {
+      if (rcl_wait_set_add_service(wait_set, service) != 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.get()) != RCL_RET_OK) {
+      if (rcl_wait_set_add_timer(wait_set, timer) != 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<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_;
+  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_;
 
   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
-  std::shared_ptr<rcl_subscription_t>
+  rcl_subscription_t *
   get_subscription_handle();
 
   RCLCPP_PUBLIC
-  const std::shared_ptr<rcl_subscription_t>
+  const rcl_subscription_t *
   get_subscription_handle() const;
 
   RCLCPP_PUBLIC
-  virtual const std::shared_ptr<rcl_subscription_t>
+  virtual const 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:
-  std::shared_ptr<rcl_subscription_t> intra_process_subscription_handle_;
-  std::shared_ptr<rcl_subscription_t> subscription_handle_;
+  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_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_.get(),
+      &intra_process_subscription_handle_,
       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 std::shared_ptr<rcl_subscription_t>
+  const 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
-  std::shared_ptr<const rcl_timer_t>
+  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:
-  std::shared_ptr<rcl_timer_t> timer_handle_;
+  rcl_timer_t timer_handle_ = rcl_get_zero_initialized_timer();
 };
 
 
@@ -122,12 +122,15 @@ 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_.get());
+    rcl_ret_t ret = rcl_timer_call(&timer_handle_);
     if (ret == RCL_RET_TIMER_CANCELED) {
       return;
     }