.. _program_listing_file_include_rclcpp_any_subscription_callback.hpp: Program Listing for File any_subscription_callback.hpp ====================================================== |exhale_lsh| :ref:`Return to documentation for file ` (``include/rclcpp/any_subscription_callback.hpp``) .. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS .. code-block:: cpp // Copyright 2014 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__ANY_SUBSCRIPTION_CALLBACK_HPP_ #define RCLCPP__ANY_SUBSCRIPTION_CALLBACK_HPP_ #include #include #include #include #include #include // NOLINT[build/include_order] #include "rosidl_runtime_cpp/traits.hpp" #include "tracetools/tracetools.h" #include "tracetools/utils.hpp" #include "rclcpp/allocator/allocator_common.hpp" #include "rclcpp/detail/subscription_callback_type_helper.hpp" #include "rclcpp/function_traits.hpp" #include "rclcpp/message_info.hpp" #include "rclcpp/serialized_message.hpp" #include "rclcpp/type_adapter.hpp" template inline constexpr bool always_false_v = false; namespace rclcpp { namespace detail { template struct MessageDeleterHelper { using AllocTraits = allocator::AllocRebind; using Alloc = typename AllocTraits::allocator_type; using Deleter = allocator::Deleter; }; template struct AnySubscriptionCallbackPossibleTypes { using SubscribedType = typename rclcpp::TypeAdapter::custom_type; using ROSMessageType = typename rclcpp::TypeAdapter::ros_message_type; using SubscribedMessageDeleter = typename MessageDeleterHelper::Deleter; using ROSMessageDeleter = typename MessageDeleterHelper::Deleter; using SerializedMessageDeleter = typename MessageDeleterHelper::Deleter; using ConstRefCallback = std::function; using ConstRefROSMessageCallback = std::function; using ConstRefWithInfoCallback = std::function; using ConstRefWithInfoROSMessageCallback = std::function; using ConstRefSerializedMessageCallback = std::function; using ConstRefSerializedMessageWithInfoCallback = std::function; using UniquePtrCallback = std::function)>; using UniquePtrROSMessageCallback = std::function)>; using UniquePtrWithInfoCallback = std::function, const rclcpp::MessageInfo &)>; using UniquePtrWithInfoROSMessageCallback = std::function, const rclcpp::MessageInfo &)>; using UniquePtrSerializedMessageCallback = std::function)>; using UniquePtrSerializedMessageWithInfoCallback = std::function, const rclcpp::MessageInfo &)>; using SharedConstPtrCallback = std::function)>; using SharedConstPtrROSMessageCallback = std::function)>; using SharedConstPtrWithInfoCallback = std::function, const rclcpp::MessageInfo &)>; using SharedConstPtrWithInfoROSMessageCallback = std::function, const rclcpp::MessageInfo &)>; using SharedConstPtrSerializedMessageCallback = std::function)>; using SharedConstPtrSerializedMessageWithInfoCallback = std::function, const rclcpp::MessageInfo &)>; using ConstRefSharedConstPtrCallback = std::function &)>; using ConstRefSharedConstPtrROSMessageCallback = std::function &)>; using ConstRefSharedConstPtrWithInfoCallback = std::function &, const rclcpp::MessageInfo &)>; using ConstRefSharedConstPtrWithInfoROSMessageCallback = std::function &, const rclcpp::MessageInfo &)>; using ConstRefSharedConstPtrSerializedMessageCallback = std::function &)>; using ConstRefSharedConstPtrSerializedMessageWithInfoCallback = std::function &, const rclcpp::MessageInfo &)>; // Deprecated signatures: using SharedPtrCallback = std::function)>; using SharedPtrROSMessageCallback = std::function)>; using SharedPtrWithInfoCallback = std::function, const rclcpp::MessageInfo &)>; using SharedPtrWithInfoROSMessageCallback = std::function, const rclcpp::MessageInfo &)>; using SharedPtrSerializedMessageCallback = std::function)>; using SharedPtrSerializedMessageWithInfoCallback = std::function, const rclcpp::MessageInfo &)>; }; template< typename MessageT, typename AllocatorT, bool is_adapted_type = rclcpp::TypeAdapter::is_specialized::value > struct AnySubscriptionCallbackHelper; template struct AnySubscriptionCallbackHelper { using CallbackTypes = AnySubscriptionCallbackPossibleTypes; using variant_type = std::variant< typename CallbackTypes::ConstRefCallback, typename CallbackTypes::ConstRefWithInfoCallback, typename CallbackTypes::ConstRefSerializedMessageCallback, typename CallbackTypes::ConstRefSerializedMessageWithInfoCallback, typename CallbackTypes::UniquePtrCallback, typename CallbackTypes::UniquePtrWithInfoCallback, typename CallbackTypes::UniquePtrSerializedMessageCallback, typename CallbackTypes::UniquePtrSerializedMessageWithInfoCallback, typename CallbackTypes::SharedConstPtrCallback, typename CallbackTypes::SharedConstPtrWithInfoCallback, typename CallbackTypes::SharedConstPtrSerializedMessageCallback, typename CallbackTypes::SharedConstPtrSerializedMessageWithInfoCallback, typename CallbackTypes::ConstRefSharedConstPtrCallback, typename CallbackTypes::ConstRefSharedConstPtrWithInfoCallback, typename CallbackTypes::ConstRefSharedConstPtrSerializedMessageCallback, typename CallbackTypes::ConstRefSharedConstPtrSerializedMessageWithInfoCallback, typename CallbackTypes::SharedPtrCallback, typename CallbackTypes::SharedPtrWithInfoCallback, typename CallbackTypes::SharedPtrSerializedMessageCallback, typename CallbackTypes::SharedPtrSerializedMessageWithInfoCallback >; }; template struct AnySubscriptionCallbackHelper { using CallbackTypes = AnySubscriptionCallbackPossibleTypes; using variant_type = std::variant< typename CallbackTypes::ConstRefCallback, typename CallbackTypes::ConstRefROSMessageCallback, typename CallbackTypes::ConstRefWithInfoCallback, typename CallbackTypes::ConstRefWithInfoROSMessageCallback, typename CallbackTypes::ConstRefSerializedMessageCallback, typename CallbackTypes::ConstRefSerializedMessageWithInfoCallback, typename CallbackTypes::UniquePtrCallback, typename CallbackTypes::UniquePtrROSMessageCallback, typename CallbackTypes::UniquePtrWithInfoCallback, typename CallbackTypes::UniquePtrWithInfoROSMessageCallback, typename CallbackTypes::UniquePtrSerializedMessageCallback, typename CallbackTypes::UniquePtrSerializedMessageWithInfoCallback, typename CallbackTypes::SharedConstPtrCallback, typename CallbackTypes::SharedConstPtrROSMessageCallback, typename CallbackTypes::SharedConstPtrWithInfoCallback, typename CallbackTypes::SharedConstPtrWithInfoROSMessageCallback, typename CallbackTypes::SharedConstPtrSerializedMessageCallback, typename CallbackTypes::SharedConstPtrSerializedMessageWithInfoCallback, typename CallbackTypes::ConstRefSharedConstPtrCallback, typename CallbackTypes::ConstRefSharedConstPtrROSMessageCallback, typename CallbackTypes::ConstRefSharedConstPtrWithInfoCallback, typename CallbackTypes::ConstRefSharedConstPtrWithInfoROSMessageCallback, typename CallbackTypes::ConstRefSharedConstPtrSerializedMessageCallback, typename CallbackTypes::ConstRefSharedConstPtrSerializedMessageWithInfoCallback, typename CallbackTypes::SharedPtrCallback, typename CallbackTypes::SharedPtrROSMessageCallback, typename CallbackTypes::SharedPtrWithInfoCallback, typename CallbackTypes::SharedPtrWithInfoROSMessageCallback, typename CallbackTypes::SharedPtrSerializedMessageCallback, typename CallbackTypes::SharedPtrSerializedMessageWithInfoCallback >; }; } // namespace detail template< typename MessageT, typename AllocatorT = std::allocator > class AnySubscriptionCallback { private: using SubscribedType = typename rclcpp::TypeAdapter::custom_type; using ROSMessageType = typename rclcpp::TypeAdapter::ros_message_type; using HelperT = typename rclcpp::detail::AnySubscriptionCallbackHelper; using SubscribedTypeDeleterHelper = rclcpp::detail::MessageDeleterHelper; using SubscribedTypeAllocatorTraits = typename SubscribedTypeDeleterHelper::AllocTraits; using SubscribedTypeAllocator = typename SubscribedTypeDeleterHelper::Alloc; using SubscribedTypeDeleter = typename SubscribedTypeDeleterHelper::Deleter; using ROSMessageTypeDeleterHelper = rclcpp::detail::MessageDeleterHelper; using ROSMessageTypeAllocatorTraits = typename ROSMessageTypeDeleterHelper::AllocTraits; using ROSMessageTypeAllocator = typename ROSMessageTypeDeleterHelper::Alloc; using ROSMessageTypeDeleter = typename ROSMessageTypeDeleterHelper::Deleter; using SerializedMessageDeleterHelper = rclcpp::detail::MessageDeleterHelper; using SerializedMessageAllocatorTraits = typename SerializedMessageDeleterHelper::AllocTraits; using SerializedMessageAllocator = typename SerializedMessageDeleterHelper::Alloc; using SerializedMessageDeleter = typename SerializedMessageDeleterHelper::Deleter; // See AnySubscriptionCallbackPossibleTypes for the types of these. using CallbackTypes = detail::AnySubscriptionCallbackPossibleTypes; using ConstRefCallback = typename CallbackTypes::ConstRefCallback; using ConstRefROSMessageCallback = typename CallbackTypes::ConstRefROSMessageCallback; using ConstRefWithInfoCallback = typename CallbackTypes::ConstRefWithInfoCallback; using ConstRefWithInfoROSMessageCallback = typename CallbackTypes::ConstRefWithInfoROSMessageCallback; using ConstRefSerializedMessageCallback = typename CallbackTypes::ConstRefSerializedMessageCallback; using ConstRefSerializedMessageWithInfoCallback = typename CallbackTypes::ConstRefSerializedMessageWithInfoCallback; using UniquePtrCallback = typename CallbackTypes::UniquePtrCallback; using UniquePtrROSMessageCallback = typename CallbackTypes::UniquePtrROSMessageCallback; using UniquePtrWithInfoCallback = typename CallbackTypes::UniquePtrWithInfoCallback; using UniquePtrWithInfoROSMessageCallback = typename CallbackTypes::UniquePtrWithInfoROSMessageCallback; using UniquePtrSerializedMessageCallback = typename CallbackTypes::UniquePtrSerializedMessageCallback; using UniquePtrSerializedMessageWithInfoCallback = typename CallbackTypes::UniquePtrSerializedMessageWithInfoCallback; using SharedConstPtrCallback = typename CallbackTypes::SharedConstPtrCallback; using SharedConstPtrROSMessageCallback = typename CallbackTypes::SharedConstPtrROSMessageCallback; using SharedConstPtrWithInfoCallback = typename CallbackTypes::SharedConstPtrWithInfoCallback; using SharedConstPtrWithInfoROSMessageCallback = typename CallbackTypes::SharedConstPtrWithInfoROSMessageCallback; using SharedConstPtrSerializedMessageCallback = typename CallbackTypes::SharedConstPtrSerializedMessageCallback; using SharedConstPtrSerializedMessageWithInfoCallback = typename CallbackTypes::SharedConstPtrSerializedMessageWithInfoCallback; using ConstRefSharedConstPtrCallback = typename CallbackTypes::ConstRefSharedConstPtrCallback; using ConstRefSharedConstPtrROSMessageCallback = typename CallbackTypes::ConstRefSharedConstPtrROSMessageCallback; using ConstRefSharedConstPtrWithInfoCallback = typename CallbackTypes::ConstRefSharedConstPtrWithInfoCallback; using ConstRefSharedConstPtrWithInfoROSMessageCallback = typename CallbackTypes::ConstRefSharedConstPtrWithInfoROSMessageCallback; using ConstRefSharedConstPtrSerializedMessageCallback = typename CallbackTypes::ConstRefSharedConstPtrSerializedMessageCallback; using ConstRefSharedConstPtrSerializedMessageWithInfoCallback = typename CallbackTypes::ConstRefSharedConstPtrSerializedMessageWithInfoCallback; using SharedPtrCallback = typename CallbackTypes::SharedPtrCallback; using SharedPtrROSMessageCallback = typename CallbackTypes::SharedPtrROSMessageCallback; using SharedPtrWithInfoCallback = typename CallbackTypes::SharedPtrWithInfoCallback; using SharedPtrWithInfoROSMessageCallback = typename CallbackTypes::SharedPtrWithInfoROSMessageCallback; using SharedPtrSerializedMessageCallback = typename CallbackTypes::SharedPtrSerializedMessageCallback; using SharedPtrSerializedMessageWithInfoCallback = typename CallbackTypes::SharedPtrSerializedMessageWithInfoCallback; template struct NotNull { NotNull(const T * pointer_in, const char * msg) : pointer(pointer_in) { if (pointer == nullptr) { throw std::invalid_argument(msg); } } const T * pointer; }; public: explicit AnySubscriptionCallback(const AllocatorT & allocator = AllocatorT()) // NOLINT[runtime/explicit] : subscribed_type_allocator_(allocator), ros_message_type_allocator_(allocator) { allocator::set_allocator_for_deleter(&subscribed_type_deleter_, &subscribed_type_allocator_); allocator::set_allocator_for_deleter(&ros_message_type_deleter_, &ros_message_type_allocator_); } AnySubscriptionCallback(const AnySubscriptionCallback &) = default; template AnySubscriptionCallback set(CallbackT callback) { // Use the SubscriptionCallbackTypeHelper to determine the actual type of // the CallbackT, in terms of std::function<...>, which does not happen // automatically with lambda functions in cases where the arguments can be // converted to one another, e.g. shared_ptr and unique_ptr. using scbth = detail::SubscriptionCallbackTypeHelper; // Determine if the given CallbackT is a deprecated signature or not. constexpr auto is_deprecated = rclcpp::function_traits::same_arguments< typename scbth::callback_type, std::function)> >::value || rclcpp::function_traits::same_arguments< typename scbth::callback_type, std::function, const rclcpp::MessageInfo &)> >::value; // Use the discovered type to force the type of callback when assigning // into the variant. if constexpr (is_deprecated) { // If deprecated, call sub-routine that is deprecated. set_deprecated(static_cast(callback)); } else { // Otherwise just assign it. callback_variant_ = static_cast(callback); } // Return copy of self for easier testing, normally will be compiled out. return *this; } template #if !defined(RCLCPP_AVOID_DEPRECATIONS_FOR_UNIT_TESTS) // suppress deprecation warnings in `test_any_subscription_callback.cpp` [[deprecated("use 'void(std::shared_ptr)' instead")]] #endif void set_deprecated(std::function)> callback) { callback_variant_ = callback; } template #if !defined(RCLCPP_AVOID_DEPRECATIONS_FOR_UNIT_TESTS) // suppress deprecation warnings in `test_any_subscription_callback.cpp` [[deprecated( "use 'void(std::shared_ptr, const rclcpp::MessageInfo &)' instead" )]] #endif void set_deprecated(std::function, const rclcpp::MessageInfo &)> callback) { callback_variant_ = callback; } std::unique_ptr create_ros_unique_ptr_from_ros_shared_ptr_message( const std::shared_ptr & message) { auto ptr = ROSMessageTypeAllocatorTraits::allocate(ros_message_type_allocator_, 1); ROSMessageTypeAllocatorTraits::construct(ros_message_type_allocator_, ptr, *message); return std::unique_ptr(ptr, ros_message_type_deleter_); } std::unique_ptr create_serialized_message_unique_ptr_from_shared_ptr( const std::shared_ptr & serialized_message) { auto ptr = SerializedMessageAllocatorTraits::allocate(serialized_message_allocator_, 1); SerializedMessageAllocatorTraits::construct( serialized_message_allocator_, ptr, *serialized_message); return std::unique_ptr< rclcpp::SerializedMessage, SerializedMessageDeleter >(ptr, serialized_message_deleter_); } std::unique_ptr create_custom_unique_ptr_from_custom_shared_ptr_message( const std::shared_ptr & message) { auto ptr = SubscribedTypeAllocatorTraits::allocate(subscribed_type_allocator_, 1); SubscribedTypeAllocatorTraits::construct(subscribed_type_allocator_, ptr, *message); return std::unique_ptr(ptr, subscribed_type_deleter_); } std::unique_ptr convert_ros_message_to_custom_type_unique_ptr(const ROSMessageType & msg) { if constexpr (rclcpp::TypeAdapter::is_specialized::value) { auto ptr = SubscribedTypeAllocatorTraits::allocate(subscribed_type_allocator_, 1); SubscribedTypeAllocatorTraits::construct(subscribed_type_allocator_, ptr); rclcpp::TypeAdapter::convert_to_custom(msg, *ptr); return std::unique_ptr(ptr, subscribed_type_deleter_); } else { throw std::runtime_error( "convert_ros_message_to_custom_type_unique_ptr " "unexpectedly called without TypeAdapter"); } } 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( std::shared_ptr message, const rclcpp::MessageInfo & message_info) { TRACEPOINT(callback_start, static_cast(this), false); // Check if the variant is "unset", throw if it is. if (callback_variant_.index() == 0) { if (std::get<0>(callback_variant_) == nullptr) { // This can happen if it is default initialized, or if it is assigned nullptr. throw std::runtime_error("dispatch called on an unset AnySubscriptionCallback"); } } // Dispatch. std::visit( [&message, &message_info, this](auto && callback) { using T = std::decay_t; static constexpr bool is_ta = rclcpp::TypeAdapter::is_specialized::value; // conditions for output is custom message if constexpr (is_ta && std::is_same_v) { // TODO(wjwwood): consider avoiding heap allocation for small messages // maybe something like: // if constexpr (rosidl_generator_traits::has_fixed_size && sizeof(T) < N) { // ... on stack // } auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message); callback(*local_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); } else if constexpr (is_ta && std::is_same_v) { callback(convert_ros_message_to_custom_type_unique_ptr(*message)); } else if constexpr (is_ta && std::is_same_v) { callback(convert_ros_message_to_custom_type_unique_ptr(*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)); } 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), message_info); } // conditions for output is ros message 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) { callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message)); } else if constexpr (std::is_same_v) { 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|| std::is_same_v) { callback(message); } else if constexpr ( // NOLINT[readability/braces] std::is_same_v|| std::is_same_v|| std::is_same_v) { callback(message, message_info); } // condition to catch SerializedMessage types else if constexpr ( // NOLINT[readability/braces] std::is_same_v|| std::is_same_v|| std::is_same_v|| std::is_same_v|| std::is_same_v|| std::is_same_v|| std::is_same_v|| std::is_same_v|| std::is_same_v|| std::is_same_v) { throw std::runtime_error( "Cannot dispatch std::shared_ptr message " "to rclcpp::SerializedMessage"); } // condition to catch unhandled callback types else { // NOLINT[readability/braces] static_assert(always_false_v, "unhandled callback type"); } }, callback_variant_); TRACEPOINT(callback_end, static_cast(this)); } // Dispatch when input is a serialized message and the output could be anything. void dispatch( std::shared_ptr serialized_message, const rclcpp::MessageInfo & message_info) { TRACEPOINT(callback_start, static_cast(this), false); // Check if the variant is "unset", throw if it is. if (callback_variant_.index() == 0) { if (std::get<0>(callback_variant_) == nullptr) { // This can happen if it is default initialized, or if it is assigned nullptr. throw std::runtime_error("dispatch called on an unset AnySubscriptionCallback"); } } // Dispatch. std::visit( [&serialized_message, &message_info, this](auto && callback) { using T = std::decay_t; // condition to catch SerializedMessage types if constexpr (std::is_same_v) { callback(*serialized_message); } else if constexpr (std::is_same_v) { callback(*serialized_message, message_info); } else if constexpr (std::is_same_v) { callback(create_serialized_message_unique_ptr_from_shared_ptr(serialized_message)); } else if constexpr (std::is_same_v) { callback( create_serialized_message_unique_ptr_from_shared_ptr(serialized_message), message_info); } else if constexpr ( // NOLINT[readability/braces] std::is_same_v|| std::is_same_v|| std::is_same_v) { callback(create_serialized_message_unique_ptr_from_shared_ptr(serialized_message)); } else if constexpr ( // NOLINT[readability/braces] std::is_same_v|| std::is_same_v|| std::is_same_v) { callback( create_serialized_message_unique_ptr_from_shared_ptr(serialized_message), message_info); } // conditions for output anything else else if constexpr ( // NOLINT[whitespace/newline] std::is_same_v|| std::is_same_v|| std::is_same_v|| std::is_same_v|| std::is_same_v|| std::is_same_v|| std::is_same_v|| std::is_same_v|| std::is_same_v|| std::is_same_v|| std::is_same_v|| std::is_same_v|| std::is_same_v|| std::is_same_v|| std::is_same_v|| std::is_same_v|| std::is_same_v|| std::is_same_v|| std::is_same_v|| std::is_same_v) { throw std::runtime_error( "cannot dispatch rclcpp::SerializedMessage to " "non-rclcpp::SerializedMessage callbacks"); } // condition to catch unhandled callback types else { // NOLINT[readability/braces] static_assert(always_false_v, "unhandled callback type"); } }, callback_variant_); TRACEPOINT(callback_end, static_cast(this)); } void dispatch_intra_process( std::shared_ptr message, const rclcpp::MessageInfo & message_info) { TRACEPOINT(callback_start, static_cast(this), true); // Check if the variant is "unset", throw if it is. if (callback_variant_.index() == 0) { if (std::get<0>(callback_variant_) == nullptr) { // This can happen if it is default initialized, or if it is assigned nullptr. throw std::runtime_error("dispatch called on an unset AnySubscriptionCallback"); } } // Dispatch. std::visit( [&message, &message_info, this](auto && callback) { 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) { callback(*message); } else if constexpr (is_ta && std::is_same_v) { // NOLINT callback(*message, message_info); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v )) { 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(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(message); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v )) { callback(message, message_info); } // conditions for ros message type 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) { 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) { 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) { 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) { 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] std::is_same_v|| std::is_same_v|| std::is_same_v|| std::is_same_v|| std::is_same_v|| std::is_same_v|| std::is_same_v|| std::is_same_v|| std::is_same_v|| std::is_same_v) { throw std::runtime_error( "Cannot dispatch std::shared_ptr message " "to rclcpp::SerializedMessage"); } // condition to catch unhandled callback types else { // NOLINT[readability/braces] static_assert(always_false_v, "unhandled callback type"); } }, callback_variant_); TRACEPOINT(callback_end, static_cast(this)); } void dispatch_intra_process( std::unique_ptr message, const rclcpp::MessageInfo & message_info) { TRACEPOINT(callback_start, static_cast(this), true); // Check if the variant is "unset", throw if it is. if (callback_variant_.index() == 0) { if (std::get<0>(callback_variant_) == nullptr) { // This can happen if it is default initialized, or if it is assigned nullptr. throw std::runtime_error("dispatch called on an unset 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) { callback(*message); } else if constexpr (is_ta && std::is_same_v) { // NOLINT callback(*message, message_info); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v)) { callback(std::move(message)); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v )) { callback(std::move(message), message_info); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v )) { callback(std::move(message)); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v )) { callback(std::move(message), message_info); } // conditions for ros message type 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) { 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) { 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) { 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) { 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] std::is_same_v|| std::is_same_v|| std::is_same_v|| std::is_same_v|| std::is_same_v|| std::is_same_v|| std::is_same_v|| std::is_same_v|| std::is_same_v|| std::is_same_v) { throw std::runtime_error( "Cannot dispatch std::unique_ptr message " "to rclcpp::SerializedMessage"); } // condition to catch unhandled callback types else { // NOLINT[readability/braces] static_assert(always_false_v, "unhandled callback type"); } }, callback_variant_); TRACEPOINT(callback_end, static_cast(this)); } constexpr bool use_take_shared_method() const { return std::holds_alternative(callback_variant_) || std::holds_alternative(callback_variant_) || std::holds_alternative(callback_variant_) || std::holds_alternative(callback_variant_); } constexpr bool is_serialized_message_callback() const { return std::holds_alternative(callback_variant_) || std::holds_alternative(callback_variant_) || std::holds_alternative(callback_variant_) || std::holds_alternative(callback_variant_) || std::holds_alternative(callback_variant_); } void register_callback_for_tracing() { #ifndef TRACETOOLS_DISABLED std::visit( [this](auto && callback) { TRACEPOINT( rclcpp_callback_register, static_cast(this), tracetools::get_symbol(callback)); }, callback_variant_); #endif // TRACETOOLS_DISABLED } typename HelperT::variant_type & get_variant() { return callback_variant_; } const typename HelperT::variant_type & get_variant() const { return callback_variant_; } private: // TODO(wjwwood): switch to inheriting from std::variant (i.e. HelperT::variant_type) once // inheriting from std::variant is realistic (maybe C++23?), see: // http://www.open-std.org/jtc1/sc22/wg21/docs/papers/2020/p2162r0.html // For now, compose the variant into this class as a private attribute. typename HelperT::variant_type callback_variant_; SubscribedTypeAllocator subscribed_type_allocator_; SubscribedTypeDeleter subscribed_type_deleter_; ROSMessageTypeAllocator ros_message_type_allocator_; ROSMessageTypeDeleter ros_message_type_deleter_; SerializedMessageAllocator serialized_message_allocator_; SerializedMessageDeleter serialized_message_deleter_; }; } // namespace rclcpp #endif // RCLCPP__ANY_SUBSCRIPTION_CALLBACK_HPP_