diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 401beb0a73..cc89232898 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -597,6 +597,7 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) [&]() {return subscription->take_serialized(*serialized_msg.get(), message_info);}, [&]() { +<<<<<<< HEAD subscription->handle_serialized_message(serialized_msg, message_info); }); subscription->return_serialized_message(serialized_msg); @@ -621,6 +622,60 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) return false; } else if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); +======= + if (subscription->can_loan_messages()) { + // This is the case where a loaned message is taken from the middleware via + // inter-process communication, given to the user for their callback, + // and then returned. + void * loaned_msg = nullptr; + // TODO(wjwwood): refactor this into methods on subscription when LoanedMessage + // is extened to support subscriptions as well. + take_and_do_error_handling( + "taking a loaned message from topic", + subscription->get_topic_name(), + [&]() + { + rcl_ret_t ret = rcl_take_loaned_message( + subscription->get_subscription_handle().get(), + &loaned_msg, + &message_info.get_rmw_message_info(), + nullptr); + if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) { + return false; + } else if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + 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); + } + loaned_msg = nullptr; + } + } else { + // This case is taking a copy of the message data from the middleware via + // inter-process communication. + std::shared_ptr message = subscription->create_message(); + take_and_do_error_handling( + "taking a message from topic", + subscription->get_topic_name(), + [&]() {return subscription->take_type_erased(message.get(), message_info);}, + [&]() {subscription->handle_message(message, message_info);}); + // TODO(clalancette): In the case that the user is using the MessageMemoryPool, + // and they take a shared_ptr reference to the message in the callback, this can + // inadvertently return the message to the pool when the user is still using it. + // This is a bug that needs to be fixed in the pool, and we should probably have + // a custom deleter for the message that actually does the return_message(). + subscription->return_message(message); +>>>>>>> f294488e (Disable the loaned messages inside the executor. (#2335)) } return true; }, diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 300f465a41..ee2ec11da0 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -229,7 +229,20 @@ SubscriptionBase::setup_intra_process( bool SubscriptionBase::can_loan_messages() const { - return rcl_subscription_can_loan_messages(subscription_handle_.get()); + 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; } rclcpp::Waitable::SharedPtr