diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index f531079..8b41b90 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -16,10 +16,12 @@ #define RCLCPP_RCLCPP_CLIENT_HPP_ #include +#include #include #include #include +#include #include #include @@ -44,15 +46,21 @@ class ClientBase public: RCLCPP_MAKE_SHARED_DEFINITIONS(ClientBase); - ClientBase(rmw_client_t * client_handle, const std::string & service_name) - : client_handle_(client_handle), service_name_(service_name) + ClientBase( + std::shared_ptr node_handle, + rmw_client_t * client_handle, + const std::string & service_name) + : node_handle_(node_handle), client_handle_(client_handle), service_name_(service_name) {} ~ClientBase() { - if (client_handle_ != nullptr) { - rmw_destroy_client(client_handle_); - client_handle_ = nullptr; + if (client_handle_) { + if (rmw_destroy_client(client_handle_) == RMW_RET_ERROR) { + std::cerr << "Error in destruction of rmw client handle: " << + (rmw_get_error_string() ? rmw_get_error_string() : "") << + std::endl; + } } } @@ -74,6 +82,8 @@ public: private: RCLCPP_DISABLE_COPY(ClientBase); + std::shared_ptr node_handle_; + rmw_client_t * client_handle_; std::string service_name_; @@ -91,8 +101,11 @@ public: RCLCPP_MAKE_SHARED_DEFINITIONS(Client); - Client(rmw_client_t * client_handle, const std::string & service_name) - : ClientBase(client_handle, service_name) + Client( + std::shared_ptr node_handle, + rmw_client_t * client_handle, + const std::string & service_name) + : ClientBase(node_handle, client_handle, service_name) {} std::shared_ptr create_response() diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index f4b5962..602316a 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -187,7 +187,7 @@ private: std::string name_; - rmw_node_t * node_handle_; + std::shared_ptr node_handle_; rclcpp::context::Context::SharedPtr context_; @@ -226,6 +226,7 @@ private: > typename rclcpp::service::Service::SharedPtr create_service_internal( + std::shared_ptr node_handle, rmw_service_t * service_handle, const std::string & service_name, FunctorT callback) @@ -233,7 +234,7 @@ private: typename rclcpp::service::Service::CallbackType callback_without_header = callback; return service::Service::make_shared( - service_handle, service_name, callback_without_header); + node_handle, service_handle, service_name, callback_without_header); } template< @@ -271,6 +272,7 @@ private: > typename rclcpp::service::Service::SharedPtr create_service_internal( + std::shared_ptr node_handle, rmw_service_t * service_handle, const std::string & service_name, FunctorT callback) @@ -284,7 +286,7 @@ private: typename rclcpp::service::Service::CallbackWithHeaderType callback_with_header = callback; return service::Service::make_shared( - service_handle, service_name, callback_with_header); + node_handle, service_handle, service_name, callback_with_header); } }; diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 2d0aaf0..5f045ba 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -16,6 +16,7 @@ #define RCLCPP_RCLCPP_NODE_IMPL_HPP_ #include +#include #include #include #include @@ -44,7 +45,17 @@ Node::Node(std::string node_name, context::Context::SharedPtr context) : name_(node_name), context_(context), number_of_subscriptions_(0), number_of_timers_(0), number_of_services_(0) { - node_handle_ = rmw_create_node(name_.c_str()); + // Initialize node handle shared_ptr with custom deleter. + node_handle_.reset(rmw_create_node(name_.c_str()), [ = ](rmw_node_t * node) { + if (node_handle_) { + auto ret = rmw_destroy_node(node); + if (ret != RMW_RET_OK) { + std::cerr << "Error in destruction of rmw node handle: " + << (rmw_get_error_string() ? rmw_get_error_string() : "") + << std::endl; + } + } + }); if (!node_handle_) { // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) throw std::runtime_error( @@ -54,7 +65,7 @@ Node::Node(std::string node_name, context::Context::SharedPtr context) } using rclcpp::callback_group::CallbackGroupType; - default_callback_group_ = \ + default_callback_group_ = create_callback_group(CallbackGroupType::MutuallyExclusive); // TODO(esteve): remove hardcoded values events_publisher_ = @@ -79,7 +90,7 @@ Node::create_publisher(std::string topic_name, size_t queue_size) using rosidl_generator_cpp::get_message_type_support_handle; auto type_support_handle = get_message_type_support_handle(); rmw_publisher_t * publisher_handle = rmw_create_publisher( - node_handle_, type_support_handle, topic_name.c_str(), queue_size); + node_handle_.get(), type_support_handle, topic_name.c_str(), queue_size); if (!publisher_handle) { // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) throw std::runtime_error( @@ -88,7 +99,7 @@ Node::create_publisher(std::string topic_name, size_t queue_size) // *INDENT-ON* } - return publisher::Publisher::make_shared(publisher_handle); + return publisher::Publisher::make_shared(node_handle_, publisher_handle); } bool @@ -116,7 +127,7 @@ Node::create_subscription( using rosidl_generator_cpp::get_message_type_support_handle; auto type_support_handle = get_message_type_support_handle(); rmw_subscription_t * subscriber_handle = rmw_create_subscription( - node_handle_, type_support_handle, topic_name.c_str(), queue_size, ignore_local_publications); + node_handle_.get(), type_support_handle, topic_name.c_str(), queue_size, ignore_local_publications); if (!subscriber_handle) { // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) throw std::runtime_error( @@ -128,6 +139,7 @@ Node::create_subscription( using namespace rclcpp::subscription; auto sub = Subscription::make_shared( + node_handle_, subscriber_handle, topic_name, ignore_local_publications, @@ -189,7 +201,7 @@ Node::create_client( get_service_type_support_handle(); rmw_client_t * client_handle = rmw_create_client( - this->node_handle_, service_type_support_handle, service_name.c_str()); + this->node_handle_.get(), service_type_support_handle, service_name.c_str()); if (!client_handle) { // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) throw std::runtime_error( @@ -201,6 +213,7 @@ Node::create_client( using namespace rclcpp::client; auto cli = Client::make_shared( + node_handle_, client_handle, service_name); @@ -231,7 +244,7 @@ Node::create_service( get_service_type_support_handle(); rmw_service_t * service_handle = rmw_create_service( - this->node_handle_, service_type_support_handle, service_name.c_str()); + node_handle_.get(), service_type_support_handle, service_name.c_str()); if (!service_handle) { // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) throw std::runtime_error( @@ -241,7 +254,7 @@ Node::create_service( } auto serv = create_service_internal( - service_handle, service_name, callback); + node_handle_, service_handle, service_name, callback); auto serv_base_ptr = std::dynamic_pointer_cast(serv); if (group) { if (!group_in_node(group)) { diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 7ae9450..9b414d6 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -15,8 +15,10 @@ #ifndef RCLCPP_RCLCPP_PUBLISHER_HPP_ #define RCLCPP_RCLCPP_PUBLISHER_HPP_ +#include #include +#include #include #include @@ -38,10 +40,21 @@ class Publisher public: RCLCPP_MAKE_SHARED_DEFINITIONS(Publisher); - Publisher(rmw_publisher_t * publisher_handle) - : publisher_handle_(publisher_handle) + Publisher(std::shared_ptr node_handle, rmw_publisher_t * publisher_handle) + : node_handle_(node_handle), publisher_handle_(publisher_handle) {} + ~Publisher() + { + if (publisher_handle_) { + if (rmw_destroy_publisher(node_handle_.get(), publisher_handle_) == RMW_RET_ERROR) { + std::cerr << "Error in destruction of rmw publisher handle: " + << (rmw_get_error_string() ? rmw_get_error_string() : "") + << std::endl; + } + } + } + template void publish(std::shared_ptr & msg) @@ -50,6 +63,8 @@ public: } private: + std::shared_ptr node_handle_; + rmw_publisher_t * publisher_handle_; }; diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index ed345a3..9141d48 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -16,9 +16,11 @@ #define RCLCPP_RCLCPP_SERVICE_HPP_ #include +#include #include #include +#include #include #include @@ -44,16 +46,20 @@ public: RCLCPP_MAKE_SHARED_DEFINITIONS(ServiceBase); ServiceBase( + std::shared_ptr node_handle, rmw_service_t * service_handle, const std::string service_name) - : service_handle_(service_handle), service_name_(service_name) + : node_handle_(node_handle), service_handle_(service_handle), service_name_(service_name) {} ~ServiceBase() { - if (service_handle_ != nullptr) { - rmw_destroy_service(service_handle_); - service_handle_ = nullptr; + if (service_handle_) { + if (rmw_destroy_service(service_handle_) == RMW_RET_ERROR) { + std::cerr << "Error in destruction of rmw service_handle_ handle: " << + (rmw_get_error_string() ? rmw_get_error_string() : "") << + std::endl; + } } } @@ -76,6 +82,8 @@ public: private: RCLCPP_DISABLE_COPY(ServiceBase); + std::shared_ptr node_handle_; + rmw_service_t * service_handle_; std::string service_name_; @@ -98,17 +106,20 @@ public: RCLCPP_MAKE_SHARED_DEFINITIONS(Service); Service( + std::shared_ptr node_handle, rmw_service_t * service_handle, const std::string & service_name, CallbackType callback) - : ServiceBase(service_handle, service_name), callback_(callback), callback_with_header_(nullptr) + : ServiceBase(node_handle, service_handle, service_name), callback_(callback), + callback_with_header_(nullptr) {} Service( + std::shared_ptr node_handle, rmw_service_t * service_handle, const std::string & service_name, CallbackWithHeaderType callback_with_header) - : ServiceBase(service_handle, service_name), callback_(nullptr), + : ServiceBase(node_handle, service_handle, service_name), callback_(nullptr), callback_with_header_(callback_with_header) {} diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index b93dbd2..40fad86 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -16,9 +16,11 @@ #define RCLCPP_RCLCPP_SUBSCRIPTION_HPP_ #include +#include #include #include +#include #include #include @@ -43,14 +45,27 @@ public: RCLCPP_MAKE_SHARED_DEFINITIONS(SubscriptionBase); SubscriptionBase( + std::shared_ptr node_handle, rmw_subscription_t * subscription_handle, std::string & topic_name, bool ignore_local_publications) - : subscription_handle_(subscription_handle), + : node_handle_(node_handle), + subscription_handle_(subscription_handle), topic_name_(topic_name), ignore_local_publications_(ignore_local_publications) {} + ~SubscriptionBase() + { + if (subscription_handle_) { + if (rmw_destroy_subscription(node_handle_.get(), subscription_handle_) == RMW_RET_ERROR) { + std::cerr << "Error in destruction of rmw subscription handle: " << + (rmw_get_error_string() ? rmw_get_error_string() : "") << + std::endl; + } + } + } + std::string get_topic_name() { return this->topic_name_; @@ -62,6 +77,8 @@ public: private: RCLCPP_DISABLE_COPY(SubscriptionBase); + std::shared_ptr node_handle_; + rmw_subscription_t * subscription_handle_; std::string topic_name_; bool ignore_local_publications_; @@ -76,11 +93,12 @@ public: RCLCPP_MAKE_SHARED_DEFINITIONS(Subscription); Subscription( + std::shared_ptr node_handle, rmw_subscription_t * subscription_handle, std::string & topic_name, bool ignore_local_publications, CallbackType callback) - : SubscriptionBase(subscription_handle, topic_name, ignore_local_publications), + : SubscriptionBase(node_handle, subscription_handle, topic_name, ignore_local_publications), callback_(callback) {}