diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 1ed83f5c1e..0c5dc23b5d 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/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(loaned_message); + + auto sub_handle = this->get_subscription_handle(); + auto sptr = std::shared_ptr( + 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(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(loaned_message); - // message is loaned, so we have to make sure that the deleter does not deallocate the message - auto sptr = std::shared_ptr( - typed_message, [](ROSMessageType * msg) {(void) msg;}); any_callback_.dispatch(sptr, message_info); } diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 3a6f61fe99..b8d0132c8f 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -542,6 +542,7 @@ class SubscriptionBase : public std::enable_shared_from_this 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 std::shared_ptr 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 diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 15ba78b104..35c560a98f 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/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. diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 2ea4664f52..2dbab07ce4 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/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 diff --git a/rclcpp/test/rclcpp/test_subscription.cpp b/rclcpp/test/rclcpp/test_subscription.cpp index fb7a57a1f7..b90db762ff 100644 --- a/rclcpp/test/rclcpp/test_subscription.cpp +++ b/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) {}; - auto sub = node_->create_subscription("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. */