diff --git a/rclcpp/include/rclcpp/loaned_message.hpp b/rclcpp/include/rclcpp/loaned_message.hpp index 3b94d86ae0..1d7b0d5e4f 100644 --- a/rclcpp/include/rclcpp/loaned_message.hpp +++ b/rclcpp/include/rclcpp/loaned_message.hpp @@ -183,14 +183,30 @@ class LoanedMessage /** * A call to `release()` will unmanage the memory for the ROS message. * That means that the destructor of this class will not free the memory on scope exit. + * If the message is loaned from the middleware but not be published, the user needs to call + * `rcl_return_loaned_message_from_publisher` manually. + * If the memory is from the local allocator, the memory is freed when the unique pointer + * goes out instead. * - * \return Raw pointer to the message instance. + * \return std::unique_ptr to the message instance. */ - MessageT * release() + std::unique_ptr> + release() { auto msg = message_; message_ = nullptr; - return msg; + + if (pub_.can_loan_messages()) { + return std::unique_ptr>(msg, [](MessageT *) {}); + } + + return std::unique_ptr>( + msg, + [allocator = message_allocator_](MessageT * msg_ptr) mutable { + // call destructor before deallocating + msg_ptr->~MessageT(); + allocator.deallocate(msg_ptr, 1); + }); } protected: diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index d8b572c90c..14b2f333c1 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -261,7 +261,7 @@ class Publisher : public PublisherBase if (this->can_loan_messages()) { // we release the ownership from the rclpp::LoanedMessage instance // and let the middleware clean up the memory. - this->do_loaned_message_publish(loaned_msg.release()); + this->do_loaned_message_publish(std::move(loaned_msg.release())); } else { // we don't release the ownership, let the middleware copy the ros message // and thus the destructor of rclcpp::LoanedMessage cleans up the memory. @@ -310,9 +310,9 @@ class Publisher : public PublisherBase } void - do_loaned_message_publish(MessageT * msg) + do_loaned_message_publish(std::unique_ptr> msg) { - auto status = rcl_publish_loaned_message(publisher_handle_.get(), msg, nullptr); + auto status = rcl_publish_loaned_message(publisher_handle_.get(), msg.get(), nullptr); if (RCL_RET_PUBLISHER_INVALID == status) { rcl_reset_error(); // next call will reset error message if not context diff --git a/rclcpp/test/rclcpp/test_loaned_message.cpp b/rclcpp/test/rclcpp/test_loaned_message.cpp index b68024c772..c82ff5fc7c 100644 --- a/rclcpp/test/rclcpp/test_loaned_message.cpp +++ b/rclcpp/test/rclcpp/test_loaned_message.cpp @@ -67,7 +67,7 @@ TEST_F(TestLoanedMessage, release) { auto node = std::make_shared("loaned_message_test_node"); auto pub = node->create_publisher("loaned_message_test_topic", 1); - MessageT * msg = nullptr; + std::unique_ptr> msg; { auto loaned_msg = pub->borrow_loaned_message(); ASSERT_TRUE(loaned_msg.is_valid()); @@ -81,6 +81,15 @@ TEST_F(TestLoanedMessage, release) { ASSERT_EQ(42.0f, msg->float64_value); + // Generally, the memory released from `LoanedMessage::release()` will be freed + // in deleter of unique_ptr or is managed in the middleware after calling + // `Publisher::do_loaned_message_publish` inside Publisher::publish(). + if (pub->can_loan_messages()) { + ASSERT_EQ( + RCL_RET_OK, + rcl_return_loaned_message_from_publisher(pub->get_publisher_handle().get(), msg.get())); + } + SUCCEED(); } diff --git a/rclcpp/test/rclcpp/test_publisher.cpp b/rclcpp/test/rclcpp/test_publisher.cpp index 5f0712fddc..5492844b23 100644 --- a/rclcpp/test/rclcpp/test_publisher.cpp +++ b/rclcpp/test/rclcpp/test_publisher.cpp @@ -462,9 +462,9 @@ class TestPublisherProtectedMethods : public rclcpp::Publisher::Publisher; - void publish_loaned_message(MessageT * msg) + void publish_loaned_message(rclcpp::LoanedMessage && loaned_msg) { - this->do_loaned_message_publish(msg); + this->do_loaned_message_publish(std::move(loaned_msg.release())); } void call_default_incompatible_qos_callback(rclcpp::QOSOfferedIncompatibleQoSInfo & event) const @@ -479,13 +479,14 @@ TEST_F(TestPublisher, do_loaned_message_publish_error) { auto publisher = node->create_publisher, PublisherT>("topic", 10); - auto msg = std::make_shared(); + auto msg = publisher->borrow_loaned_message(); + { // Using 'self' instead of 'lib:rclcpp' because `rcl_publish_loaned_message` is entirely // defined in a header auto mock = mocking_utils::patch_and_return( "self", rcl_publish_loaned_message, RCL_RET_PUBLISHER_INVALID); - EXPECT_THROW(publisher->publish_loaned_message(msg.get()), rclcpp::exceptions::RCLError); + EXPECT_THROW(publisher->publish_loaned_message(std::move(msg)), rclcpp::exceptions::RCLError); } }