Destroy msg extracted from LoanedMessage. (#1305)

* Destroy msg extracted from LoanedMessage.

Signed-off-by: Chen Lihui <Lihui.Chen@sony.com>

* Remove the release method of LoadedMessage and a related test case

Signed-off-by: Chen Lihui <Lihui.Chen@sony.com>

* Revert "Remove the release method of LoadedMessage and a related test case"

This reverts commit b982525.

Signed-off-by: Chen Lihui <Lihui.Chen@sony.com>

* Use unique_ptr as return type for release method of LoanedMessage

Signed-off-by: Chen Lihui <Lihui.Chen@sony.com>

* Update based on review.

Co-authored-by: William Woodall <william@osrfoundation.org>
Signed-off-by: Chen Lihui <Lihui.Chen@sony.com>

* Update description for the release method

Signed-off-by: Chen Lihui <Lihui.Chen@sony.com>

* update based on suggestion and revert some changes.

Co-authored-by: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>
Signed-off-by: Chen Lihui <Lihui.Chen@sony.com>

* Use explicit capture

Co-authored-by: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>
Signed-off-by: Chen Lihui <lihui.chen@sony.com>

* Use unique_ptr as argument type and update exist test

Signed-off-by: Chen Lihui <lihui.chen@sony.com>

Co-authored-by: William Woodall <william@osrfoundation.org>
Co-authored-by: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>

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<MessageT, std::function<void(MessageT *)>>
+  release()
   {
     auto msg = message_;
     message_ = nullptr;
-    return msg;
+
+    if (pub_.can_loan_messages()) {
+      return std::unique_ptr<MessageT, std::function<void(MessageT *)>>(msg, [](MessageT *) {});
+    }
+
+    return std::unique_ptr<MessageT, std::function<void(MessageT *)>>(
+      msg,
+      [allocator = message_allocator_](MessageT * msg_ptr) mutable {
+        // call destructor before deallocating
+        msg_ptr->~MessageT();
+        allocator.deallocate(msg_ptr, 1);
+      });
   }
 
 protected:

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<MessageT, std::function<void(MessageT *)>> 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

rclcpp/test/rclcpp/test_loaned_message.cpp

@@ -67,7 +67,7 @@ TEST_F(TestLoanedMessage, release) {
   auto node = std::make_shared<rclcpp::Node>("loaned_message_test_node");
   auto pub = node->create_publisher<MessageT>("loaned_message_test_topic", 1);
 
-  MessageT * msg = nullptr;
+  std::unique_ptr<MessageT, std::function<void(MessageT *)>> 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();
 }
 

rclcpp/test/rclcpp/test_publisher.cpp

@@ -462,9 +462,9 @@ class TestPublisherProtectedMethods : public rclcpp::Publisher<MessageT, Allocat
 public:
   using rclcpp::Publisher<MessageT, AllocatorT>::Publisher;
 
-  void publish_loaned_message(MessageT * msg)
+  void publish_loaned_message(rclcpp::LoanedMessage<MessageT, AllocatorT> && 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<test_msgs::msg::Empty, std::allocator<void>, PublisherT>("topic", 10);
 
-  auto msg = std::make_shared<test_msgs::msg::Empty>();
+  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);
   }
 }