From 9141c8f89911f57ee4bfbdec5cbc1ebe1dc0ce7b Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Fri, 11 Sep 2020 16:47:39 +0800 Subject: [PATCH 1/9] Destroy msg extracted from LoanedMessage. Signed-off-by: Chen Lihui --- rclcpp/test/rclcpp/test_loaned_message.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rclcpp/test/rclcpp/test_loaned_message.cpp b/rclcpp/test/rclcpp/test_loaned_message.cpp index b68024c772..8c809e662b 100644 --- a/rclcpp/test/rclcpp/test_loaned_message.cpp +++ b/rclcpp/test/rclcpp/test_loaned_message.cpp @@ -81,6 +81,8 @@ TEST_F(TestLoanedMessage, release) { ASSERT_EQ(42.0f, msg->float64_value); + msg->~MessageT(); + pub->get_allocator()->deallocate(msg, 1); SUCCEED(); } From 3fa343c67d8a6dedd7e356fc1c97b92f60eb6b09 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Wed, 23 Sep 2020 09:39:58 +0800 Subject: [PATCH 2/9] Remove the release method of LoadedMessage and a related test case Signed-off-by: Chen Lihui --- rclcpp/include/rclcpp/loaned_message.hpp | 14 -------------- 1 file changed, 14 deletions(-) diff --git a/rclcpp/include/rclcpp/loaned_message.hpp b/rclcpp/include/rclcpp/loaned_message.hpp index 3b94d86ae0..530ddc5024 100644 --- a/rclcpp/include/rclcpp/loaned_message.hpp +++ b/rclcpp/include/rclcpp/loaned_message.hpp @@ -179,20 +179,6 @@ class LoanedMessage return *message_; } - /// Release ownership of the ROS message instance. - /** - * 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. - * - * \return Raw pointer to the message instance. - */ - MessageT * release() - { - auto msg = message_; - message_ = nullptr; - return msg; - } - protected: const rclcpp::PublisherBase & pub_; From a5bbd4e2d1933a2020488facfde230794ffc7e93 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Wed, 23 Sep 2020 11:13:31 +0800 Subject: [PATCH 3/9] Revert "Remove the release method of LoadedMessage and a related test case" This reverts commit b9825251d148198cb63dc841139e88e77ac02aff. Signed-off-by: Chen Lihui --- rclcpp/include/rclcpp/loaned_message.hpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/rclcpp/include/rclcpp/loaned_message.hpp b/rclcpp/include/rclcpp/loaned_message.hpp index 530ddc5024..3b94d86ae0 100644 --- a/rclcpp/include/rclcpp/loaned_message.hpp +++ b/rclcpp/include/rclcpp/loaned_message.hpp @@ -179,6 +179,20 @@ class LoanedMessage return *message_; } + /// Release ownership of the ROS message instance. + /** + * 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. + * + * \return Raw pointer to the message instance. + */ + MessageT * release() + { + auto msg = message_; + message_ = nullptr; + return msg; + } + protected: const rclcpp::PublisherBase & pub_; From 37399997d31a115f06a59c138046159e4c3f0588 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Wed, 23 Sep 2020 14:03:06 +0800 Subject: [PATCH 4/9] Use unique_ptr as return type for release method of LoanedMessage Signed-off-by: Chen Lihui --- rclcpp/include/rclcpp/loaned_message.hpp | 52 +++++++++++++--------- rclcpp/test/rclcpp/test_loaned_message.cpp | 6 +-- 2 files changed, 34 insertions(+), 24 deletions(-) diff --git a/rclcpp/include/rclcpp/loaned_message.hpp b/rclcpp/include/rclcpp/loaned_message.hpp index 3b94d86ae0..ba71e2ce3a 100644 --- a/rclcpp/include/rclcpp/loaned_message.hpp +++ b/rclcpp/include/rclcpp/loaned_message.hpp @@ -130,26 +130,10 @@ class LoanedMessage */ virtual ~LoanedMessage() { - auto error_logger = rclcpp::get_logger("LoanedMessage"); - if (message_ == nullptr) { return; } - - if (pub_.can_loan_messages()) { - // return allocated memory to the middleware - auto ret = - 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); - rcl_reset_error(); - } - } else { - // call destructor before deallocating - message_->~MessageT(); - message_allocator_.deallocate(message_, 1); - } + DestroyMessage(pub_, message_, message_allocator_); message_ = nullptr; } @@ -184,13 +168,18 @@ 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. * - * \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; + return std::unique_ptr>( + msg, + [pub = pub_, allocator = message_allocator_](MessageT * msg_ptr) { + LoanedMessage::DestroyMessage(pub, msg_ptr, allocator); + }); } protected: @@ -202,6 +191,29 @@ class LoanedMessage /// Deleted copy constructor to preserve memory integrity. LoanedMessage(const LoanedMessage & other) = delete; + +private: + static void DestroyMessage( + const rclcpp::PublisherBase & pub, MessageT * msg_ptr, + std::allocator allocator) + { + auto error_logger = rclcpp::get_logger("LoanedMessage"); + + if (pub.can_loan_messages()) { + // return allocated memory to the middleware + auto ret = + rcl_return_loaned_message_from_publisher(pub.get_publisher_handle().get(), msg_ptr); + if (ret != RCL_RET_OK) { + RCLCPP_ERROR( + error_logger, "rcl_deallocate_loaned_message failed: %s", rcl_get_error_string().str); + rcl_reset_error(); + } + } else { + // call destructor before deallocating + msg_ptr->~MessageT(); + allocator.deallocate(msg_ptr, 1); + } + } }; } // namespace rclcpp diff --git a/rclcpp/test/rclcpp/test_loaned_message.cpp b/rclcpp/test/rclcpp/test_loaned_message.cpp index 8c809e662b..c5206b5d5c 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()); @@ -79,10 +79,8 @@ TEST_F(TestLoanedMessage, release) { // of the data after a call to release. } - ASSERT_EQ(42.0f, msg->float64_value); + ASSERT_EQ(42.0f, msg.get()->float64_value); - msg->~MessageT(); - pub->get_allocator()->deallocate(msg, 1); SUCCEED(); } From 459cb2199b0463f2d75608bb1442280a427dbfb1 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Thu, 24 Sep 2020 09:09:15 +0800 Subject: [PATCH 5/9] Update based on review. Co-authored-by: William Woodall Signed-off-by: Chen Lihui --- rclcpp/include/rclcpp/loaned_message.hpp | 9 +++++---- rclcpp/test/rclcpp/test_loaned_message.cpp | 2 +- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/rclcpp/include/rclcpp/loaned_message.hpp b/rclcpp/include/rclcpp/loaned_message.hpp index ba71e2ce3a..9809b6776f 100644 --- a/rclcpp/include/rclcpp/loaned_message.hpp +++ b/rclcpp/include/rclcpp/loaned_message.hpp @@ -133,7 +133,7 @@ class LoanedMessage if (message_ == nullptr) { return; } - DestroyMessage(pub_, message_, message_allocator_); + destroy_message(pub_, message_, message_allocator_); message_ = nullptr; } @@ -178,7 +178,7 @@ class LoanedMessage return std::unique_ptr>( msg, [pub = pub_, allocator = message_allocator_](MessageT * msg_ptr) { - LoanedMessage::DestroyMessage(pub, msg_ptr, allocator); + LoanedMessage::destroy_message(pub, msg_ptr, allocator); }); } @@ -193,8 +193,9 @@ class LoanedMessage LoanedMessage(const LoanedMessage & other) = delete; private: - static void DestroyMessage( - const rclcpp::PublisherBase & pub, MessageT * msg_ptr, + static void destroy_message( + const rclcpp::PublisherBase & pub, + MessageT * msg_ptr, std::allocator allocator) { auto error_logger = rclcpp::get_logger("LoanedMessage"); diff --git a/rclcpp/test/rclcpp/test_loaned_message.cpp b/rclcpp/test/rclcpp/test_loaned_message.cpp index c5206b5d5c..5139245437 100644 --- a/rclcpp/test/rclcpp/test_loaned_message.cpp +++ b/rclcpp/test/rclcpp/test_loaned_message.cpp @@ -79,7 +79,7 @@ TEST_F(TestLoanedMessage, release) { // of the data after a call to release. } - ASSERT_EQ(42.0f, msg.get()->float64_value); + ASSERT_EQ(42.0f, msg->float64_value); SUCCEED(); } From 5d96ba0a4f641350a31993f2dfc0cea9328c2282 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Thu, 24 Sep 2020 11:02:07 +0800 Subject: [PATCH 6/9] Update description for the release method Signed-off-by: Chen Lihui --- rclcpp/include/rclcpp/loaned_message.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rclcpp/include/rclcpp/loaned_message.hpp b/rclcpp/include/rclcpp/loaned_message.hpp index 9809b6776f..e9e0b18115 100644 --- a/rclcpp/include/rclcpp/loaned_message.hpp +++ b/rclcpp/include/rclcpp/loaned_message.hpp @@ -167,6 +167,8 @@ 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. + * The memory is freed when the unique pointer goes out instead, and the unique pointer + * will keep the publisher alive until it is destroyed. * * \return std::unique_ptr to the message instance. */ From 2b7e1b3968ca3eb8fd84f550cd0cd5525bc83334 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Sat, 9 Jan 2021 22:51:40 +0800 Subject: [PATCH 7/9] update based on suggestion and revert some changes. Co-authored-by: Ivan Santiago Paunovic Signed-off-by: Chen Lihui --- rclcpp/include/rclcpp/loaned_message.hpp | 59 +++++++++++----------- rclcpp/include/rclcpp/publisher.hpp | 2 +- rclcpp/test/rclcpp/test_loaned_message.cpp | 9 ++++ 3 files changed, 40 insertions(+), 30 deletions(-) diff --git a/rclcpp/include/rclcpp/loaned_message.hpp b/rclcpp/include/rclcpp/loaned_message.hpp index e9e0b18115..b6861eaa11 100644 --- a/rclcpp/include/rclcpp/loaned_message.hpp +++ b/rclcpp/include/rclcpp/loaned_message.hpp @@ -130,10 +130,26 @@ class LoanedMessage */ virtual ~LoanedMessage() { + auto error_logger = rclcpp::get_logger("LoanedMessage"); + if (message_ == nullptr) { return; } - destroy_message(pub_, message_, message_allocator_); + + if (pub_.can_loan_messages()) { + // return allocated memory to the middleware + auto ret = + 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); + rcl_reset_error(); + } + } else { + // call destructor before deallocating + message_->~MessageT(); + message_allocator_.deallocate(message_, 1); + } message_ = nullptr; } @@ -167,8 +183,10 @@ 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. - * The memory is freed when the unique pointer goes out instead, and the unique pointer - * will keep the publisher alive until it is destroyed. + * 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, and the unique pointer will keep the publisher alive until it is destroyed. * * \return std::unique_ptr to the message instance. */ @@ -177,10 +195,17 @@ class LoanedMessage { auto msg = message_; message_ = nullptr; + + if (pub_.can_loan_messages()) { + return std::unique_ptr>(msg, [](MessageT *) {}); + } + return std::unique_ptr>( msg, - [pub = pub_, allocator = message_allocator_](MessageT * msg_ptr) { - LoanedMessage::destroy_message(pub, msg_ptr, allocator); + [ = ](MessageT * msg_ptr) { + // call destructor before deallocating + msg_ptr->~MessageT(); + message_allocator_.deallocate(msg_ptr, 1); }); } @@ -193,30 +218,6 @@ class LoanedMessage /// Deleted copy constructor to preserve memory integrity. LoanedMessage(const LoanedMessage & other) = delete; - -private: - static void destroy_message( - const rclcpp::PublisherBase & pub, - MessageT * msg_ptr, - std::allocator allocator) - { - auto error_logger = rclcpp::get_logger("LoanedMessage"); - - if (pub.can_loan_messages()) { - // return allocated memory to the middleware - auto ret = - rcl_return_loaned_message_from_publisher(pub.get_publisher_handle().get(), msg_ptr); - if (ret != RCL_RET_OK) { - RCLCPP_ERROR( - error_logger, "rcl_deallocate_loaned_message failed: %s", rcl_get_error_string().str); - rcl_reset_error(); - } - } else { - // call destructor before deallocating - msg_ptr->~MessageT(); - allocator.deallocate(msg_ptr, 1); - } - } }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index d8b572c90c..8a1c1245d5 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(loaned_msg.release().get()); } 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. diff --git a/rclcpp/test/rclcpp/test_loaned_message.cpp b/rclcpp/test/rclcpp/test_loaned_message.cpp index 5139245437..c82ff5fc7c 100644 --- a/rclcpp/test/rclcpp/test_loaned_message.cpp +++ b/rclcpp/test/rclcpp/test_loaned_message.cpp @@ -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(); } From c94cb69505941e0f8fdb0188b5a67eab5db9208e Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Tue, 12 Jan 2021 12:54:55 +0800 Subject: [PATCH 8/9] Use explicit capture Co-authored-by: Ivan Santiago Paunovic Signed-off-by: Chen Lihui --- rclcpp/include/rclcpp/loaned_message.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rclcpp/include/rclcpp/loaned_message.hpp b/rclcpp/include/rclcpp/loaned_message.hpp index b6861eaa11..1d7b0d5e4f 100644 --- a/rclcpp/include/rclcpp/loaned_message.hpp +++ b/rclcpp/include/rclcpp/loaned_message.hpp @@ -186,7 +186,7 @@ class LoanedMessage * 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, and the unique pointer will keep the publisher alive until it is destroyed. + * goes out instead. * * \return std::unique_ptr to the message instance. */ @@ -202,10 +202,10 @@ class LoanedMessage return std::unique_ptr>( msg, - [ = ](MessageT * msg_ptr) { + [allocator = message_allocator_](MessageT * msg_ptr) mutable { // call destructor before deallocating msg_ptr->~MessageT(); - message_allocator_.deallocate(msg_ptr, 1); + allocator.deallocate(msg_ptr, 1); }); } From 9867273051e915090ef7d2ec22ab1474ed2bdfe1 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Mon, 25 Jan 2021 11:26:53 +0800 Subject: [PATCH 9/9] Use unique_ptr as argument type and update exist test Signed-off-by: Chen Lihui --- rclcpp/include/rclcpp/publisher.hpp | 6 +++--- rclcpp/test/rclcpp/test_publisher.cpp | 9 +++++---- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 8a1c1245d5..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().get()); + 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_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); } }