Fix loaned message ownership transfer by returning loans via shared_ptr custom deleter

rclcpp/include/rclcpp/subscription.hpp

@@ -335,16 +335,31 @@ class Subscription : public SubscriptionBase
     void * loaned_message,
     const rclcpp::MessageInfo & message_info) override
   {
+    auto typed_message = static_cast<ROSMessageType *>(loaned_message);
+
+    auto sub_handle = this->get_subscription_handle();
+    auto sptr = std::shared_ptr<ROSMessageType>(
+      typed_message, [sub_handle](ROSMessageType * msg) {
+        if (msg == nullptr) {
+          return;
+        }
+        rcl_ret_t ret = rcl_return_loaned_message_from_subscription(
+          sub_handle.get(), static_cast<void *>(msg));
+        if (RCL_RET_OK != ret) {
+          RCLCPP_ERROR(
+            rclcpp::get_logger("rclcpp"),
+            "rcl_return_loaned_message_from_subscription() failed: %s",
+            rcl_get_error_string().str);
+          rcl_reset_error();
+        }
+      });
+
     if (matches_any_intra_process_publishers(&message_info.get_rmw_message_info().publisher_gid)) {
       // In this case, the message will be delivered via intra process and
       // we should ignore this copy of the message.
       return;
     }
 
-    auto typed_message = static_cast<ROSMessageType *>(loaned_message);
-    // message is loaned, so we have to make sure that the deleter does not deallocate the message
-    auto sptr = std::shared_ptr<ROSMessageType>(
-      typed_message, [](ROSMessageType * msg) {(void) msg;});
     any_callback_.dispatch(sptr, message_info);
   }
 

rclcpp/include/rclcpp/subscription_base.hpp

@@ -542,6 +542,7 @@ class SubscriptionBase : public std::enable_shared_from_this<SubscriptionBase>
   take_dynamic_message(
     rclcpp::dynamic_typesupport::DynamicMessage & message_out,
     rclcpp::MessageInfo & message_info_out);
+
   // ===============================================================================================
 
 protected:
@@ -580,6 +581,7 @@ class SubscriptionBase : public std::enable_shared_from_this<SubscriptionBase>
   std::shared_ptr<rcl_node_t> node_handle_;
 
   std::recursive_mutex on_new_message_callback_mutex_;
+
   // It is important to declare on_new_message_callback_ before
   // subscription_handle_, so on destruction the subscription is
   // destroyed first. Otherwise, the rmw subscription callback

rclcpp/src/rclcpp/executor.cpp

@@ -613,19 +613,6 @@ Executor::execute_subscription(const rclcpp::SubscriptionBase::SharedPtr & subsc
               return true;
             },
             [&]() {subscription->handle_loaned_message(loaned_msg, message_info);});
-          if (nullptr != loaned_msg) {
-            rcl_ret_t ret = rcl_return_loaned_message_from_subscription(
-              subscription->get_subscription_handle().get(), loaned_msg);
-            if (RCL_RET_OK != ret) {
-              RCLCPP_ERROR(
-                rclcpp::get_logger("rclcpp"),
-                "rcl_return_loaned_message_from_subscription() failed for subscription on topic "
-                "'%s': %s",
-                subscription->get_topic_name(), rcl_get_error_string().str);
-              rcl_reset_error();
-            }
-            loaned_msg = nullptr;
-          }
         } else {
           // This case is taking a copy of the message data from the middleware via
           // inter-process communication.

rclcpp/src/rclcpp/subscription_base.cpp

@@ -99,6 +99,18 @@ SubscriptionBase::SubscriptionBase(
 
 SubscriptionBase::~SubscriptionBase()
 {
+  try {
+    this->clear_on_new_message_callback();
+  } catch (const std::exception & exc) {
+    RCLCPP_ERROR(
+      rclcpp::get_logger("rclcpp"),
+      "Failed to clear on new message callback in destructor: %s",
+      exc.what());
+  } catch (...) {
+    RCLCPP_ERROR(
+      rclcpp::get_logger("rclcpp"),
+      "Failed to clear on new message callback in destructor");
+  }
   if (!use_intra_process_) {
     return;
   }
@@ -341,20 +353,7 @@ SubscriptionBase::setup_intra_process(
 bool
 SubscriptionBase::can_loan_messages() const
 {
-  bool retval = rcl_subscription_can_loan_messages(subscription_handle_.get());
-  if (retval) {
-    // TODO(clalancette): The loaned message interface is currently not safe to use with
-    // shared_ptr callbacks.  If a user takes a copy of the shared_ptr, it can get freed from
-    // underneath them via rcl_return_loaned_message_from_subscription().  The correct solution is
-    // to return the loaned message in a custom deleter, but that needs to be carefully handled
-    // with locking.  Warn the user about this until we fix it.
-    RCLCPP_WARN_ONCE(
-      this->node_logger_,
-      "Loaned messages are only safe with const ref subscription callbacks. "
-      "If you are using any other kind of subscriptions, "
-      "set the ROS_DISABLE_LOANED_MESSAGES environment variable to 1 (the default).");
-  }
-  return retval;
+  return rcl_subscription_can_loan_messages(subscription_handle_.get());
 }
 
 rclcpp::Waitable::SharedPtr

rclcpp/test/rclcpp/test_subscription.cpp

@@ -363,16 +363,6 @@ TEST_F(TestSubscription, rcl_subscription_get_publisher_count_error) {
   EXPECT_THROW(sub->get_publisher_count(), rclcpp::exceptions::RCLError);
 }
 
-TEST_F(TestSubscription, handle_loaned_message) {
-  initialize();
-  auto callback = [](std::shared_ptr<const test_msgs::msg::Empty>) {};
-  auto sub = node_->create_subscription<test_msgs::msg::Empty>("topic", 10, callback);
-
-  test_msgs::msg::Empty msg;
-  rclcpp::MessageInfo message_info;
-  EXPECT_NO_THROW(sub->handle_loaned_message(&msg, message_info));
-}
-
 /*
    Testing on_new_message callbacks.
  */