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

rclcpp/include/rclcpp/subscription.hpp

@@ -397,9 +397,24 @@ class Subscription : public SubscriptionBase
     }
 
     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 sub_handle = this->get_subscription_handle();
+    auto loan_mutex = this->loaned_message_mutex_;
     auto sptr = std::shared_ptr<ROSMessageType>(
-      typed_message, [](ROSMessageType * msg) {(void) msg;});
+      typed_message, [sub_handle, loan_mutex](ROSMessageType * msg) {
+        if (msg) {
+          std::lock_guard<std::mutex> lock(*loan_mutex);
+          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();
+          }
+        }
+      });
 
     std::chrono::time_point<std::chrono::system_clock> now;
     if (subscription_topic_statistics_) {

rclcpp/include/rclcpp/subscription_base.hpp

@@ -650,8 +650,16 @@ class SubscriptionBase : public std::enable_shared_from_this<SubscriptionBase>
   take_dynamic_message(
     rclcpp::dynamic_typesupport::DynamicMessage & message_out,
     rclcpp::MessageInfo & message_info_out);
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<std::mutex>
+  get_loaned_message_mutex() const
+  {
+    return loaned_message_mutex_;
+  }
   // ===============================================================================================
 
+
 protected:
   template<typename EventCallbackT>
   void
@@ -688,6 +696,9 @@ 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_;
+
+  /// Mutex to protect rcl_take_loaned_message and rcl_return_loaned_message_from_subscription
+  std::shared_ptr<std::mutex> loaned_message_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

@@ -575,6 +575,8 @@ Executor::execute_subscription(const rclcpp::SubscriptionBase::SharedPtr & subsc
             subscription->get_topic_name(),
             [&]()
             {
+              std::shared_ptr<std::mutex> loan_mutex = subscription->get_loaned_message_mutex();
+              std::lock_guard<std::mutex> lock(*loan_mutex);
               rcl_ret_t ret = rcl_take_loaned_message(
                 subscription->get_subscription_handle().get(),
                 &loaned_msg,
@@ -589,19 +591,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

@@ -53,7 +53,8 @@ SubscriptionBase::SubscriptionBase(
   intra_process_subscription_id_(0),
   event_callbacks_(event_callbacks),
   type_support_(type_support_handle),
-  delivered_message_kind_(delivered_message_kind)
+  delivered_message_kind_(delivered_message_kind),
+  loaned_message_mutex_(std::make_shared<std::mutex>())
 {
   auto custom_deletor = [node_handle = this->node_handle_](rcl_subscription_t * rcl_subs)
     {
@@ -337,20 +338,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