From 4efcfdc16bd039326896f9e38bc06255c6268797 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 20 May 2020 19:54:18 +0000 Subject: [PATCH] Make the rcl publisher handle a shared pointer. Signed-off-by: Chris Lalancette --- rclcpp/include/rclcpp/loaned_message.hpp | 4 +- rclcpp/include/rclcpp/publisher.hpp | 14 +++--- rclcpp/include/rclcpp/publisher_base.hpp | 8 ++-- rclcpp/src/rclcpp/publisher_base.cpp | 55 ++++++++++++++---------- 4 files changed, 45 insertions(+), 36 deletions(-) diff --git a/rclcpp/include/rclcpp/loaned_message.hpp b/rclcpp/include/rclcpp/loaned_message.hpp index 50a8766..3b94d86 100644 --- a/rclcpp/include/rclcpp/loaned_message.hpp +++ b/rclcpp/include/rclcpp/loaned_message.hpp @@ -66,7 +66,7 @@ public: if (pub_.can_loan_messages()) { void * message_ptr = nullptr; auto ret = rcl_borrow_loaned_message( - pub_.get_publisher_handle(), + pub_.get_publisher_handle().get(), rosidl_typesupport_cpp::get_message_type_support_handle(), &message_ptr); if (RCL_RET_OK != ret) { @@ -139,7 +139,7 @@ public: if (pub_.can_loan_messages()) { // return allocated memory to the middleware auto ret = - rcl_return_loaned_message_from_publisher(pub_.get_publisher_handle(), message_); + rcl_return_loaned_message_from_publisher(pub_.get_publisher_handle().get(), message_); if (ret != RCL_RET_OK) { RCLCPP_ERROR( error_logger, "rcl_deallocate_loaned_message failed: %s", rcl_get_error_string().str); diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index ebe349c..8518353 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -279,12 +279,12 @@ protected: void do_inter_process_publish(const MessageT & msg) { - auto status = rcl_publish(&publisher_handle_, &msg, nullptr); + auto status = rcl_publish(publisher_handle_.get(), &msg, nullptr); if (RCL_RET_PUBLISHER_INVALID == status) { rcl_reset_error(); // next call will reset error message if not context - if (rcl_publisher_is_valid_except_context(&publisher_handle_)) { - rcl_context_t * context = rcl_publisher_get_context(&publisher_handle_); + if (rcl_publisher_is_valid_except_context(publisher_handle_.get())) { + rcl_context_t * context = rcl_publisher_get_context(publisher_handle_.get()); if (nullptr != context && !rcl_context_is_valid(context)) { // publisher is invalid due to context being shutdown return; @@ -303,7 +303,7 @@ protected: // TODO(Karsten1987): support serialized message passed by intraprocess throw std::runtime_error("storing serialized messages in intra process is not supported yet"); } - auto status = rcl_publish_serialized_message(&publisher_handle_, serialized_msg, nullptr); + auto status = rcl_publish_serialized_message(publisher_handle_.get(), serialized_msg, nullptr); if (RCL_RET_OK != status) { rclcpp::exceptions::throw_from_rcl_error(status, "failed to publish serialized message"); } @@ -312,12 +312,12 @@ protected: void do_loaned_message_publish(MessageT * msg) { - auto status = rcl_publish_loaned_message(&publisher_handle_, msg, nullptr); + auto status = rcl_publish_loaned_message(publisher_handle_.get(), msg, nullptr); if (RCL_RET_PUBLISHER_INVALID == status) { rcl_reset_error(); // next call will reset error message if not context - if (rcl_publisher_is_valid_except_context(&publisher_handle_)) { - rcl_context_t * context = rcl_publisher_get_context(&publisher_handle_); + if (rcl_publisher_is_valid_except_context(publisher_handle_.get())) { + rcl_context_t * context = rcl_publisher_get_context(publisher_handle_.get()); if (nullptr != context && !rcl_context_is_valid(context)) { // publisher is invalid due to context being shutdown return; diff --git a/rclcpp/include/rclcpp/publisher_base.hpp b/rclcpp/include/rclcpp/publisher_base.hpp index 9627aca..0afe29b 100644 --- a/rclcpp/include/rclcpp/publisher_base.hpp +++ b/rclcpp/include/rclcpp/publisher_base.hpp @@ -99,13 +99,13 @@ public: /// Get the rcl publisher handle. /** \return The rcl publisher handle. */ RCLCPP_PUBLIC - rcl_publisher_t * + std::shared_ptr get_publisher_handle(); /// Get the rcl publisher handle. /** \return The rcl publisher handle. */ RCLCPP_PUBLIC - const rcl_publisher_t * + std::shared_ptr get_publisher_handle() const; /// Get all the QoS event handlers associated with this publisher. @@ -203,7 +203,7 @@ protected: auto handler = std::make_shared>( callback, rcl_publisher_event_init, - &publisher_handle_, + publisher_handle_.get(), event_type); event_handlers_.emplace_back(handler); } @@ -213,7 +213,7 @@ protected: std::shared_ptr rcl_node_handle_; - rcl_publisher_t publisher_handle_ = rcl_get_zero_initialized_publisher(); + std::shared_ptr publisher_handle_; std::vector> event_handlers_; diff --git a/rclcpp/src/rclcpp/publisher_base.cpp b/rclcpp/src/rclcpp/publisher_base.cpp index a10f2e1..decdf0b 100644 --- a/rclcpp/src/rclcpp/publisher_base.cpp +++ b/rclcpp/src/rclcpp/publisher_base.cpp @@ -47,8 +47,24 @@ PublisherBase::PublisherBase( : rcl_node_handle_(node_base->get_shared_rcl_node_handle()), intra_process_is_enabled_(false), intra_process_publisher_id_(0) { + auto custom_deleter = [node_handle = this->rcl_node_handle_](rcl_publisher_t * rcl_pub) + { + if (rcl_publisher_fini(rcl_pub, node_handle.get()) != RCL_RET_OK) { + RCLCPP_ERROR( + rclcpp::get_node_logger(node_handle.get()).get_child("rclcpp"), + "Error in destruction of rcl publisher handle: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + delete rcl_pub; + }; + + publisher_handle_ = std::shared_ptr( + new rcl_publisher_t, custom_deleter); + *publisher_handle_.get() = rcl_get_zero_initialized_publisher(); + rcl_ret_t ret = rcl_publisher_init( - &publisher_handle_, + publisher_handle_.get(), rcl_node_handle_.get(), &type_support, topic.c_str(), @@ -67,7 +83,7 @@ PublisherBase::PublisherBase( rclcpp::exceptions::throw_from_rcl_error(ret, "could not create publisher"); } // Life time of this object is tied to the publisher handle. - rmw_publisher_t * publisher_rmw_handle = rcl_publisher_get_rmw_handle(&publisher_handle_); + rmw_publisher_t * publisher_rmw_handle = rcl_publisher_get_rmw_handle(publisher_handle_.get()); if (!publisher_rmw_handle) { auto msg = std::string("failed to get rmw handle: ") + rcl_get_error_string().str; rcl_reset_error(); @@ -85,14 +101,6 @@ PublisherBase::~PublisherBase() // must fini the events before fini-ing the publisher event_handlers_.clear(); - if (rcl_publisher_fini(&publisher_handle_, rcl_node_handle_.get()) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Error in destruction of rcl publisher handle: %s", - rcl_get_error_string().str); - rcl_reset_error(); - } - auto ipm = weak_ipm_.lock(); if (!intra_process_is_enabled_) { @@ -102,7 +110,7 @@ PublisherBase::~PublisherBase() // TODO(ivanpauno): should this raise an error? RCLCPP_WARN( rclcpp::get_logger("rclcpp"), - "Intra process manager died before than a publisher."); + "Intra process manager died before a publisher."); return; } ipm->remove_publisher(intra_process_publisher_id_); @@ -111,13 +119,14 @@ PublisherBase::~PublisherBase() const char * PublisherBase::get_topic_name() const { - return rcl_publisher_get_topic_name(&publisher_handle_); + return rcl_publisher_get_topic_name(publisher_handle_.get()); } size_t PublisherBase::get_queue_size() const { - const rcl_publisher_options_t * publisher_options = rcl_publisher_get_options(&publisher_handle_); + const rcl_publisher_options_t * publisher_options = rcl_publisher_get_options( + publisher_handle_.get()); if (!publisher_options) { auto msg = std::string("failed to get publisher options: ") + rcl_get_error_string().str; rcl_reset_error(); @@ -132,16 +141,16 @@ PublisherBase::get_gid() const return rmw_gid_; } -rcl_publisher_t * +std::shared_ptr PublisherBase::get_publisher_handle() { - return &publisher_handle_; + return publisher_handle_; } -const rcl_publisher_t * +std::shared_ptr PublisherBase::get_publisher_handle() const { - return &publisher_handle_; + return publisher_handle_; } const std::vector> & @@ -156,13 +165,13 @@ PublisherBase::get_subscription_count() const size_t inter_process_subscription_count = 0; rcl_ret_t status = rcl_publisher_get_subscription_count( - &publisher_handle_, + publisher_handle_.get(), &inter_process_subscription_count); if (RCL_RET_PUBLISHER_INVALID == status) { rcl_reset_error(); /* next call will reset error message if not context */ - if (rcl_publisher_is_valid_except_context(&publisher_handle_)) { - rcl_context_t * context = rcl_publisher_get_context(&publisher_handle_); + if (rcl_publisher_is_valid_except_context(publisher_handle_.get())) { + rcl_context_t * context = rcl_publisher_get_context(publisher_handle_.get()); if (nullptr != context && !rcl_context_is_valid(context)) { /* publisher is invalid due to context being shutdown */ return 0; @@ -195,7 +204,7 @@ PublisherBase::get_intra_process_subscription_count() const rclcpp::QoS PublisherBase::get_actual_qos() const { - const rmw_qos_profile_t * qos = rcl_publisher_get_actual_qos(&publisher_handle_); + const rmw_qos_profile_t * qos = rcl_publisher_get_actual_qos(publisher_handle_.get()); if (!qos) { auto msg = std::string("failed to get qos settings: ") + rcl_get_error_string().str; rcl_reset_error(); @@ -208,13 +217,13 @@ PublisherBase::get_actual_qos() const bool PublisherBase::assert_liveliness() const { - return RCL_RET_OK == rcl_publisher_assert_liveliness(&publisher_handle_); + return RCL_RET_OK == rcl_publisher_assert_liveliness(publisher_handle_.get()); } bool PublisherBase::can_loan_messages() const { - return rcl_publisher_can_loan_messages(&publisher_handle_); + return rcl_publisher_can_loan_messages(publisher_handle_.get()); } bool