From 6b321edb4fabcc57f666f77a4dc091e95948070b Mon Sep 17 00:00:00 2001 From: Gonzo <42421541+gonzodepedro@users.noreply.github.com> Date: Fri, 7 Jan 2022 16:45:06 -0300 Subject: [PATCH] Add non transform capabilities for intra-process (#1849) That is, make it so that if both a publisher and a subscriber using intra-process and using TypeAdaptation are the same type, we don't need to convert to a ROS Message type on the publisher and from a ROS Message type before delivering to the subscriber. Instead, we store the original message type in the subscription buffer, and deliver it straight through to the subscriber. This has two main benefits: 1. It is better for performance; we are eliding unnecessary work. 2. We can use TypeAdaptation to pass custom handles (like hardware accelerator handles) between a publisher and subscriber. That means we can avoid unnecessary overhead when using a hardware accelerator, like synchronizing between the hardware accelerator and the main CPU. The main way this is accomplished is by causing the SubscriptionIntraProcessBuffer to store messages of the Subscribed Type (which in the case of a ROS message type subscriber is a ROS message type, but in the case of a TypeAdapter type is the custom type). When a publish with a Type Adapted type happens, it delays conversion of the message, and passes the message type from the user down into the IntraProcessManager. The IntraProcessManager checks to see if it can cast the SubscriptionBuffer to the custom type being published first. If it can, then it knows it can deliver the message directly into the SubscriptionBuffer with no conversion necessary. If it can't, it then sees if the SubscriptionBuffer is of a ROS message type. If it is, then it performs the conversion as necessary, and then stores it. In all cases, the Subscription is then triggered underneath so that the message will eventually be delivered by an executor. We also add tests to ensure that in the case where the publisher and subscriber and using the same type adapter, no conversion happens when delivering the message. Signed-off-by: Chris Lalancette Signed-off-by: Gonzalo de Pedro --- .../rclcpp/any_subscription_callback.hpp | 140 ++- .../experimental/intra_process_manager.hpp | 182 +++- .../ros_message_intra_process_buffer.hpp | 68 ++ .../subscription_intra_process.hpp | 47 +- .../subscription_intra_process_base.hpp | 16 +- .../subscription_intra_process_buffer.hpp | 103 +- rclcpp/include/rclcpp/publisher.hpp | 87 +- rclcpp/include/rclcpp/subscription.hpp | 12 +- rclcpp/test/rclcpp/CMakeLists.txt | 12 + .../rclcpp/test_any_subscription_callback.cpp | 114 +- .../rclcpp/test_intra_process_manager.cpp | 38 +- .../test_publisher_with_type_adapter.cpp | 10 + ...ption_publisher_with_same_type_adapter.cpp | 991 ++++++++++++++++++ 13 files changed, 1603 insertions(+), 217 deletions(-) create mode 100644 rclcpp/include/rclcpp/experimental/ros_message_intra_process_buffer.hpp create mode 100644 rclcpp/test/rclcpp/test_subscription_publisher_with_same_type_adapter.cpp diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp index 62e116af56..56af35c518 100644 --- a/rclcpp/include/rclcpp/any_subscription_callback.hpp +++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp @@ -476,6 +476,22 @@ class AnySubscriptionCallback } } + std::unique_ptr + convert_custom_type_to_ros_message_unique_ptr(const SubscribedType & msg) + { + if constexpr (rclcpp::TypeAdapter::is_specialized::value) { + auto ptr = ROSMessageTypeAllocatorTraits::allocate(ros_message_type_allocator_, 1); + ROSMessageTypeAllocatorTraits::construct(ros_message_type_allocator_, ptr); + rclcpp::TypeAdapter::convert_to_ros_message(msg, *ptr); + return std::unique_ptr(ptr, ros_message_type_deleter_); + } else { + static_assert( + !sizeof(MessageT *), + "convert_custom_type_to_ros_message_unique_ptr() " + "unexpectedly called without specialized TypeAdapter"); + } + } + // Dispatch when input is a ros message and the output could be anything. void dispatch( @@ -658,7 +674,7 @@ class AnySubscriptionCallback void dispatch_intra_process( - std::shared_ptr message, + std::shared_ptr message, const rclcpp::MessageInfo & message_info) { TRACEPOINT(callback_start, static_cast(this), true); @@ -677,65 +693,89 @@ class AnySubscriptionCallback // conditions for custom type if constexpr (is_ta && std::is_same_v) { - auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message); - callback(*local_message); + callback(*message); } else if constexpr (is_ta && std::is_same_v) { // NOLINT - auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message); - callback(*local_message, message_info); + callback(*message, message_info); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v )) { - callback(convert_ros_message_to_custom_type_unique_ptr(*message)); + callback(create_custom_unique_ptr_from_custom_shared_ptr_message(message)); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v )) { - callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info); + callback(create_custom_unique_ptr_from_custom_shared_ptr_message(message), message_info); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v )) { - callback(convert_ros_message_to_custom_type_unique_ptr(*message)); + callback(message); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v )) { - callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info); + callback(message, message_info); } // conditions for ros message type - else if constexpr (std::is_same_v) { // NOLINT - callback(*message); - } else if constexpr (std::is_same_v) { - callback(*message, message_info); + else if constexpr (std::is_same_v) { // NOLINT[readability/braces] + if constexpr (is_ta) { + auto local = convert_custom_type_to_ros_message_unique_ptr(*message); + callback(*local); + } else { + callback(*message); + } + } else if constexpr (std::is_same_v) { // NOLINT[readability/braces] + if constexpr (is_ta) { + auto local = convert_custom_type_to_ros_message_unique_ptr(*message); + callback(*local, message_info); + } else { + callback(*message, message_info); + } } else if constexpr ( // NOLINT[readability/braces] std::is_same_v|| std::is_same_v) { - callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message)); + if constexpr (is_ta) { + callback(convert_custom_type_to_ros_message_unique_ptr(*message)); + } else { + callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message)); + } } else if constexpr ( // NOLINT[readability/braces] std::is_same_v|| std::is_same_v) { - callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message), message_info); + if constexpr (is_ta) { + callback(convert_custom_type_to_ros_message_unique_ptr(*message), message_info); + } else { + callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message), message_info); + } } else if constexpr ( // NOLINT[readability/braces] std::is_same_v|| std::is_same_v) { - callback(message); + if constexpr (is_ta) { + callback(convert_custom_type_to_ros_message_unique_ptr(*message)); + } else { + callback(message); + } } else if constexpr ( // NOLINT[readability/braces] std::is_same_v|| std::is_same_v) { - callback(message, message_info); + if constexpr (is_ta) { + callback(convert_custom_type_to_ros_message_unique_ptr(*message), message_info); + } else { + callback(message, message_info); + } } // condition to catch SerializedMessage types else if constexpr ( // NOLINT[readability/braces] @@ -764,7 +804,7 @@ class AnySubscriptionCallback void dispatch_intra_process( - std::unique_ptr message, + std::unique_ptr message, const rclcpp::MessageInfo & message_info) { TRACEPOINT(callback_start, static_cast(this), true); @@ -778,70 +818,98 @@ class AnySubscriptionCallback // Dispatch. std::visit( [&message, &message_info, this](auto && callback) { + // clang complains that 'this' lambda capture is unused, which is true + // in *some* specializations of this template, but not others. Just + // quiet it down. + (void)this; + using T = std::decay_t; static constexpr bool is_ta = rclcpp::TypeAdapter::is_specialized::value; // conditions for custom type if constexpr (is_ta && std::is_same_v) { - auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message); - callback(*local_message); + callback(*message); } else if constexpr (is_ta && std::is_same_v) { // NOLINT - auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message); - callback(*local_message, message_info); + callback(*message, message_info); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| - std::is_same_v - )) + std::is_same_v)) { - callback(convert_ros_message_to_custom_type_unique_ptr(*message)); + callback(std::move(message)); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v )) { - callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info); + callback(std::move(message), message_info); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v )) { - callback(convert_ros_message_to_custom_type_unique_ptr(*message)); + callback(std::move(message)); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v )) { - callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info); + callback(std::move(message), message_info); } // conditions for ros message type - else if constexpr (std::is_same_v) { // NOLINT - callback(*message); - } else if constexpr (std::is_same_v) { - callback(*message, message_info); + else if constexpr (std::is_same_v) { // NOLINT[readability/braces] + if constexpr (is_ta) { + auto local = convert_custom_type_to_ros_message_unique_ptr(*message); + callback(*local); + } else { + callback(*message); + } + } else if constexpr (std::is_same_v) { // NOLINT[readability/braces] + if constexpr (is_ta) { + auto local = convert_custom_type_to_ros_message_unique_ptr(*message); + callback(*local, message_info); + } else { + callback(*message, message_info); + } } else if constexpr ( // NOLINT[readability/braces] std::is_same_v|| std::is_same_v) { - callback(std::move(message)); + if constexpr (is_ta) { + callback(convert_custom_type_to_ros_message_unique_ptr(*message)); + } else { + callback(std::move(message)); + } } else if constexpr ( // NOLINT[readability/braces] std::is_same_v|| std::is_same_v) { - callback(std::move(message), message_info); + if constexpr (is_ta) { + callback(convert_custom_type_to_ros_message_unique_ptr(*message), message_info); + } else { + callback(std::move(message), message_info); + } } else if constexpr ( // NOLINT[readability/braces] std::is_same_v|| std::is_same_v) { - callback(std::move(message)); + if constexpr (is_ta) { + callback(convert_custom_type_to_ros_message_unique_ptr(*message)); + } else { + callback(std::move(message)); + } } else if constexpr ( // NOLINT[readability/braces] std::is_same_v|| std::is_same_v) { - callback(std::move(message), message_info); + if constexpr (is_ta) { + callback(convert_custom_type_to_ros_message_unique_ptr(*message), message_info); + } else { + callback(std::move(message), message_info); + } } // condition to catch SerializedMessage types else if constexpr ( // NOLINT[readability/braces] diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index 16c6039b76..0950669e9a 100644 --- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -29,8 +29,10 @@ #include #include #include +#include #include "rclcpp/allocator/allocator_deleter.hpp" +#include "rclcpp/experimental/ros_message_intra_process_buffer.hpp" #include "rclcpp/experimental/subscription_intra_process.hpp" #include "rclcpp/experimental/subscription_intra_process_base.hpp" #include "rclcpp/experimental/subscription_intra_process_buffer.hpp" @@ -38,6 +40,7 @@ #include "rclcpp/logging.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/publisher_base.hpp" +#include "rclcpp/type_adapter.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp @@ -177,8 +180,10 @@ class IntraProcessManager */ template< typename MessageT, - typename Alloc = std::allocator, - typename Deleter = std::default_delete> + typename ROSMessageType, + typename Alloc, + typename Deleter = std::default_delete + > void do_intra_process_publish( uint64_t intra_process_publisher_id, @@ -204,7 +209,7 @@ class IntraProcessManager // None of the buffers require ownership, so we promote the pointer std::shared_ptr msg = std::move(message); - this->template add_shared_msg_to_buffers( + this->template add_shared_msg_to_buffers( msg, sub_ids.take_shared_subscriptions); } else if (!sub_ids.take_ownership_subscriptions.empty() && // NOLINT sub_ids.take_shared_subscriptions.size() <= 1) @@ -218,8 +223,7 @@ class IntraProcessManager concatenated_vector.end(), sub_ids.take_ownership_subscriptions.begin(), sub_ids.take_ownership_subscriptions.end()); - - this->template add_owned_msg_to_buffers( + this->template add_owned_msg_to_buffers( std::move(message), concatenated_vector, allocator); @@ -230,17 +234,19 @@ class IntraProcessManager // for the buffers that do not require ownership auto shared_msg = std::allocate_shared(allocator, *message); - this->template add_shared_msg_to_buffers( + this->template add_shared_msg_to_buffers( shared_msg, sub_ids.take_shared_subscriptions); - this->template add_owned_msg_to_buffers( + this->template add_owned_msg_to_buffers( std::move(message), sub_ids.take_ownership_subscriptions, allocator); } } template< typename MessageT, - typename Alloc = std::allocator, - typename Deleter = std::default_delete> + typename ROSMessageType, + typename Alloc, + typename Deleter = std::default_delete + > std::shared_ptr do_intra_process_publish_and_return_shared( uint64_t intra_process_publisher_id, @@ -266,7 +272,7 @@ class IntraProcessManager // If there are no owning, just convert to shared. std::shared_ptr shared_msg = std::move(message); if (!sub_ids.take_shared_subscriptions.empty()) { - this->template add_shared_msg_to_buffers( + this->template add_shared_msg_to_buffers( shared_msg, sub_ids.take_shared_subscriptions); } return shared_msg; @@ -276,17 +282,16 @@ class IntraProcessManager auto shared_msg = std::allocate_shared(allocator, *message); if (!sub_ids.take_shared_subscriptions.empty()) { - this->template add_shared_msg_to_buffers( + this->template add_shared_msg_to_buffers( shared_msg, sub_ids.take_shared_subscriptions); } if (!sub_ids.take_ownership_subscriptions.empty()) { - this->template add_owned_msg_to_buffers( + this->template add_owned_msg_to_buffers( std::move(message), sub_ids.take_ownership_subscriptions, allocator); } - return shared_msg; } } @@ -339,41 +344,83 @@ class IntraProcessManager template< typename MessageT, typename Alloc, - typename Deleter> + typename Deleter, + typename ROSMessageType> void add_shared_msg_to_buffers( std::shared_ptr message, std::vector subscription_ids) { + using ROSMessageTypeAllocatorTraits = allocator::AllocRebind; + using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type; + using ROSMessageTypeDeleter = allocator::Deleter; + + using PublishedType = typename rclcpp::TypeAdapter::custom_type; + using PublishedTypeAllocatorTraits = allocator::AllocRebind; + using PublishedTypeAllocator = typename PublishedTypeAllocatorTraits::allocator_type; + using PublishedTypeDeleter = allocator::Deleter; + for (auto id : subscription_ids) { auto subscription_it = subscriptions_.find(id); if (subscription_it == subscriptions_.end()) { throw std::runtime_error("subscription has unexpectedly gone out of scope"); } auto subscription_base = subscription_it->second.lock(); - if (subscription_base) { - auto subscription = std::dynamic_pointer_cast< - rclcpp::experimental::SubscriptionIntraProcessBuffer - >(subscription_base); - if (nullptr == subscription) { - throw std::runtime_error( - "failed to dynamic cast SubscriptionIntraProcessBase to " - "SubscriptionIntraProcessBuffer, which " - "can happen when the publisher and subscription use different " - "allocator types, which is not supported"); - } + if (subscription_base == nullptr) { + subscriptions_.erase(id); + continue; + } + + auto subscription = std::dynamic_pointer_cast< + rclcpp::experimental::SubscriptionIntraProcessBuffer + >(subscription_base); + if (subscription != nullptr) { + subscription->provide_intra_process_data(message); + continue; + } - subscription->provide_intra_process_message(message); + auto ros_message_subscription = std::dynamic_pointer_cast< + rclcpp::experimental::SubscriptionROSMsgIntraProcessBuffer + >(subscription_base); + if (nullptr == ros_message_subscription) { + throw std::runtime_error( + "failed to dynamic cast SubscriptionIntraProcessBase to " + "SubscriptionIntraProcessBuffer, or to " + "SubscriptionROSMsgIntraProcessBuffer which can happen when the publisher and " + "subscription use different allocator types, which is not supported"); + } + + if constexpr (rclcpp::TypeAdapter::is_specialized::value) { + ROSMessageType ros_msg; + rclcpp::TypeAdapter::convert_to_ros_message(*message, ros_msg); + ros_message_subscription->provide_intra_process_message( + std::make_shared(ros_msg)); } else { - subscriptions_.erase(id); + if constexpr (std::is_same::value) { + ros_message_subscription->provide_intra_process_message(message); + } else { + if constexpr (std::is_same::ros_message_type, ROSMessageType>::value) + { + ROSMessageType ros_msg; + rclcpp::TypeAdapter::convert_to_ros_message( + *message, ros_msg); + ros_message_subscription->provide_intra_process_message( + std::make_shared(ros_msg)); + } + } } } } template< typename MessageT, - typename Alloc = std::allocator, - typename Deleter = std::default_delete> + typename Alloc, + typename Deleter, + typename ROSMessageType> void add_owned_msg_to_buffers( std::unique_ptr message, @@ -383,39 +430,84 @@ class IntraProcessManager using MessageAllocTraits = allocator::AllocRebind; using MessageUniquePtr = std::unique_ptr; + using ROSMessageTypeAllocatorTraits = allocator::AllocRebind; + using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type; + using ROSMessageTypeDeleter = allocator::Deleter; + + using PublishedType = typename rclcpp::TypeAdapter::custom_type; + using PublishedTypeAllocatorTraits = allocator::AllocRebind; + using PublishedTypeAllocator = typename PublishedTypeAllocatorTraits::allocator_type; + using PublishedTypeDeleter = allocator::Deleter; + for (auto it = subscription_ids.begin(); it != subscription_ids.end(); it++) { auto subscription_it = subscriptions_.find(*it); if (subscription_it == subscriptions_.end()) { throw std::runtime_error("subscription has unexpectedly gone out of scope"); } auto subscription_base = subscription_it->second.lock(); - if (subscription_base) { - auto subscription = std::dynamic_pointer_cast< - rclcpp::experimental::SubscriptionIntraProcessBuffer - >(subscription_base); - if (nullptr == subscription) { - throw std::runtime_error( - "failed to dynamic cast SubscriptionIntraProcessBase to " - "SubscriptionIntraProcessBuffer, which " - "can happen when the publisher and subscription use different " - "allocator types, which is not supported"); - } + if (subscription_base == nullptr) { + subscriptions_.erase(subscription_it); + continue; + } + auto subscription = std::dynamic_pointer_cast< + rclcpp::experimental::SubscriptionIntraProcessBuffer + >(subscription_base); + if (subscription != nullptr) { if (std::next(it) == subscription_ids.end()) { // If this is the last subscription, give up ownership - subscription->provide_intra_process_message(std::move(message)); + subscription->provide_intra_process_data(std::move(message)); } else { // Copy the message since we have additional subscriptions to serve - MessageUniquePtr copy_message; Deleter deleter = message.get_deleter(); auto ptr = MessageAllocTraits::allocate(allocator, 1); MessageAllocTraits::construct(allocator, ptr, *message); - copy_message = MessageUniquePtr(ptr, deleter); - subscription->provide_intra_process_message(std::move(copy_message)); + subscription->provide_intra_process_data(std::move(MessageUniquePtr(ptr, deleter))); } + + continue; + } + + auto ros_message_subscription = std::dynamic_pointer_cast< + rclcpp::experimental::SubscriptionROSMsgIntraProcessBuffer + >(subscription_base); + if (nullptr == ros_message_subscription) { + throw std::runtime_error( + "failed to dynamic cast SubscriptionIntraProcessBase to " + "SubscriptionIntraProcessBuffer, or to " + "SubscriptionROSMsgIntraProcessBuffer which can happen when the publisher and " + "subscription use different allocator types, which is not supported"); + } + + if constexpr (rclcpp::TypeAdapter::is_specialized::value) { + ROSMessageTypeAllocator ros_message_alloc(allocator); + auto ptr = ros_message_alloc.allocate(1); + ros_message_alloc.construct(ptr); + ROSMessageTypeDeleter deleter; + allocator::set_allocator_for_deleter(&deleter, &allocator); + rclcpp::TypeAdapter::convert_to_ros_message(*message, *ptr); + auto ros_msg = std::unique_ptr(ptr, deleter); + ros_message_subscription->provide_intra_process_message(std::move(ros_msg)); } else { - subscriptions_.erase(subscription_it); + if constexpr (std::is_same::value) { + if (std::next(it) == subscription_ids.end()) { + // If this is the last subscription, give up ownership + ros_message_subscription->provide_intra_process_message(std::move(message)); + } else { + // Copy the message since we have additional subscriptions to serve + Deleter deleter = message.get_deleter(); + allocator::set_allocator_for_deleter(&deleter, &allocator); + auto ptr = MessageAllocTraits::allocate(allocator, 1); + MessageAllocTraits::construct(allocator, ptr, *message); + + ros_message_subscription->provide_intra_process_message( + std::move(MessageUniquePtr(ptr, deleter))); + } + } } } } diff --git a/rclcpp/include/rclcpp/experimental/ros_message_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/ros_message_intra_process_buffer.hpp new file mode 100644 index 0000000000..7b8b3b833d --- /dev/null +++ b/rclcpp/include/rclcpp/experimental/ros_message_intra_process_buffer.hpp @@ -0,0 +1,68 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__EXPERIMENTAL__ROS_MESSAGE_INTRA_PROCESS_BUFFER_HPP_ +#define RCLCPP__EXPERIMENTAL__ROS_MESSAGE_INTRA_PROCESS_BUFFER_HPP_ + +#include +#include + +#include "rcl/error_handling.h" + +#include "rclcpp/any_subscription_callback.hpp" +#include "rclcpp/context.hpp" +#include "rclcpp/experimental/subscription_intra_process_base.hpp" +#include "tracetools/tracetools.h" + +namespace rclcpp +{ +namespace experimental +{ + +template< + typename RosMessageT, + typename Alloc = std::allocator, + typename Deleter = std::default_delete +> +class SubscriptionROSMsgIntraProcessBuffer : public SubscriptionIntraProcessBase +{ +public: + using ROSMessageTypeAllocatorTraits = allocator::AllocRebind; + using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type; + using ROSMessageTypeDeleter = allocator::Deleter; + + using ConstMessageSharedPtr = std::shared_ptr; + using MessageUniquePtr = std::unique_ptr; + + SubscriptionROSMsgIntraProcessBuffer( + rclcpp::Context::SharedPtr context, + const std::string & topic_name, + const rclcpp::QoS & qos_profile) + : SubscriptionIntraProcessBase(context, topic_name, qos_profile) + {} + + virtual ~SubscriptionROSMsgIntraProcessBuffer() + {} + + virtual void + provide_intra_process_message(ConstMessageSharedPtr message) = 0; + + virtual void + provide_intra_process_message(MessageUniquePtr message) = 0; +}; + +} // namespace experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__ROS_MESSAGE_INTRA_PROCESS_BUFFER_HPP_ diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp index f174cd5c07..54857562d0 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp @@ -43,40 +43,47 @@ namespace experimental template< typename MessageT, - typename Alloc = std::allocator, - typename Deleter = std::default_delete, - typename CallbackMessageT = MessageT> + typename SubscribedType, + typename SubscribedTypeAlloc = std::allocator, + typename SubscribedTypeDeleter = std::default_delete, + typename ROSMessageType = SubscribedType, + typename Alloc = std::allocator +> class SubscriptionIntraProcess : public SubscriptionIntraProcessBuffer< - MessageT, - Alloc, - Deleter + SubscribedType, + SubscribedTypeAlloc, + SubscribedTypeDeleter, + ROSMessageType > { using SubscriptionIntraProcessBufferT = SubscriptionIntraProcessBuffer< - MessageT, - Alloc, - Deleter + SubscribedType, + SubscribedTypeAlloc, + SubscribedTypeDeleter, + ROSMessageType >; public: RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcess) - using MessageAllocTraits = typename SubscriptionIntraProcessBufferT::MessageAllocTraits; - using MessageAlloc = typename SubscriptionIntraProcessBufferT::MessageAlloc; - using ConstMessageSharedPtr = typename SubscriptionIntraProcessBufferT::ConstMessageSharedPtr; - using MessageUniquePtr = typename SubscriptionIntraProcessBufferT::MessageUniquePtr; + using MessageAllocTraits = + typename SubscriptionIntraProcessBufferT::SubscribedTypeAllocatorTraits; + using MessageAlloc = typename SubscriptionIntraProcessBufferT::SubscribedTypeAllocator; + using ConstMessageSharedPtr = typename SubscriptionIntraProcessBufferT::ConstDataSharedPtr; + using MessageUniquePtr = typename SubscriptionIntraProcessBufferT::SubscribedTypeUniquePtr; using BufferUniquePtr = typename SubscriptionIntraProcessBufferT::BufferUniquePtr; SubscriptionIntraProcess( - AnySubscriptionCallback callback, + AnySubscriptionCallback callback, std::shared_ptr allocator, rclcpp::Context::SharedPtr context, const std::string & topic_name, const rclcpp::QoS & qos_profile, rclcpp::IntraProcessBufferType buffer_type) - : SubscriptionIntraProcessBuffer( - allocator, + : SubscriptionIntraProcessBuffer( + std::make_shared(*allocator), context, topic_name, qos_profile, @@ -98,7 +105,7 @@ class SubscriptionIntraProcess virtual ~SubscriptionIntraProcess() = default; std::shared_ptr - take_data() + take_data() override { ConstMessageSharedPtr shared_msg; MessageUniquePtr unique_msg; @@ -115,9 +122,9 @@ class SubscriptionIntraProcess ); } - void execute(std::shared_ptr & data) + void execute(std::shared_ptr & data) override { - execute_impl(data); + execute_impl(data); } protected: @@ -154,7 +161,7 @@ class SubscriptionIntraProcess shared_ptr.reset(); } - AnySubscriptionCallback any_callback_; + AnySubscriptionCallback any_callback_; }; } // namespace experimental diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index ba575c763c..f0ee39ce47 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -52,21 +52,11 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable RCLCPP_PUBLIC size_t - get_number_of_ready_guard_conditions() {return 1;} + get_number_of_ready_guard_conditions() override {return 1;} RCLCPP_PUBLIC void - add_to_wait_set(rcl_wait_set_t * wait_set); - - virtual bool - is_ready(rcl_wait_set_t * wait_set) = 0; - - virtual - std::shared_ptr - take_data() = 0; - - virtual void - execute(std::shared_ptr & data) = 0; + add_to_wait_set(rcl_wait_set_t * wait_set) override; virtual bool use_take_shared_method() const = 0; @@ -83,10 +73,10 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable std::recursive_mutex reentrant_mutex_; rclcpp::GuardCondition gc_; -private: virtual void trigger_guard_condition() = 0; +private: std::string topic_name_; QoS qos_profile_; }; diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp index 3ec335f9f6..2e20c3de9b 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp @@ -20,8 +20,8 @@ #include #include #include -#include #include +#include #include #include "rcl/error_handling.h" @@ -30,6 +30,7 @@ #include "rclcpp/experimental/buffers/intra_process_buffer.hpp" #include "rclcpp/experimental/create_intra_process_buffer.hpp" #include "rclcpp/experimental/subscription_intra_process_base.hpp" +#include "rclcpp/experimental/ros_message_intra_process_buffer.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/waitable.hpp" @@ -41,24 +42,39 @@ namespace experimental { template< - typename MessageT, - typename Alloc = std::allocator, - typename Deleter = std::default_delete + typename SubscribedType, + typename Alloc = std::allocator, + typename Deleter = std::default_delete, + /// MessageT::ros_message_type if MessageT is a TypeAdapter, + /// otherwise just MessageT. + typename ROSMessageType = SubscribedType > -class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase +class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuffer::allocator_type, + allocator::Deleter::allocator_type, + ROSMessageType>> { public: RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcessBuffer) - using MessageAllocTraits = allocator::AllocRebind; - using MessageAlloc = typename MessageAllocTraits::allocator_type; - using ConstMessageSharedPtr = std::shared_ptr; - using MessageUniquePtr = std::unique_ptr; + using SubscribedTypeAllocatorTraits = allocator::AllocRebind; + using SubscribedTypeAllocator = typename SubscribedTypeAllocatorTraits::allocator_type; + using SubscribedTypeDeleter = allocator::Deleter; + + using ROSMessageTypeAllocatorTraits = allocator::AllocRebind; + using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type; + using ROSMessageTypeDeleter = allocator::Deleter; + + using ConstMessageSharedPtr = std::shared_ptr; + using MessageUniquePtr = std::unique_ptr; + + using ConstDataSharedPtr = std::shared_ptr; + using SubscribedTypeUniquePtr = std::unique_ptr; using BufferUniquePtr = typename rclcpp::experimental::buffers::IntraProcessBuffer< - MessageT, + SubscribedType, Alloc, - Deleter + SubscribedTypeDeleter >::UniquePtr; SubscriptionIntraProcessBuffer( @@ -67,50 +83,97 @@ class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase const std::string & topic_name, const rclcpp::QoS & qos_profile, rclcpp::IntraProcessBufferType buffer_type) - : SubscriptionIntraProcessBase(context, topic_name, qos_profile) + : SubscriptionROSMsgIntraProcessBuffer( + context, topic_name, qos_profile), + subscribed_type_allocator_(*allocator) { + allocator::set_allocator_for_deleter(&subscribed_type_deleter_, &subscribed_type_allocator_); + // Create the intra-process buffer. - buffer_ = rclcpp::experimental::create_intra_process_buffer( + buffer_ = rclcpp::experimental::create_intra_process_buffer( buffer_type, qos_profile, - allocator); + std::make_shared(subscribed_type_allocator_)); } bool - is_ready(rcl_wait_set_t * wait_set) + is_ready(rcl_wait_set_t * wait_set) override { (void) wait_set; return buffer_->has_data(); } + SubscribedTypeUniquePtr + convert_ros_message_to_subscribed_type_unique_ptr(const ROSMessageType & msg) + { + if constexpr (!std::is_same::value) { + auto ptr = SubscribedTypeAllocatorTraits::allocate(subscribed_type_allocator_, 1); + SubscribedTypeAllocatorTraits::construct(subscribed_type_allocator_, ptr); + rclcpp::TypeAdapter::convert_to_custom(msg, *ptr); + return SubscribedTypeUniquePtr(ptr, subscribed_type_deleter_); + } else { + throw std::runtime_error( + "convert_ros_message_to_subscribed_type_unique_ptr " + "unexpectedly called without TypeAdapter"); + } + } + + void + provide_intra_process_message(ConstMessageSharedPtr message) override + { + if constexpr (std::is_same::value) { + buffer_->add_shared(std::move(message)); + trigger_guard_condition(); + } else { + buffer_->add_shared(convert_ros_message_to_subscribed_type_unique_ptr(*message)); + trigger_guard_condition(); + } + } + + void + provide_intra_process_message(MessageUniquePtr message) override + { + if constexpr (std::is_same::value) { + buffer_->add_unique(std::move(message)); + trigger_guard_condition(); + } else { + buffer_->add_unique(convert_ros_message_to_subscribed_type_unique_ptr(*message)); + trigger_guard_condition(); + } + } + void - provide_intra_process_message(ConstMessageSharedPtr message) + provide_intra_process_data(ConstDataSharedPtr message) { buffer_->add_shared(std::move(message)); trigger_guard_condition(); } void - provide_intra_process_message(MessageUniquePtr message) + provide_intra_process_data(SubscribedTypeUniquePtr message) { buffer_->add_unique(std::move(message)); trigger_guard_condition(); } bool - use_take_shared_method() const + use_take_shared_method() const override { return buffer_->use_take_shared_method(); } protected: void - trigger_guard_condition() + trigger_guard_condition() override { - gc_.trigger(); + this->gc_.trigger(); } BufferUniquePtr buffer_; + SubscribedTypeAllocator subscribed_type_allocator_; + SubscribedTypeDeleter subscribed_type_deleter_; }; } // namespace experimental diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index bebedda845..6afb397177 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -81,7 +81,6 @@ class Publisher : public PublisherBase /// MessageT::custom_type if MessageT is a TypeAdapter, otherwise just MessageT. using PublishedType = typename rclcpp::TypeAdapter::custom_type; - /// MessageT::ros_message_type if MessageT is a TypeAdapter, otherwise just MessageT. using ROSMessageType = typename rclcpp::TypeAdapter::ros_message_type; using PublishedTypeAllocatorTraits = allocator::AllocRebind; @@ -263,10 +262,11 @@ class Publisher : public PublisherBase get_subscription_count() > get_intra_process_subscription_count(); if (inter_process_publish_needed) { - auto shared_msg = this->do_intra_process_publish_and_return_shared(std::move(msg)); + auto shared_msg = + this->do_intra_process_ros_message_publish_and_return_shared(std::move(msg)); this->do_inter_process_publish(*shared_msg); } else { - this->do_intra_process_publish(std::move(msg)); + this->do_intra_process_ros_message_publish(std::move(msg)); } } @@ -319,13 +319,28 @@ class Publisher : public PublisherBase > publish(std::unique_ptr msg) { - // TODO(wjwwood): later update this to give the unique_ptr to the intra - // process manager and let it decide if it needs to be converted or not. - // For now, convert it unconditionally and pass it the ROSMessageType - // publish function specialization. - auto unique_ros_msg = this->create_ros_message_unique_ptr(); - rclcpp::TypeAdapter::convert_to_ros_message(*msg, *unique_ros_msg); - this->publish(std::move(unique_ros_msg)); + // Avoid allocating when not using intra process. + if (!intra_process_is_enabled_) { + // In this case we're not using intra process. + ROSMessageType ros_msg; + rclcpp::TypeAdapter::convert_to_ros_message(*msg, ros_msg); + return this->do_inter_process_publish(ros_msg); + } + + bool inter_process_publish_needed = + get_subscription_count() > get_intra_process_subscription_count(); + + if (inter_process_publish_needed) { + ROSMessageType ros_msg; + // TODO(clalancette): This is unnecessarily doing an additional conversion + // that may have already been done in do_intra_process_publish_and_return_shared(). + // We should just reuse that effort. + rclcpp::TypeAdapter::convert_to_ros_message(*msg, ros_msg); + this->do_intra_process_publish(std::move(msg)); + this->do_inter_process_publish(ros_msg); + } else { + this->do_intra_process_publish(std::move(msg)); + } } /// Publish a message on the topic. @@ -346,12 +361,7 @@ class Publisher : public PublisherBase > publish(const T & msg) { - // TODO(wjwwood): later update this to give the unique_ptr to the intra - // process manager and let it decide if it needs to be converted or not. - // For now, convert it unconditionally and pass it the ROSMessageType - // publish function specialization. - - // Avoid allocating when not using intra process. + // Avoid double allocating when not using intra process. if (!intra_process_is_enabled_) { // Convert to the ROS message equivalent and publish it. ROSMessageType ros_msg; @@ -359,13 +369,12 @@ class Publisher : public PublisherBase // In this case we're not using intra process. return this->do_inter_process_publish(ros_msg); } - // Otherwise we have to allocate memory in a unique_ptr, convert it, - // and pass it along. + + // Otherwise we have to allocate memory in a unique_ptr and pass it along. // As the message is not const, a copy should be made. // A shared_ptr could also be constructed here. - auto unique_ros_msg = this->create_ros_message_unique_ptr(); - rclcpp::TypeAdapter::convert_to_ros_message(msg, *unique_ros_msg); - this->publish(std::move(unique_ros_msg)); + auto unique_msg = this->duplicate_type_adapt_message_as_unique_ptr(msg); + this->publish(std::move(unique_msg)); } void @@ -491,7 +500,25 @@ class Publisher : public PublisherBase } void - do_intra_process_publish(std::unique_ptr msg) + do_intra_process_publish(std::unique_ptr msg) + { + auto ipm = weak_ipm_.lock(); + if (!ipm) { + throw std::runtime_error( + "intra process publish called after destruction of intra process manager"); + } + if (!msg) { + throw std::runtime_error("cannot publish msg which is a null pointer"); + } + + ipm->template do_intra_process_publish( + intra_process_publisher_id_, + std::move(msg), + published_type_allocator_); + } + + void + do_intra_process_ros_message_publish(std::unique_ptr msg) { auto ipm = weak_ipm_.lock(); if (!ipm) { @@ -502,14 +529,14 @@ class Publisher : public PublisherBase throw std::runtime_error("cannot publish msg which is a null pointer"); } - ipm->template do_intra_process_publish( + ipm->template do_intra_process_publish( intra_process_publisher_id_, std::move(msg), ros_message_type_allocator_); } std::shared_ptr - do_intra_process_publish_and_return_shared( + do_intra_process_ros_message_publish_and_return_shared( std::unique_ptr msg) { auto ipm = weak_ipm_.lock(); @@ -521,13 +548,14 @@ class Publisher : public PublisherBase throw std::runtime_error("cannot publish msg which is a null pointer"); } - return ipm->template do_intra_process_publish_and_return_sharedtemplate do_intra_process_publish_and_return_shared( intra_process_publisher_id_, std::move(msg), ros_message_type_allocator_); } + /// Return a new unique_ptr using the ROSMessageType of the publisher. std::unique_ptr create_ros_message_unique_ptr() @@ -546,6 +574,15 @@ class Publisher : public PublisherBase return std::unique_ptr(ptr, ros_message_type_deleter_); } + /// Duplicate a given type adapted message as a unique_ptr. + std::unique_ptr + duplicate_type_adapt_message_as_unique_ptr(const PublishedType & msg) + { + auto ptr = PublishedTypeAllocatorTraits::allocate(published_type_allocator_, 1); + PublishedTypeAllocatorTraits::construct(published_type_allocator_, ptr, msg); + return std::unique_ptr(ptr, published_type_deleter_); + } + /// Copy of original options passed during construction. /** * It is important to save a copy of this so that the rmw payload which it diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index a77f4f8ef1..2d52388e3b 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -276,7 +276,7 @@ class Subscription : public SubscriptionBase /// Take the next message from the inter-process subscription. /** - * This verison takes a SubscribedType which is different frmo the + * This version takes a SubscribedType which is different from the * ROSMessageType when a rclcpp::TypeAdapter is in used. * * \sa take(ROSMessageType &, rclcpp::MessageInfo &) @@ -406,10 +406,12 @@ class Subscription : public SubscriptionBase SubscriptionTopicStatisticsSharedPtr subscription_topic_statistics_{nullptr}; using SubscriptionIntraProcessT = rclcpp::experimental::SubscriptionIntraProcess< - ROSMessageType, - AllocatorT, - ROSMessageTypeDeleter, - MessageT>; + MessageT, + SubscribedType, + SubscribedTypeAllocator, + SubscribedTypeDeleter, + ROSMessageT, + AllocatorT>; std::shared_ptr subscription_intra_process_; }; diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index d78a1adef5..34bdc0daed 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -400,6 +400,18 @@ if(TARGET test_subscription_with_type_adapter) ${cpp_typesupport_target}) endif() +ament_add_gtest(test_subscription_publisher_with_same_type_adapter test_subscription_publisher_with_same_type_adapter.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" +) +if(TARGET test_subscription_publisher_with_same_type_adapter) + ament_target_dependencies(test_subscription_publisher_with_same_type_adapter + "statistics_msgs" + ) + target_link_libraries(test_subscription_publisher_with_same_type_adapter + ${PROJECT_NAME} + ${cpp_typesupport_target}) +endif() + ament_add_gtest(test_publisher_subscription_count_api test_publisher_subscription_count_api.cpp) if(TARGET test_publisher_subscription_count_api) ament_target_dependencies(test_publisher_subscription_count_api diff --git a/rclcpp/test/rclcpp/test_any_subscription_callback.cpp b/rclcpp/test/rclcpp/test_any_subscription_callback.cpp index 32abdf2934..02daee010b 100644 --- a/rclcpp/test/rclcpp/test_any_subscription_callback.cpp +++ b/rclcpp/test/rclcpp/test_any_subscription_callback.cpp @@ -25,6 +25,29 @@ #include "test_msgs/msg/empty.hpp" #include "test_msgs/msg/empty.h" +// Type adapter to be used in tests. +struct MyEmpty {}; + +template<> +struct rclcpp::TypeAdapter +{ + using is_specialized = std::true_type; + using custom_type = MyEmpty; + using ros_message_type = test_msgs::msg::Empty; + + static + void + convert_to_ros_message(const custom_type &, ros_message_type &) + {} + + static + void + convert_to_custom(const ros_message_type &, custom_type &) + {} +}; + +using MyTA = rclcpp::TypeAdapter; + class TestAnySubscriptionCallback : public ::testing::Test { public: @@ -43,6 +66,24 @@ class TestAnySubscriptionCallback : public ::testing::Test rclcpp::MessageInfo message_info_; }; +class TestAnySubscriptionCallbackTA : public ::testing::Test +{ +public: + TestAnySubscriptionCallbackTA() {} + + static + std::unique_ptr + get_unique_ptr_msg() + { + return std::make_unique(); + } + +protected: + rclcpp::AnySubscriptionCallback any_subscription_callback_; + std::shared_ptr msg_shared_ptr_{std::make_shared()}; + rclcpp::MessageInfo message_info_; +}; + void construct_with_null_allocator() { // suppress deprecated function warning @@ -97,29 +138,6 @@ TEST_F(TestAnySubscriptionCallback, unset_dispatch_throw) { // Parameterized test to test across all callback types and dispatch types. // -// Type adapter to be used in tests. -struct MyEmpty {}; - -template<> -struct rclcpp::TypeAdapter -{ - using is_specialized = std::true_type; - using custom_type = MyEmpty; - using ros_message_type = test_msgs::msg::Empty; - - static - void - convert_to_ros_message(const custom_type &, ros_message_type &) - {} - - static - void - convert_to_custom(const ros_message_type &, custom_type &) - {} -}; - -using MyTA = rclcpp::TypeAdapter; - template class InstanceContextImpl { @@ -177,7 +195,7 @@ class DispatchTests {}; class DispatchTestsWithTA - : public TestAnySubscriptionCallback, + : public TestAnySubscriptionCallbackTA, public ::testing::WithParamInterface> {}; @@ -193,27 +211,35 @@ format_parameter_with_ta(const ::testing::TestParamInfo as input */ \ - TEST_P(DispatchTests_name, test_inter_shared_dispatch) { \ - auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); \ - any_subscription_callback_to_test.dispatch(msg_shared_ptr_, message_info_); \ - } \ - \ - /* Testing dispatch with shared_ptr as input */ \ - TEST_P(DispatchTests_name, test_intra_shared_dispatch) { \ - auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); \ - any_subscription_callback_to_test.dispatch_intra_process(msg_shared_ptr_, message_info_); \ - } \ - \ - /* Testing dispatch with unique_ptr as input */ \ - TEST_P(DispatchTests_name, test_intra_unique_dispatch) { \ - auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); \ - any_subscription_callback_to_test.dispatch_intra_process(get_unique_ptr_msg(), message_info_); \ - } +/* Testing dispatch with shared_ptr as input */ +TEST_P(DispatchTests, test_inter_shared_dispatch) { + auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); + any_subscription_callback_to_test.dispatch(msg_shared_ptr_, message_info_); +} + +/* Testing dispatch with shared_ptr as input */ +TEST_P(DispatchTests, test_intra_shared_dispatch) { + auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); + any_subscription_callback_to_test.dispatch_intra_process(msg_shared_ptr_, message_info_); +} + +/* Testing dispatch with unique_ptr as input */ +TEST_P(DispatchTests, test_intra_unique_dispatch) { + auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); + any_subscription_callback_to_test.dispatch_intra_process(get_unique_ptr_msg(), message_info_); +} + +/* Testing dispatch with shared_ptr as input */ +TEST_P(DispatchTestsWithTA, test_intra_shared_dispatch) { + auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); + any_subscription_callback_to_test.dispatch_intra_process(msg_shared_ptr_, message_info_); +} -PARAMETERIZED_TESTS(DispatchTests) -PARAMETERIZED_TESTS(DispatchTestsWithTA) +/* Testing dispatch with unique_ptr as input */ +TEST_P(DispatchTestsWithTA, test_intra_unique_dispatch) { + auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); + any_subscription_callback_to_test.dispatch_intra_process(get_unique_ptr_msg(), message_info_); +} // Generic classes for testing callbacks using std::bind to class methods. template diff --git a/rclcpp/test/rclcpp/test_intra_process_manager.cpp b/rclcpp/test/rclcpp/test_intra_process_manager.cpp index 59437aa560..c78900ac1d 100644 --- a/rclcpp/test/rclcpp/test_intra_process_manager.cpp +++ b/rclcpp/test/rclcpp/test_intra_process_manager.cpp @@ -22,6 +22,7 @@ #define RCLCPP_BUILDING_LIBRARY 1 #include "rclcpp/allocator/allocator_common.hpp" +#include "rclcpp/context.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/qos.hpp" #include "rmw/types.h" @@ -194,9 +195,14 @@ class SubscriptionIntraProcessBase public: RCLCPP_SMART_PTR_ALIASES_ONLY(SubscriptionIntraProcessBase) - explicit SubscriptionIntraProcessBase(rclcpp::QoS qos = rclcpp::QoS(10)) - : qos_profile(qos), topic_name("topic") - {} + explicit SubscriptionIntraProcessBase( + rclcpp::Context::SharedPtr context, + const std::string & topic = "topic", + rclcpp::QoS qos = rclcpp::QoS(10)) + : qos_profile(qos), topic_name(topic) + { + (void)context; + } virtual ~SubscriptionIntraProcessBase() {} @@ -212,24 +218,26 @@ class SubscriptionIntraProcessBase const char * get_topic_name() { - return topic_name; + return topic_name.c_str(); } rclcpp::QoS qos_profile; - const char * topic_name; + std::string topic_name; }; template< typename MessageT, typename Alloc = std::allocator, - typename Deleter = std::default_delete> + typename Deleter = std::default_delete, + typename ROSMessageType = MessageT +> class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase { public: RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcessBuffer) explicit SubscriptionIntraProcessBuffer(rclcpp::QoS qos) - : SubscriptionIntraProcessBase(qos), take_shared_method(false) + : SubscriptionIntraProcessBase(nullptr, "topic", qos), take_shared_method(false) { buffer = std::make_unique>(); } @@ -246,6 +254,18 @@ class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase buffer->add(std::move(msg)); } + void + provide_intra_process_data(std::shared_ptr msg) + { + buffer->add(msg); + } + + void + provide_intra_process_data(std::unique_ptr msg) + { + buffer->add(std::move(msg)); + } + std::uintptr_t pop() { @@ -267,7 +287,7 @@ class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase template< typename MessageT, - typename Alloc = std::allocator, + typename Alloc = std::allocator, typename Deleter = std::default_delete> class SubscriptionIntraProcess : public SubscriptionIntraProcessBuffer< MessageT, @@ -328,7 +348,7 @@ void Publisher::publish(MessageUniquePtr msg) throw std::runtime_error("cannot publish msg which is a null pointer"); } - ipm->template do_intra_process_publish( + ipm->template do_intra_process_publish( intra_process_publisher_id_, std::move(msg), *message_allocator_); diff --git a/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp b/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp index f5ab43cb7c..8c6ebaee57 100644 --- a/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp +++ b/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp @@ -172,8 +172,18 @@ TEST_F(TestPublisher, conversion_exception_is_passed_up) { for (auto is_intra_process : {true, false}) { rclcpp::NodeOptions options; options.use_intra_process_comms(is_intra_process); + + auto callback = + [](const rclcpp::msg::String::ConstSharedPtr msg) -> void + { + (void)msg; + }; + initialize(options); auto pub = node->create_publisher("topic_name", 1); + // A subscription is created to ensure the existence of a buffer in the intra proccess + // manager which will trigger the faulty conversion. + auto sub = node->create_subscription("topic_name", 1, callback); EXPECT_THROW(pub->publish(1), std::runtime_error); } } diff --git a/rclcpp/test/rclcpp/test_subscription_publisher_with_same_type_adapter.cpp b/rclcpp/test/rclcpp/test_subscription_publisher_with_same_type_adapter.cpp new file mode 100644 index 0000000000..5cee82c3ef --- /dev/null +++ b/rclcpp/test/rclcpp/test_subscription_publisher_with_same_type_adapter.cpp @@ -0,0 +1,991 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +#include + +#include +#include +#include +#include +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "rclcpp/msg/string.hpp" +#include "statistics_msgs/msg/statistic_data_point.hpp" + +static const int g_max_loops = 200; +static const std::chrono::milliseconds g_sleep_per_loop(10); + +class test_intra_process_within_one_node : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +namespace rclcpp +{ + +template<> +struct TypeAdapter +{ + using is_specialized = std::true_type; + using custom_type = std::string; + using ros_message_type = rclcpp::msg::String; + + static void + convert_to_ros_message( + const custom_type & source, + ros_message_type & destination) + { + (void) source; + (void) destination; + throw std::runtime_error("This should not happen"); + } + + static void + convert_to_custom( + const ros_message_type & source, + custom_type & destination) + { + (void) source; + (void) destination; + throw std::runtime_error("This should not happen"); + } +}; + +template<> +struct TypeAdapter +{ + using is_specialized = std::true_type; + using custom_type = double; + using ros_message_type = statistics_msgs::msg::StatisticDataPoint; + + static void + convert_to_ros_message( + const custom_type & source, + ros_message_type & destination) + { + destination.data_type = 0; + destination.data = source; + } + + static void + convert_to_custom( + const ros_message_type & source, + custom_type & destination) + { + destination = source.data; + } +}; + +} // namespace rclcpp + +void wait_for_message_to_be_received( + bool & is_received, + const std::shared_ptr & node) +{ + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + executor.spin_once(std::chrono::milliseconds(0)); + int i = 0; + while (!is_received && i < g_max_loops) { + printf("spin_node_once() - callback (1) expected - try %d/%d\n", ++i, g_max_loops); + executor.spin_once(g_sleep_per_loop); + } +} + +/* + * Testing that subscriber receives type adapted types and ROS message types with intra proccess communications. + */ +TEST_F( + test_intra_process_within_one_node, + type_adapted_messages_unique_pub_received_by_intra_process_subscription) +{ + using StringTypeAdapter = rclcpp::TypeAdapter; + const std::string message_data = "Message Data"; + const std::string topic_name = "topic_name"; + + auto node = rclcpp::Node::make_shared( + "test_intra_process", + rclcpp::NodeOptions().use_intra_process_comms(true)); + + auto pub = node->create_publisher(topic_name, 1); + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::string &, publish with unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::string & msg) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), msg.c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::string & with message info, publish with unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::string & msg, const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), msg.c_str()); + EXPECT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::unique_ptr, publish with unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::unique_ptr with message info, publish with unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + EXPECT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr, publish with unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr with message info, publish with unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + EXPECT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr, publish with unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr with message info, publish with unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + EXPECT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::shared_ptr &, publish with unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::shared_ptr & msg) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::shared_ptr & with message info, + // publish with unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::shared_ptr & msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + EXPECT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } +} + +TEST_F( + test_intra_process_within_one_node, + type_adapted_messages_const_ref_pub_received_by_intra_process_subscription) +{ + using StringTypeAdapter = rclcpp::TypeAdapter; + const std::string message_data = "Message Data"; + const std::string topic_name = "topic_name"; + + auto node = rclcpp::Node::make_shared( + "test_intra_process", + rclcpp::NodeOptions().use_intra_process_comms(true)); + + auto pub = node->create_publisher(topic_name, 1); + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::string &, publish with const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::string & msg) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), msg.c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + std::string pu_message(message_data); + pub->publish(pu_message); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::string & with message info, publish with const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::string & msg, const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), msg.c_str()); + EXPECT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + std::string pu_message(message_data); + pub->publish(pu_message); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::unique_ptr, publish with const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + std::string pu_message(message_data); + pub->publish(pu_message); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::unique_ptr with message info, publish with const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + EXPECT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + std::string pu_message(message_data); + pub->publish(pu_message); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr, publish with const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + std::string pu_message(message_data); + pub->publish(pu_message); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr with message info, publish with const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + EXPECT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + std::string pu_message(message_data); + pub->publish(pu_message); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr, publish with const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + std::string pu_message(message_data); + pub->publish(pu_message); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr with message info, + // publish with const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + EXPECT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + std::string pu_message(message_data); + pub->publish(pu_message); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::shared_ptr &, publish with const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::shared_ptr & msg) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + std::string pu_message(message_data); + pub->publish(pu_message); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::shared_ptr & with message info, + // publish with const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::shared_ptr & msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + EXPECT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + std::string pu_message(message_data); + pub->publish(pu_message); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } +} + +TEST_F( + test_intra_process_within_one_node, + type_adapted_messages_ros_message_ref_pub_received_by_intra_process_subscription) +{ + using DoubleTypeAdapter = rclcpp::TypeAdapter; + const double message_data = 0.894; + const std::string topic_name = "topic_name"; + + auto node = rclcpp::Node::make_shared( + "test_intra_process", + rclcpp::NodeOptions().use_intra_process_comms(true)); + + auto pub = node->create_publisher(topic_name, 1); + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::string &, publish with rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + const double & msg) -> void { + is_received = true; + ASSERT_EQ(message_data, msg); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + statistics_msgs::msg::StatisticDataPoint msg; + msg.data = message_data; + pub->publish(msg); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::string & with message info, publish with rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + const double & msg, const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_EQ(message_data, msg); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + statistics_msgs::msg::StatisticDataPoint msg; + msg.data = message_data; + pub->publish(msg); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::unique_ptr, publish with rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + statistics_msgs::msg::StatisticDataPoint msg; + msg.data = message_data; + pub->publish(msg); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::unique_ptr with message info, publish with rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + statistics_msgs::msg::StatisticDataPoint msg; + msg.data = message_data; + pub->publish(msg); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr, publish with rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + statistics_msgs::msg::StatisticDataPoint msg; + msg.data = message_data; + pub->publish(msg); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr with message info, publish with rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + statistics_msgs::msg::StatisticDataPoint msg; + msg.data = message_data; + pub->publish(msg); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr, publish with rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + statistics_msgs::msg::StatisticDataPoint msg; + msg.data = message_data; + pub->publish(msg); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr with message info, + // publish with rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + statistics_msgs::msg::StatisticDataPoint msg; + msg.data = message_data; + pub->publish(msg); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::shared_ptr &, publish with rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::shared_ptr & msg) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + statistics_msgs::msg::StatisticDataPoint msg; + msg.data = message_data; + pub->publish(msg); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::shared_ptr & with message info, + // publish with rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::shared_ptr & msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + statistics_msgs::msg::StatisticDataPoint msg; + msg.data = message_data; + pub->publish(msg); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } +} + +TEST_F( + test_intra_process_within_one_node, + type_adapted_messages_unique_ptr_ros_message_pub_received_by_intra_process_subscription) +{ + using DoubleTypeAdapter = rclcpp::TypeAdapter; + const double message_data = 0.7508; + const std::string topic_name = "topic_name"; + + auto node = rclcpp::Node::make_shared( + "test_intra_process", + rclcpp::NodeOptions().use_intra_process_comms(true)); + + auto pub = node->create_publisher(topic_name, 1); + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::string &, publish with unique statistics_msgs::msg::StatisticDataPoint + bool is_received = false; + auto callback = + [message_data, &is_received]( + const double & msg) -> void { + is_received = true; + ASSERT_EQ(message_data, msg); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::string & with message info, publish with unique + // statistics_msgs::msg::StatisticDataPoint + bool is_received = false; + auto callback = + [message_data, &is_received]( + const double & msg, const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_EQ(message_data, msg); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::unique_ptr, publish with unique + // statistics_msgs::msg::StatisticDataPoint + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::unique_ptr with message info, publish with unique + // statistics_msgs::msg::StatisticDataPoint + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr, publish with unique + // statistics_msgs::msg::StatisticDataPoint + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr with message info, publish with unique + // statistics_msgs::msg::StatisticDataPoint + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr, publish with unique + // statistics_msgs::msg::StatisticDataPoint + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr with message info, publish with unique + // statistics_msgs::msg::StatisticDataPoint + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::shared_ptr &, publish with unique + // statistics_msgs::msg::StatisticDataPoint + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::shared_ptr & msg) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::shared_ptr & with message info, publish with unique + // statistics_msgs::msg::StatisticDataPoint + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::shared_ptr & msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } +}