From 1037822a63330495dcf0de5e8f20544375a5f116 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 2 Apr 2021 17:30:29 -0700 Subject: [PATCH] refactor AnySubscriptionCallback and add/deprecate callback signatures (#1598) * refactor AnySubscriptionCallback to... use std::variant and make the dispatch functions constexpr, avoiding runtime dispatching. Also, deprecate the std::function)> signature, as it is unsafe to share one shared_ptr when multiple subscriptions take it, because they could mutate MessageT while others are using it. So you'd have to make a copy for each subscription, which is no different than the std::unique_ptr signature or the user making their own copy in a shared_ptr from the const MessageT & signature or the std::shared_ptr signature. Added a std::function &)> signature to go along side the existing std::function)> signature. Removed redundant 'const' before pass-by-value signatures, e.g. std::function)> became std::function)>. This will not affect API or any users using the old style. Signed-off-by: William Woodall * fix use of std::bind, free functions, etc. using new function_traits::as_std_function<> Signed-off-by: William Woodall * fix use of const MessageT & callbacks by fixing subscriptin_traits Signed-off-by: William Woodall * fix deprecation warnings Signed-off-by: William Woodall * use target_compile_features to require c++17 for downstream users of rclcpp Signed-off-by: William Woodall * uncrustify Signed-off-by: William Woodall * cpplint Signed-off-by: William Woodall * use target_compile_features(... cxx_std_17) Signed-off-by: William Woodall * Keep both std::shared_ptr and const std::shared_ptr & signatures. The const std::shared_ptr & signature is being kept because it can be more flexible and efficient than std::shared_ptr, but costs realtively little to support. The std::shared_ptr signature is being kept because we want to avoid deprecating it and causing disruption, and because it is convenient to write, and in most cases will not materially impact the performance. Signed-off-by: William Woodall * defer deprecation of the shared_ptr sub callbacks Signed-off-by: William Woodall * fix unused variable warning Signed-off-by: William Woodall * small fixups to AnySubscriptionCallback Signed-off-by: William Woodall * add check for unset AnySubscriptionCallback in dispatch methods Signed-off-by: William Woodall * update dispatch methods to handle all scenarios Signed-off-by: William Woodall * updated tests for AnySubscriptionCallback, include full parameterized i/o matrix Signed-off-by: William Woodall * fixup test with changed assumption Signed-off-by: William Woodall * remove use of std::unary_function, which was removed in c++17 Signed-off-by: William Woodall * silence c++17 warnings on windows for now Signed-off-by: William Woodall --- rclcpp/CMakeLists.txt | 13 +- .../rclcpp/any_subscription_callback.hpp | 492 +++++++++++------- .../subscription_callback_type_helper.hpp | 164 ++++++ rclcpp/include/rclcpp/function_traits.hpp | 26 + .../rclcpp/parameter_event_handler.hpp | 12 +- .../rclcpp/parameter_events_filter.hpp | 6 +- .../include/rclcpp/subscription_factory.hpp | 2 +- rclcpp/include/rclcpp/subscription_traits.hpp | 2 +- rclcpp/include/rclcpp/time_source.hpp | 6 +- .../node_interfaces/node_parameters.cpp | 20 +- rclcpp/src/rclcpp/parameter_event_handler.cpp | 30 +- rclcpp/src/rclcpp/parameter_events_filter.cpp | 3 +- rclcpp/src/rclcpp/time_source.cpp | 5 +- rclcpp/test/benchmark/benchmark_executor.cpp | 2 +- .../test/rclcpp/executors/test_executors.cpp | 2 +- ...est_static_executor_entities_collector.cpp | 4 +- .../node_interfaces/test_node_graph.cpp | 2 +- .../test_allocator_memory_strategy.cpp | 4 +- .../test_add_callback_groups_to_executor.cpp | 8 +- .../rclcpp/test_any_subscription_callback.cpp | 418 ++++++++++----- .../test/rclcpp/test_create_subscription.cpp | 8 +- rclcpp/test/rclcpp/test_memory_strategy.cpp | 4 +- rclcpp/test/rclcpp/test_node.cpp | 2 +- rclcpp/test/rclcpp/test_parameter_client.cpp | 2 +- .../rclcpp/test_parameter_event_handler.cpp | 22 +- .../test_publisher_subscription_count_api.cpp | 2 +- rclcpp/test/rclcpp/test_qos_event.cpp | 4 +- .../test_serialized_message_allocator.cpp | 2 +- rclcpp/test/rclcpp/test_subscription.cpp | 16 +- .../test_subscription_publisher_count_api.cpp | 2 +- rclcpp/test/rclcpp/test_wait_set.cpp | 4 +- .../test_dynamic_storage.cpp | 8 +- .../wait_set_policies/test_static_storage.cpp | 4 +- .../test_storage_policy_common.cpp | 4 +- .../test_thread_safe_synchronization.cpp | 8 +- 35 files changed, 884 insertions(+), 429 deletions(-) create mode 100644 rclcpp/include/rclcpp/detail/subscription_callback_type_helper.hpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index d28ce2be97..ea1e516fc0 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -21,9 +21,10 @@ find_package(rosidl_typesupport_cpp REQUIRED) find_package(statistics_msgs REQUIRED) find_package(tracetools REQUIRED) -# Default to C++14 +# TODO(wjwwood): remove this when gtest can build on its own, when using target_compile_features() +# Default to C++17 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") # About -Wno-sign-conversion: With Clang, -Wconversion implies -Wsign-conversion. There are a number of @@ -174,8 +175,12 @@ foreach(interface_file ${interface_files}) include/rclcpp/node_interfaces/get_${interface_name}.hpp) endforeach() -add_library(${PROJECT_NAME} - ${${PROJECT_NAME}_SRCS}) +add_library(${PROJECT_NAME} ${${PROJECT_NAME}_SRCS}) +target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17) +# TODO(wjwwood): address all deprecation warnings and then remove this +if(WIN32) + target_compile_definitions(${PROJECT_NAME} PUBLIC "_SILENCE_ALL_CXX17_DEPRECATION_WARNINGS") +endif() target_include_directories(${PROJECT_NAME} PUBLIC "$" "$" diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp index 9a31ecff53..61b5e2e82e 100644 --- a/rclcpp/include/rclcpp/any_subscription_callback.hpp +++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp @@ -15,256 +15,394 @@ #ifndef RCLCPP__ANY_SUBSCRIPTION_CALLBACK_HPP_ #define RCLCPP__ANY_SUBSCRIPTION_CALLBACK_HPP_ -#include - #include #include #include #include #include +#include // NOLINT[build/include_order] + +#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/visibility_control.hpp" -#include "tracetools/tracetools.h" -#include "tracetools/utils.hpp" + +template +inline constexpr bool always_false_v = false; namespace rclcpp { -template -class AnySubscriptionCallback +namespace detail { - using MessageAllocTraits = allocator::AllocRebind; + +template +struct MessageDeleterHelper +{ + using MessageAllocTraits = allocator::AllocRebind; using MessageAlloc = typename MessageAllocTraits::allocator_type; using MessageDeleter = allocator::Deleter; - using ConstMessageSharedPtr = std::shared_ptr; - using MessageUniquePtr = std::unique_ptr; +}; - using SharedPtrCallback = std::function)>; - using SharedPtrWithInfoCallback = - std::function, const rclcpp::MessageInfo &)>; - using ConstSharedPtrCallback = std::function)>; - using ConstSharedPtrWithInfoCallback = - std::function, const rclcpp::MessageInfo &)>; - using UniquePtrCallback = std::function; +template +struct AnySubscriptionCallbackHelper +{ + using MessageDeleter = typename MessageDeleterHelper::MessageDeleter; + + using ConstRefCallback = + std::function; + using ConstRefWithInfoCallback = + std::function; + + using UniquePtrCallback = + std::function)>; using UniquePtrWithInfoCallback = - std::function; + std::function, const rclcpp::MessageInfo &)>; + + using SharedConstPtrCallback = + std::function)>; + using SharedConstPtrWithInfoCallback = + std::function, const rclcpp::MessageInfo &)>; + + using ConstRefSharedConstPtrCallback = + std::function &)>; + using ConstRefSharedConstPtrWithInfoCallback = + std::function &, const rclcpp::MessageInfo &)>; - SharedPtrCallback shared_ptr_callback_; - SharedPtrWithInfoCallback shared_ptr_with_info_callback_; - ConstSharedPtrCallback const_shared_ptr_callback_; - ConstSharedPtrWithInfoCallback const_shared_ptr_with_info_callback_; - UniquePtrCallback unique_ptr_callback_; - UniquePtrWithInfoCallback unique_ptr_with_info_callback_; + // Deprecated signatures: + using SharedPtrCallback = + std::function)>; + using SharedPtrWithInfoCallback = + std::function, const rclcpp::MessageInfo &)>; + + using variant_type = std::variant< + ConstRefCallback, + ConstRefWithInfoCallback, + UniquePtrCallback, + UniquePtrWithInfoCallback, + SharedConstPtrCallback, + SharedConstPtrWithInfoCallback, + ConstRefSharedConstPtrCallback, + ConstRefSharedConstPtrWithInfoCallback, + SharedPtrCallback, + SharedPtrWithInfoCallback + >; +}; + +} // namespace detail + +template< + typename MessageT, + typename AllocatorT = std::allocator +> +class AnySubscriptionCallback +{ +private: + using HelperT = typename rclcpp::detail::AnySubscriptionCallbackHelper; + using MessageDeleterHelper = rclcpp::detail::MessageDeleterHelper; + + using MessageAllocTraits = typename MessageDeleterHelper::MessageAllocTraits; + using MessageAlloc = typename MessageDeleterHelper::MessageAlloc; + using MessageDeleter = typename MessageDeleterHelper::MessageDeleter; + + // See AnySubscriptionCallbackHelper for the types of these. + using ConstRefCallback = typename HelperT::ConstRefCallback; + using ConstRefWithInfoCallback = typename HelperT::ConstRefWithInfoCallback; + + using UniquePtrCallback = typename HelperT::UniquePtrCallback; + using UniquePtrWithInfoCallback = typename HelperT::UniquePtrWithInfoCallback; + + using SharedConstPtrCallback = typename HelperT::SharedConstPtrCallback; + using SharedConstPtrWithInfoCallback = typename HelperT::SharedConstPtrWithInfoCallback; + + using ConstRefSharedConstPtrCallback = + typename HelperT::ConstRefSharedConstPtrCallback; + using ConstRefSharedConstPtrWithInfoCallback = + typename HelperT::ConstRefSharedConstPtrWithInfoCallback; + + using SharedPtrCallback = typename HelperT::SharedPtrCallback; + using SharedPtrWithInfoCallback = typename HelperT::SharedPtrWithInfoCallback; public: - explicit AnySubscriptionCallback(std::shared_ptr allocator) - : shared_ptr_callback_(nullptr), shared_ptr_with_info_callback_(nullptr), - const_shared_ptr_callback_(nullptr), const_shared_ptr_with_info_callback_(nullptr), - unique_ptr_callback_(nullptr), unique_ptr_with_info_callback_(nullptr) + explicit + AnySubscriptionCallback(const AllocatorT & allocator = AllocatorT()) // NOLINT[runtime/explicit] + { + message_allocator_ = allocator; + allocator::set_allocator_for_deleter(&message_deleter_, &message_allocator_); + } + + [[deprecated("use AnySubscriptionCallback(const AllocatorT & allocator) instead")]] + explicit + AnySubscriptionCallback(std::shared_ptr allocator) // NOLINT[runtime/explicit] { if (allocator == nullptr) { throw std::runtime_error("invalid allocator"); } - message_allocator_ = std::make_shared(*allocator.get()); - allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_.get()); + message_allocator_ = *allocator; + allocator::set_allocator_for_deleter(&message_deleter_, &message_allocator_); } AnySubscriptionCallback(const AnySubscriptionCallback &) = default; - template< - typename CallbackT, - typename std::enable_if< - rclcpp::function_traits::same_arguments< - CallbackT, - SharedPtrCallback - >::value - >::type * = nullptr - > - void set(CallbackT callback) + /// Generic function for setting the callback. + /** + * There are specializations that overload this in order to deprecate some + * callback signatures, and also to fix ambiguity between shared_ptr and + * unique_ptr callback signatures when using them with lambda functions. + */ + template + AnySubscriptionCallback + set(CallbackT callback) { - shared_ptr_callback_ = 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; - template< - typename CallbackT, - typename std::enable_if< + // Determine if the given CallbackT is a deprecated signature or not. + constexpr auto is_deprecated = rclcpp::function_traits::same_arguments< - CallbackT, - SharedPtrWithInfoCallback + typename scbth::callback_type, + std::function)> >::value - >::type * = nullptr - > - void set(CallbackT callback) - { - shared_ptr_with_info_callback_ = callback; - } - - template< - typename CallbackT, - typename std::enable_if< + || rclcpp::function_traits::same_arguments< - CallbackT, - ConstSharedPtrCallback - >::value - >::type * = nullptr - > - void set(CallbackT callback) - { - const_shared_ptr_callback_ = callback; + 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< - typename CallbackT, - typename std::enable_if< - rclcpp::function_traits::same_arguments< - CallbackT, - ConstSharedPtrWithInfoCallback - >::value - >::type * = nullptr - > - void set(CallbackT callback) + /// Function for shared_ptr to non-const MessageT, which is deprecated. + // TODO(wjwwood): enable this deprecation after Galactic + // [[deprecated( + // "use 'void (std::shared_ptr)' instead" + // )]] + void + set_deprecated(std::function)> callback) + // set(CallbackT callback) { - const_shared_ptr_with_info_callback_ = callback; + callback_variant_ = callback; } - template< - typename CallbackT, - typename std::enable_if< - rclcpp::function_traits::same_arguments< - CallbackT, - UniquePtrCallback - >::value - >::type * = nullptr - > - void set(CallbackT callback) + /// Function for shared_ptr to non-const MessageT with MessageInfo, which is deprecated. + // TODO(wjwwood): enable this deprecation after Galactic + // [[deprecated( + // "use 'void (std::shared_ptr, const rclcpp::MessageInfo &)' instead" + // )]] + void + set_deprecated( + std::function, const rclcpp::MessageInfo &)> callback) { - unique_ptr_callback_ = callback; + callback_variant_ = callback; } - template< - typename CallbackT, - typename std::enable_if< - rclcpp::function_traits::same_arguments< - CallbackT, - UniquePtrWithInfoCallback - >::value - >::type * = nullptr - > - void set(CallbackT callback) + std::unique_ptr + create_unique_ptr_from_shared_ptr_message(const std::shared_ptr & message) { - unique_ptr_with_info_callback_ = callback; + auto ptr = MessageAllocTraits::allocate(message_allocator_, 1); + MessageAllocTraits::construct(message_allocator_, ptr, *message); + return std::unique_ptr(ptr, message_deleter_); } - void dispatch( - std::shared_ptr message, const rclcpp::MessageInfo & message_info) + void + dispatch( + std::shared_ptr message, + const rclcpp::MessageInfo & message_info) { TRACEPOINT(callback_start, static_cast(this), false); - if (shared_ptr_callback_) { - shared_ptr_callback_(message); - } else if (shared_ptr_with_info_callback_) { - shared_ptr_with_info_callback_(message, message_info); - } else if (const_shared_ptr_callback_) { - const_shared_ptr_callback_(message); - } else if (const_shared_ptr_with_info_callback_) { - const_shared_ptr_with_info_callback_(message, message_info); - } else if (unique_ptr_callback_) { - auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1); - MessageAllocTraits::construct(*message_allocator_.get(), ptr, *message); - unique_ptr_callback_(MessageUniquePtr(ptr, message_deleter_)); - } else if (unique_ptr_with_info_callback_) { - auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1); - MessageAllocTraits::construct(*message_allocator_.get(), ptr, *message); - unique_ptr_with_info_callback_(MessageUniquePtr(ptr, message_deleter_), message_info); - } else { - throw std::runtime_error("unexpected message without any callback set"); + // 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; + + if constexpr (std::is_same_v) { + callback(*message); + } else if constexpr (std::is_same_v) { + callback(*message, message_info); + } else if constexpr (std::is_same_v) { + callback(create_unique_ptr_from_shared_ptr_message(message)); + } else if constexpr (std::is_same_v) { + callback(create_unique_ptr_from_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); + } else { + static_assert(always_false_v, "unhandled callback type"); + } + }, callback_variant_); TRACEPOINT(callback_end, static_cast(this)); } - void dispatch_intra_process( - ConstMessageSharedPtr message, const rclcpp::MessageInfo & message_info) + void + dispatch_intra_process( + std::shared_ptr message, + const rclcpp::MessageInfo & message_info) { TRACEPOINT(callback_start, static_cast(this), true); - if (const_shared_ptr_callback_) { - const_shared_ptr_callback_(message); - } else if (const_shared_ptr_with_info_callback_) { - const_shared_ptr_with_info_callback_(message, message_info); - } else { - if ( - unique_ptr_callback_ || unique_ptr_with_info_callback_ || - shared_ptr_callback_ || shared_ptr_with_info_callback_) - { - throw std::runtime_error( - "unexpected dispatch_intra_process const shared " - "message call with no const shared_ptr callback"); - } else { - throw std::runtime_error("unexpected message without any callback set"); + // 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; + + if constexpr (std::is_same_v) { + callback(*message); + } else if constexpr (std::is_same_v) { + callback(*message, message_info); + } else if constexpr ( // NOLINT[readability/braces] + std::is_same_v|| + std::is_same_v) + { + callback(create_unique_ptr_from_shared_ptr_message(message)); + } else if constexpr ( // NOLINT[readability/braces] + std::is_same_v|| + std::is_same_v) + { + callback(create_unique_ptr_from_shared_ptr_message(message), message_info); + } else if constexpr ( // NOLINT[readability/braces] + std::is_same_v|| + std::is_same_v) + { + callback(message); + } else if constexpr ( // NOLINT[readability/braces] + std::is_same_v|| + std::is_same_v) + { + callback(message, message_info); + } else { + static_assert(always_false_v, "unhandled callback type"); + } + }, callback_variant_); TRACEPOINT(callback_end, static_cast(this)); } - void dispatch_intra_process( - MessageUniquePtr message, const rclcpp::MessageInfo & message_info) + void + dispatch_intra_process( + std::unique_ptr message, + const rclcpp::MessageInfo & message_info) { TRACEPOINT(callback_start, static_cast(this), true); - if (shared_ptr_callback_) { - typename std::shared_ptr shared_message = std::move(message); - shared_ptr_callback_(shared_message); - } else if (shared_ptr_with_info_callback_) { - typename std::shared_ptr shared_message = std::move(message); - shared_ptr_with_info_callback_(shared_message, message_info); - } else if (unique_ptr_callback_) { - unique_ptr_callback_(std::move(message)); - } else if (unique_ptr_with_info_callback_) { - unique_ptr_with_info_callback_(std::move(message), message_info); - } else if (const_shared_ptr_callback_ || const_shared_ptr_with_info_callback_) { - throw std::runtime_error( - "unexpected dispatch_intra_process unique message call" - " with const shared_ptr callback"); - } else { - throw std::runtime_error("unexpected message without any callback set"); + // 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](auto && callback) { + using T = std::decay_t; + + if constexpr (std::is_same_v) { + callback(*message); + } else if constexpr (std::is_same_v) { + callback(*message, message_info); + } else if constexpr ( // NOLINT[readability/braces] + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v) + { + callback(std::move(message)); + } else if constexpr ( // NOLINT[readability/braces] + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v) + { + callback(std::move(message), message_info); + } else { + static_assert(always_false_v, "unhandled callback type"); + } + }, callback_variant_); TRACEPOINT(callback_end, static_cast(this)); } - bool use_take_shared_method() const + constexpr + bool + use_take_shared_method() const { - return const_shared_ptr_callback_ || const_shared_ptr_with_info_callback_; + return + 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() + void + register_callback_for_tracing() { #ifndef TRACETOOLS_DISABLED - if (shared_ptr_callback_) { - TRACEPOINT( - rclcpp_callback_register, - static_cast(this), - tracetools::get_symbol(shared_ptr_callback_)); - } else if (shared_ptr_with_info_callback_) { - TRACEPOINT( - rclcpp_callback_register, - static_cast(this), - tracetools::get_symbol(shared_ptr_with_info_callback_)); - } else if (unique_ptr_callback_) { - TRACEPOINT( - rclcpp_callback_register, - static_cast(this), - tracetools::get_symbol(unique_ptr_callback_)); - } else if (unique_ptr_with_info_callback_) { - TRACEPOINT( - rclcpp_callback_register, - static_cast(this), - tracetools::get_symbol(unique_ptr_with_info_callback_)); - } + 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: - std::shared_ptr message_allocator_; + // 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_; + + MessageAlloc message_allocator_; MessageDeleter message_deleter_; }; diff --git a/rclcpp/include/rclcpp/detail/subscription_callback_type_helper.hpp b/rclcpp/include/rclcpp/detail/subscription_callback_type_helper.hpp new file mode 100644 index 0000000000..ec24038d79 --- /dev/null +++ b/rclcpp/include/rclcpp/detail/subscription_callback_type_helper.hpp @@ -0,0 +1,164 @@ +// 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__DETAIL__SUBSCRIPTION_CALLBACK_TYPE_HELPER_HPP_ +#define RCLCPP__DETAIL__SUBSCRIPTION_CALLBACK_TYPE_HELPER_HPP_ + +#include +#include + +#include "rclcpp/function_traits.hpp" +#include "rclcpp/message_info.hpp" + +namespace rclcpp +{ +namespace detail +{ + +/// Template metaprogramming helper used to resolve the callback argument into a std::function. +/** + * Sometimes the CallbackT is a std::function already, but it could also be a + * function pointer, lambda, bind, or some variant of those. + * In some cases, like a lambda where the arguments can be converted between one + * another, e.g. std::function)> and + * std::function)>, you need to make that not ambiguous + * by checking the arguments independently using function traits rather than + * rely on overloading the two std::function types. + * + * This issue, with the lambda's, can be demonstrated with this minimal program: + * + * #include + * #include + * + * void f(std::function)>) {} + * void f(std::function)>) {} + * + * int main() { + * // Fails to compile with an "ambiguous call" error. + * f([](std::shared_ptr){}); + * + * // Works. + * std::function)> cb = [](std::shared_ptr){}; + * f(cb); + * } + * + * If this program ever starts working in a future version of C++, this class + * may become redundant. + * + * This helper works by using SFINAE with rclcpp::function_traits::same_arguments<> + * to narrow down the exact std::function<> type for the given CallbackT. + */ +template +struct SubscriptionCallbackTypeHelper +{ + using callback_type = typename rclcpp::function_traits::as_std_function::type; +}; + +template +struct SubscriptionCallbackTypeHelper< + MessageT, + CallbackT, + typename std::enable_if_t< + rclcpp::function_traits::same_arguments< + CallbackT, + std::function)> + >::value + > +> +{ + using callback_type = std::function)>; +}; + +template +struct SubscriptionCallbackTypeHelper< + MessageT, + CallbackT, + typename std::enable_if_t< + rclcpp::function_traits::same_arguments< + CallbackT, + std::function, const rclcpp::MessageInfo &)> + >::value + > +> +{ + using callback_type = + std::function, const rclcpp::MessageInfo &)>; +}; + +template +struct SubscriptionCallbackTypeHelper< + MessageT, + CallbackT, + typename std::enable_if_t< + rclcpp::function_traits::same_arguments< + CallbackT, + std::function &)> + >::value + > +> +{ + using callback_type = std::function &)>; +}; + +template +struct SubscriptionCallbackTypeHelper< + MessageT, + CallbackT, + typename std::enable_if_t< + rclcpp::function_traits::same_arguments< + CallbackT, + std::function &, const rclcpp::MessageInfo &)> + >::value + > +> +{ + using callback_type = + std::function &, const rclcpp::MessageInfo &)>; +}; + +template +struct SubscriptionCallbackTypeHelper< + MessageT, + CallbackT, + typename std::enable_if_t< + rclcpp::function_traits::same_arguments< + CallbackT, + std::function)> + >::value + > +> +{ + using callback_type = std::function)>; +}; + +template +struct SubscriptionCallbackTypeHelper< + MessageT, + CallbackT, + typename std::enable_if_t< + rclcpp::function_traits::same_arguments< + CallbackT, + std::function, const rclcpp::MessageInfo &)> + >::value + > +> +{ + using callback_type = + std::function, const rclcpp::MessageInfo &)>; +}; + +} // namespace detail +} // namespace rclcpp + +#endif // RCLCPP__DETAIL__SUBSCRIPTION_CALLBACK_TYPE_HELPER_HPP_ diff --git a/rclcpp/include/rclcpp/function_traits.hpp b/rclcpp/include/rclcpp/function_traits.hpp index f5c56b04e9..4004476842 100644 --- a/rclcpp/include/rclcpp/function_traits.hpp +++ b/rclcpp/include/rclcpp/function_traits.hpp @@ -162,6 +162,32 @@ struct same_arguments : std::is_same< > {}; +namespace detail +{ + +template +struct as_std_function_helper; + +template +struct as_std_function_helper> +{ + using type = std::function; +}; + +} // namespace detail + +template< + typename FunctorT, + typename FunctionTraits = function_traits +> +struct as_std_function +{ + using type = typename detail::as_std_function_helper< + typename FunctionTraits::return_type, + typename FunctionTraits::arguments + >::type; +}; + } // namespace function_traits } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/parameter_event_handler.hpp b/rclcpp/include/rclcpp/parameter_event_handler.hpp index 9a2e65b15e..c7b781f0ec 100644 --- a/rclcpp/include/rclcpp/parameter_event_handler.hpp +++ b/rclcpp/include/rclcpp/parameter_event_handler.hpp @@ -52,7 +52,7 @@ struct ParameterEventCallbackHandle RCLCPP_SMART_PTR_DEFINITIONS(ParameterEventCallbackHandle) using ParameterEventCallbackType = - std::function; + std::function; ParameterEventCallbackType callback; }; @@ -115,16 +115,16 @@ struct ParameterEventCallbackHandle * For example: * * auto cb3 = - * [fqn, remote_param_name, &node](const rcl_interfaces::msg::ParameterEvent::SharedPtr & event) { + * [fqn, remote_param_name, &node](const rcl_interfaces::msg::ParameterEvent & event) { * // Look for any updates to parameters in "/a_namespace" as well as any parameter changes * // to our own node ("this_node") * std::regex re("(/a_namespace/.*)|(/this_node)"); - * if (regex_match(event->node, re)) { + * if (regex_match(event.node, re)) { * // Now that we know the event matches the regular expression we scanned for, we can * // use 'get_parameter_from_event' to get a specific parameter name that we're looking for * rclcpp::Parameter p; * if (rclcpp::ParameterEventsSubscriber::get_parameter_from_event( - * *event, p, remote_param_name, fqn)) + * event, p, remote_param_name, fqn)) * { * RCLCPP_INFO( * node->get_logger(), @@ -136,7 +136,7 @@ struct ParameterEventCallbackHandle * * // You can also use 'get_parameter*s*_from_event' to enumerate all changes that came * // in on this event - * auto params = rclcpp::ParameterEventsSubscriber::get_parameters_from_event(*event); + * auto params = rclcpp::ParameterEventsSubscriber::get_parameters_from_event(event); * for (auto & p : params) { * RCLCPP_INFO( * node->get_logger(), @@ -288,7 +288,7 @@ class ParameterEventHandler /// Callback for parameter events subscriptions. RCLCPP_PUBLIC void - event_callback(const rcl_interfaces::msg::ParameterEvent::SharedPtr event); + event_callback(const rcl_interfaces::msg::ParameterEvent & event); // Utility function for resolving node path. std::string resolve_path(const std::string & path); diff --git a/rclcpp/include/rclcpp/parameter_events_filter.hpp b/rclcpp/include/rclcpp/parameter_events_filter.hpp index d09d287762..3aa70d8a85 100644 --- a/rclcpp/include/rclcpp/parameter_events_filter.hpp +++ b/rclcpp/include/rclcpp/parameter_events_filter.hpp @@ -37,7 +37,7 @@ class ParameterEventsFilter RCLCPP_SMART_PTR_DEFINITIONS(ParameterEventsFilter) enum class EventType {NEW, DELETED, CHANGED}; ///< An enum for the type of event. /// Used for the listed results - using EventPair = std::pair; + using EventPair = std::pair; /// Construct a filtered view of a parameter event. /** @@ -60,7 +60,7 @@ class ParameterEventsFilter */ RCLCPP_PUBLIC ParameterEventsFilter( - rcl_interfaces::msg::ParameterEvent::SharedPtr event, + std::shared_ptr event, const std::vector & names, const std::vector & types); @@ -74,7 +74,7 @@ class ParameterEventsFilter private: // access only allowed via const accessor. std::vector result_; ///< Storage of the resultant vector - rcl_interfaces::msg::ParameterEvent::SharedPtr event_; ///< Keep event in scope + std::shared_ptr event_; ///< Keep event in scope }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/subscription_factory.hpp b/rclcpp/include/rclcpp/subscription_factory.hpp index 694be60a74..7708f27df0 100644 --- a/rclcpp/include/rclcpp/subscription_factory.hpp +++ b/rclcpp/include/rclcpp/subscription_factory.hpp @@ -93,7 +93,7 @@ create_subscription_factory( auto allocator = options.get_allocator(); using rclcpp::AnySubscriptionCallback; - AnySubscriptionCallback any_subscription_callback(allocator); + AnySubscriptionCallback any_subscription_callback(*allocator); any_subscription_callback.set(std::forward(callback)); SubscriptionFactory factory { diff --git a/rclcpp/include/rclcpp/subscription_traits.hpp b/rclcpp/include/rclcpp/subscription_traits.hpp index 8eacd3a14b..34f2cc9219 100644 --- a/rclcpp/include/rclcpp/subscription_traits.hpp +++ b/rclcpp/include/rclcpp/subscription_traits.hpp @@ -64,7 +64,7 @@ struct is_serialized_callback template struct extract_message_type { - using type = typename std::remove_cv::type; + using type = typename std::remove_cv_t>; }; template diff --git a/rclcpp/include/rclcpp/time_source.hpp b/rclcpp/include/rclcpp/time_source.hpp index 6f5280572a..b4eb2aff24 100644 --- a/rclcpp/include/rclcpp/time_source.hpp +++ b/rclcpp/include/rclcpp/time_source.hpp @@ -156,7 +156,7 @@ class TimeSource std::promise cancel_clock_executor_promise_; // The clock callback itself - void clock_cb(const rosgraph_msgs::msg::Clock::SharedPtr msg); + void clock_cb(std::shared_ptr msg); // Create the subscription for the clock topic void create_clock_sub(); @@ -170,7 +170,7 @@ class TimeSource std::shared_ptr parameter_subscription_; // Callback for parameter updates - void on_parameter_event(const rcl_interfaces::msg::ParameterEvent::SharedPtr event); + void on_parameter_event(std::shared_ptr event); // An enum to hold the parameter state enum UseSimTimeParameterState {UNSET, SET_TRUE, SET_FALSE}; @@ -191,7 +191,7 @@ class TimeSource // This is needed when new clocks are added. bool ros_time_active_{false}; // Last set message to be passed to newly registered clocks - rosgraph_msgs::msg::Clock::SharedPtr last_msg_set_; + std::shared_ptr last_msg_set_; // A lock to protect iterating the associated_clocks_ field. std::mutex clock_list_lock_; diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index c6aa2af6e2..16aa9c4903 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -954,22 +954,6 @@ NodeParameters::list_parameters(const std::vector & prefixes, uint6 return result; } -struct HandleCompare - : public std::unary_function -{ - explicit HandleCompare(const OnSetParametersCallbackHandle * const base) - : base_(base) {} - bool operator()(const OnSetParametersCallbackHandle::WeakPtr & handle) - { - auto shared_handle = handle.lock(); - if (base_ == shared_handle.get()) { - return true; - } - return false; - } - const OnSetParametersCallbackHandle * const base_; -}; - void NodeParameters::remove_on_set_parameters_callback( const OnSetParametersCallbackHandle * const handle) @@ -980,7 +964,9 @@ NodeParameters::remove_on_set_parameters_callback( auto it = std::find_if( on_parameters_set_callback_container_.begin(), on_parameters_set_callback_container_.end(), - HandleCompare(handle)); + [handle](const auto & weak_handle) { + return handle == weak_handle.lock().get(); + }); if (it != on_parameters_set_callback_container_.end()) { on_parameters_set_callback_container_.erase(it); } else { diff --git a/rclcpp/src/rclcpp/parameter_event_handler.cpp b/rclcpp/src/rclcpp/parameter_event_handler.cpp index 9fd5994864..232a32f887 100644 --- a/rclcpp/src/rclcpp/parameter_event_handler.cpp +++ b/rclcpp/src/rclcpp/parameter_event_handler.cpp @@ -37,23 +37,6 @@ ParameterEventHandler::add_parameter_event_callback( return handle; } -template -struct HandleCompare - : public std::unary_function -{ - explicit HandleCompare(const CallbackHandleT * const base) - : base_(base) {} - bool operator()(const typename CallbackHandleT::WeakPtr & handle) - { - auto shared_handle = handle.lock(); - if (base_ == shared_handle.get()) { - return true; - } - return false; - } - const CallbackHandleT * const base_; -}; - void ParameterEventHandler::remove_parameter_event_callback( ParameterEventCallbackHandle::SharedPtr callback_handle) @@ -62,7 +45,9 @@ ParameterEventHandler::remove_parameter_event_callback( auto it = std::find_if( event_callbacks_.begin(), event_callbacks_.end(), - HandleCompare(callback_handle.get())); + [callback_handle](const auto & weak_handle) { + return callback_handle.get() == weak_handle.lock().get(); + }); if (it != event_callbacks_.end()) { event_callbacks_.erase(it); } else { @@ -99,7 +84,9 @@ ParameterEventHandler::remove_parameter_callback( auto it = std::find_if( container.begin(), container.end(), - HandleCompare(handle)); + [handle](const auto & weak_handle) { + return handle == weak_handle.lock().get(); + }); if (it != container.end()) { container.erase(it); if (container.empty()) { @@ -171,14 +158,13 @@ ParameterEventHandler::get_parameters_from_event( } void -ParameterEventHandler::event_callback( - const rcl_interfaces::msg::ParameterEvent::SharedPtr event) +ParameterEventHandler::event_callback(const rcl_interfaces::msg::ParameterEvent & event) { std::lock_guard lock(mutex_); for (auto it = parameter_callbacks_.begin(); it != parameter_callbacks_.end(); ++it) { rclcpp::Parameter p; - if (get_parameter_from_event(*event, p, it->first.first, it->first.second)) { + if (get_parameter_from_event(event, p, it->first.first, it->first.second)) { for (auto cb = it->second.begin(); cb != it->second.end(); ++cb) { auto shared_handle = cb->lock(); if (nullptr != shared_handle) { diff --git a/rclcpp/src/rclcpp/parameter_events_filter.cpp b/rclcpp/src/rclcpp/parameter_events_filter.cpp index 36ef708c35..be9882c85b 100644 --- a/rclcpp/src/rclcpp/parameter_events_filter.cpp +++ b/rclcpp/src/rclcpp/parameter_events_filter.cpp @@ -14,6 +14,7 @@ #include "rclcpp/parameter_events_filter.hpp" +#include #include #include @@ -22,7 +23,7 @@ using EventType = rclcpp::ParameterEventsFilter::EventType; using EventPair = rclcpp::ParameterEventsFilter::EventPair; ParameterEventsFilter::ParameterEventsFilter( - rcl_interfaces::msg::ParameterEvent::SharedPtr event, + std::shared_ptr event, const std::vector & names, const std::vector & types) : event_(event) diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index 24f48b7c7c..595520da4e 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -216,7 +216,7 @@ void TimeSource::set_clock( } } -void TimeSource::clock_cb(const rosgraph_msgs::msg::Clock::SharedPtr msg) +void TimeSource::clock_cb(std::shared_ptr msg) { if (!this->ros_time_active_ && SET_TRUE == this->parameter_state_) { enable_ros_time(); @@ -294,7 +294,8 @@ void TimeSource::destroy_clock_sub() clock_subscription_.reset(); } -void TimeSource::on_parameter_event(const rcl_interfaces::msg::ParameterEvent::SharedPtr event) +void TimeSource::on_parameter_event( + std::shared_ptr event) { // Filter out events on 'use_sim_time' parameter instances in other nodes. if (event->node != node_base_->get_fully_qualified_name()) { diff --git a/rclcpp/test/benchmark/benchmark_executor.cpp b/rclcpp/test/benchmark/benchmark_executor.cpp index 27e6d2ff5b..c1fb3cf52c 100644 --- a/rclcpp/test/benchmark/benchmark_executor.cpp +++ b/rclcpp/test/benchmark/benchmark_executor.cpp @@ -42,7 +42,7 @@ class PerformanceTestExecutor : public PerformanceTest nodes[i]->create_publisher( "/empty_msgs_" + std::to_string(i), rclcpp::QoS(10))); - auto callback = [this](test_msgs::msg::Empty::SharedPtr) {this->callback_count++;}; + auto callback = [this](test_msgs::msg::Empty::ConstSharedPtr) {this->callback_count++;}; subscriptions.push_back( nodes[i]->create_subscription( "/empty_msgs_" + std::to_string(i), rclcpp::QoS(10), std::move(callback))); diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index ad34273f68..970cd3bdb2 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -62,7 +62,7 @@ class TestExecutors : public ::testing::Test const std::string topic_name = std::string("topic_") + test_name.str(); publisher = node->create_publisher(topic_name, rclcpp::QoS(10)); - auto callback = [this](test_msgs::msg::Empty::SharedPtr) {this->callback_count++;}; + auto callback = [this](test_msgs::msg::Empty::ConstSharedPtr) {this->callback_count++;}; subscription = node->create_subscription( topic_name, rclcpp::QoS(10), std::move(callback)); diff --git a/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp b/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp index 5b20c6b201..fba322f9e2 100644 --- a/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp +++ b/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp @@ -255,7 +255,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_with_entities) { // Create 1 of each entity type auto subscription = node->create_subscription( - "topic", rclcpp::QoS(10), [](test_msgs::msg::Empty::SharedPtr) {}); + "topic", rclcpp::QoS(10), [](test_msgs::msg::Empty::ConstSharedPtr) {}); auto timer = node->create_wall_timer(std::chrono::seconds(60), []() {}); auto service = @@ -455,7 +455,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, refresh_wait_set_add_handles_to_wait // Create 1 of each entity type auto subscription = node->create_subscription( - "topic", rclcpp::QoS(10), [](test_msgs::msg::Empty::SharedPtr) {}); + "topic", rclcpp::QoS(10), [](test_msgs::msg::Empty::ConstSharedPtr) {}); auto timer = node->create_wall_timer(std::chrono::seconds(60), []() {}); auto service = diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp index 08717164e2..6690b88bf6 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp @@ -335,7 +335,7 @@ TEST_F(TestNodeGraph, get_info_by_topic) { const rclcpp::QoS publisher_qos(1); auto publisher = node()->create_publisher("topic", publisher_qos); - auto callback = [](const test_msgs::msg::Empty::SharedPtr) {}; + auto callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; const rclcpp::QoS subscriber_qos(10); auto subscription = diff --git a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp index b0951c9218..0b4286dcf2 100644 --- a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp @@ -125,7 +125,7 @@ class TestAllocatorMemoryStrategy : public ::testing::Test std::shared_ptr create_node_with_subscription(const std::string & name) { - auto subscription_callback = [](const test_msgs::msg::Empty::SharedPtr) {}; + auto subscription_callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; const rclcpp::QoS qos(10); auto node_with_subscription = create_node_with_disabled_callback_groups(name); @@ -778,7 +778,7 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_subscription_out_of_scope) { subscription_options.callback_group = callback_group; - auto subscription_callback = [](const test_msgs::msg::Empty::SharedPtr) {}; + auto subscription_callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; const rclcpp::QoS qos(10); auto subscription = node->create_subscription< diff --git a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp index 25bd2aba03..fa636b7157 100644 --- a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp +++ b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp @@ -96,7 +96,7 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups) { const rclcpp::QoS qos(10); auto options = rclcpp::SubscriptionOptions(); - auto callback = [](const test_msgs::msg::Empty::SharedPtr) {}; + auto callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; rclcpp::CallbackGroup::SharedPtr cb_grp2 = node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive); options.callback_group = cb_grp2; @@ -139,7 +139,7 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, remove_callback_groups) { executor.add_callback_group(cb_grp, node->get_node_base_interface()); const rclcpp::QoS qos(10); auto options = rclcpp::SubscriptionOptions(); - auto callback = [](const test_msgs::msg::Empty::SharedPtr) {}; + auto callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; rclcpp::CallbackGroup::SharedPtr cb_grp2 = node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive); options.callback_group = cb_grp2; @@ -222,7 +222,7 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_unallowable_callback_groups) const rclcpp::QoS qos(10); auto options = rclcpp::SubscriptionOptions(); - auto callback = [](const test_msgs::msg::Empty::SharedPtr) {}; + auto callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; rclcpp::CallbackGroup::SharedPtr cb_grp2 = node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, false); options.callback_group = cb_grp2; @@ -256,7 +256,7 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, one_node_many_callback_groups_many_e timer_executor.add_callback_group(cb_grp, node->get_node_base_interface()); const rclcpp::QoS qos(10); auto options = rclcpp::SubscriptionOptions(); - auto callback = [](const test_msgs::msg::Empty::SharedPtr) {}; + auto callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; rclcpp::CallbackGroup::SharedPtr cb_grp2 = node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, false); options.callback_group = cb_grp2; diff --git a/rclcpp/test/rclcpp/test_any_subscription_callback.cpp b/rclcpp/test/rclcpp/test_any_subscription_callback.cpp index 5ed90846b7..b8d7fd9ba3 100644 --- a/rclcpp/test/rclcpp/test_any_subscription_callback.cpp +++ b/rclcpp/test/rclcpp/test_any_subscription_callback.cpp @@ -16,6 +16,7 @@ #include #include +#include #include #include "rclcpp/any_subscription_callback.hpp" @@ -25,42 +26,57 @@ class TestAnySubscriptionCallback : public ::testing::Test { public: - TestAnySubscriptionCallback() - : allocator_(std::make_shared>()), - any_subscription_callback_(allocator_) {} - void SetUp() + TestAnySubscriptionCallback() {} + + static + std::unique_ptr + get_unique_ptr_msg() { - msg_shared_ptr_ = std::make_shared(); - msg_const_shared_ptr_ = std::make_shared(); - msg_unique_ptr_ = std::make_unique(); + return std::make_unique(); } protected: - std::shared_ptr> allocator_; - rclcpp::AnySubscriptionCallback> - any_subscription_callback_; - - std::shared_ptr msg_shared_ptr_; - std::shared_ptr msg_const_shared_ptr_; - std::unique_ptr msg_unique_ptr_; + 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 +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif + // We need to wrap this in a function because `EXPECT_THROW` is a macro, and thinks // that the comma in here splits macro arguments, not the template arguments. - rclcpp::AnySubscriptionCallback< - test_msgs::msg::Empty, std::allocator> any_subscription_callback_(nullptr); + rclcpp::AnySubscriptionCallback any_subscription_callback(nullptr); + +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif } -TEST(AnySubscription, null_allocator) { +TEST(AnySubscriptionCallback, null_allocator) { EXPECT_THROW( construct_with_null_allocator(), std::runtime_error); } TEST_F(TestAnySubscriptionCallback, construct_destruct) { + // Default constructor. + rclcpp::AnySubscriptionCallback asc1; + + // Constructor with allocator. + std::allocator allocator; + rclcpp::AnySubscriptionCallback asc2(allocator); } TEST_F(TestAnySubscriptionCallback, unset_dispatch_throw) { @@ -68,153 +84,285 @@ TEST_F(TestAnySubscriptionCallback, unset_dispatch_throw) { any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_), std::runtime_error); EXPECT_THROW( - any_subscription_callback_.dispatch_intra_process(msg_const_shared_ptr_, message_info_), + any_subscription_callback_.dispatch_intra_process(msg_shared_ptr_, message_info_), std::runtime_error); EXPECT_THROW( - any_subscription_callback_.dispatch_intra_process(std::move(msg_unique_ptr_), message_info_), + any_subscription_callback_.dispatch_intra_process(get_unique_ptr_msg(), message_info_), std::runtime_error); } -TEST_F(TestAnySubscriptionCallback, set_dispatch_shared_ptr) { - int callback_count = 0; - auto shared_ptr_callback = [&callback_count]( - const std::shared_ptr) { - callback_count++; - }; - - any_subscription_callback_.set(shared_ptr_callback); - EXPECT_NO_THROW(any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_)); - EXPECT_EQ(callback_count, 1); +// +// Parameterized test to test across all callback types and dispatch types. +// - // Can't convert ConstSharedPtr to SharedPtr - EXPECT_THROW( - any_subscription_callback_.dispatch_intra_process(msg_const_shared_ptr_, message_info_), - std::runtime_error); - EXPECT_EQ(callback_count, 1); +class InstanceContextImpl +{ +public: + InstanceContextImpl() = default; + virtual ~InstanceContextImpl() = default; - // Promotes Unique into SharedPtr - EXPECT_NO_THROW( - any_subscription_callback_.dispatch_intra_process(std::move(msg_unique_ptr_), message_info_)); - EXPECT_EQ(callback_count, 2); -} + explicit InstanceContextImpl(rclcpp::AnySubscriptionCallback asc) + : any_subscription_callback_(asc) + {} -TEST_F(TestAnySubscriptionCallback, set_dispatch_shared_ptr_w_info) { - int callback_count = 0; - auto shared_ptr_w_info_callback = [&callback_count]( - const std::shared_ptr, const rclcpp::MessageInfo &) { - callback_count++; - }; + virtual + rclcpp::AnySubscriptionCallback + get_any_subscription_callback_to_test() const + { + return any_subscription_callback_; + } - any_subscription_callback_.set(shared_ptr_w_info_callback); +protected: + rclcpp::AnySubscriptionCallback any_subscription_callback_; +}; - EXPECT_NO_THROW(any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_)); - EXPECT_EQ(callback_count, 1); +class InstanceContext +{ +public: + InstanceContext(const std::string & name, std::shared_ptr impl) + : name(name), impl_(impl) + {} - // Can't convert ConstSharedPtr to SharedPtr - EXPECT_THROW( - any_subscription_callback_.dispatch_intra_process(msg_const_shared_ptr_, message_info_), - std::runtime_error); - EXPECT_EQ(callback_count, 1); + InstanceContext( + const std::string & name, + rclcpp::AnySubscriptionCallback asc) + : name(name), impl_(std::make_shared(asc)) + {} - // Promotes Unique into SharedPtr - EXPECT_NO_THROW( - any_subscription_callback_.dispatch_intra_process(std::move(msg_unique_ptr_), message_info_)); - EXPECT_EQ(callback_count, 2); -} + InstanceContext(const InstanceContext & other) + : InstanceContext(other.name, other.impl_) {} -TEST_F(TestAnySubscriptionCallback, set_dispatch_const_shared_ptr) { - int callback_count = 0; - auto const_shared_ptr_callback = [&callback_count]( - std::shared_ptr) { - callback_count++; - }; + rclcpp::AnySubscriptionCallback + get_any_subscription_callback_to_test() const + { + return impl_->get_any_subscription_callback_to_test(); + } - any_subscription_callback_.set(const_shared_ptr_callback); + std::string name; - // Ok to promote shared_ptr to ConstSharedPtr - EXPECT_NO_THROW(any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_)); - EXPECT_EQ(callback_count, 1); +protected: + std::shared_ptr impl_; +}; - EXPECT_NO_THROW( - any_subscription_callback_.dispatch_intra_process(msg_const_shared_ptr_, message_info_)); - EXPECT_EQ(callback_count, 2); +class DispatchTests + : public TestAnySubscriptionCallback, + public ::testing::WithParamInterface +{}; - // Not allowed to convert unique_ptr to const shared_ptr - EXPECT_THROW( - any_subscription_callback_.dispatch_intra_process(std::move(msg_unique_ptr_), message_info_), - std::runtime_error); - EXPECT_EQ(callback_count, 2); +auto +format_parameter(const ::testing::TestParamInfo & info) +{ + return info.param.name; } -TEST_F(TestAnySubscriptionCallback, set_dispatch_const_shared_ptr_w_info) { - int callback_count = 0; - auto const_shared_ptr_callback = [&callback_count]( - std::shared_ptr, const rclcpp::MessageInfo &) { - callback_count++; - }; - - any_subscription_callback_.set( - std::move(const_shared_ptr_callback)); - - // Ok to promote shared_ptr to ConstSharedPtr - EXPECT_NO_THROW(any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_)); - EXPECT_EQ(callback_count, 1); +// 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_); +} - EXPECT_NO_THROW( - any_subscription_callback_.dispatch_intra_process(msg_const_shared_ptr_, message_info_)); - EXPECT_EQ(callback_count, 2); +// 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_); +} - // Not allowed to convert unique_ptr to const shared_ptr - EXPECT_THROW( - any_subscription_callback_.dispatch_intra_process(std::move(msg_unique_ptr_), message_info_), - std::runtime_error); - EXPECT_EQ(callback_count, 2); +// 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_); } -TEST_F(TestAnySubscriptionCallback, set_dispatch_unique_ptr) { - int callback_count = 0; - auto unique_ptr_callback = [&callback_count]( - std::unique_ptr) { - callback_count++; - }; +// Generic classes for testing callbacks using std::bind to class methods. +template +class BindContextImpl : public InstanceContextImpl +{ + static constexpr size_t number_of_callback_args{sizeof...(CallbackArgs)}; - any_subscription_callback_.set(unique_ptr_callback); +public: + using InstanceContextImpl::InstanceContextImpl; + virtual ~BindContextImpl() = default; - // Message is copied into unique_ptr - EXPECT_NO_THROW(any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_)); - EXPECT_EQ(callback_count, 1); + void on_message(CallbackArgs ...) const {} - EXPECT_THROW( - any_subscription_callback_.dispatch_intra_process(msg_const_shared_ptr_, message_info_), - std::runtime_error); - EXPECT_EQ(callback_count, 1); + rclcpp::AnySubscriptionCallback + get_any_subscription_callback_to_test() const override + { + if constexpr (number_of_callback_args == 1) { + return rclcpp::AnySubscriptionCallback().set( + std::bind(&BindContextImpl::on_message, this, std::placeholders::_1) + ); + } else { + return rclcpp::AnySubscriptionCallback().set( + std::bind(&BindContextImpl::on_message, this, std::placeholders::_1, std::placeholders::_2) + ); + } + } +}; - // Unique_ptr is_moved - EXPECT_NO_THROW( - any_subscription_callback_.dispatch_intra_process(std::move(msg_unique_ptr_), message_info_)); - EXPECT_EQ(callback_count, 2); -} +template +class BindContext : public InstanceContext +{ +public: + explicit BindContext(const std::string & name) + : InstanceContext(name, std::make_shared>()) + {} +}; -TEST_F(TestAnySubscriptionCallback, set_dispatch_unique_ptr_w_info) { - int callback_count = 0; - auto unique_ptr_callback = [&callback_count]( - std::unique_ptr, const rclcpp::MessageInfo &) { - callback_count++; - }; +// +// Versions of `const MessageT &` +// +void const_ref_free_func(const test_msgs::msg::Empty &) {} +void const_ref_w_info_free_func(const test_msgs::msg::Empty &, const rclcpp::MessageInfo &) {} + +INSTANTIATE_TEST_SUITE_P( + ConstRefCallbackTests, + DispatchTests, + ::testing::Values( + // lambda + InstanceContext{"lambda", rclcpp::AnySubscriptionCallback().set( + [](const test_msgs::msg::Empty &) {})}, + InstanceContext{"lambda_with_info", + rclcpp::AnySubscriptionCallback().set( + [](const test_msgs::msg::Empty &, const rclcpp::MessageInfo &) {})}, + // free function + InstanceContext{"free_function", rclcpp::AnySubscriptionCallback().set( + const_ref_free_func)}, + InstanceContext{"free_function_with_info", + rclcpp::AnySubscriptionCallback().set( + const_ref_w_info_free_func)}, + // bind function + BindContext("bind_method"), + BindContext( + "bind_method_with_info") + ), + format_parameter +); - any_subscription_callback_.set(unique_ptr_callback); +// +// Versions of `std::unique_ptr` +// +void unique_ptr_free_func(std::unique_ptr) {} +void unique_ptr_w_info_free_func( + std::unique_ptr, const rclcpp::MessageInfo &) +{} + +INSTANTIATE_TEST_SUITE_P( + UniquePtrCallbackTests, + DispatchTests, + ::testing::Values( + // lambda + InstanceContext{"lambda", rclcpp::AnySubscriptionCallback().set( + [](std::unique_ptr) {})}, + InstanceContext{"lambda_with_info", + rclcpp::AnySubscriptionCallback().set( + [](std::unique_ptr, const rclcpp::MessageInfo &) {})}, + // free function + InstanceContext{"free_function", rclcpp::AnySubscriptionCallback().set( + unique_ptr_free_func)}, + InstanceContext{"free_function_with_info", + rclcpp::AnySubscriptionCallback().set( + unique_ptr_w_info_free_func)}, + // bind function + BindContext>("bind_method"), + BindContext, const rclcpp::MessageInfo &>( + "bind_method_with_info") + ), + format_parameter +); - // Message is copied into unique_ptr - EXPECT_NO_THROW(any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_)); - EXPECT_EQ(callback_count, 1); +// +// Versions of `std::shared_ptr` +// +void shared_const_ptr_free_func(std::shared_ptr) {} +void shared_const_ptr_w_info_free_func( + std::shared_ptr, const rclcpp::MessageInfo &) +{} + +INSTANTIATE_TEST_SUITE_P( + SharedConstPtrCallbackTests, + DispatchTests, + ::testing::Values( + // lambda + InstanceContext{"lambda", rclcpp::AnySubscriptionCallback().set( + [](std::shared_ptr) {})}, + InstanceContext{"lambda_with_info", + rclcpp::AnySubscriptionCallback().set( + [](std::shared_ptr, const rclcpp::MessageInfo &) {})}, + // free function + InstanceContext{"free_function", rclcpp::AnySubscriptionCallback().set( + shared_const_ptr_free_func)}, + InstanceContext{"free_function_with_info", + rclcpp::AnySubscriptionCallback().set( + shared_const_ptr_w_info_free_func)}, + // bind function + BindContext>("bind_method"), + BindContext, const rclcpp::MessageInfo &>( + "bind_method_with_info") + ), + format_parameter +); - EXPECT_THROW( - any_subscription_callback_.dispatch_intra_process(msg_const_shared_ptr_, message_info_), - std::runtime_error); - EXPECT_EQ(callback_count, 1); +// +// Versions of `const std::shared_ptr &` +// +void const_ref_shared_const_ptr_free_func(const std::shared_ptr &) {} +void const_ref_shared_const_ptr_w_info_free_func( + const std::shared_ptr &, const rclcpp::MessageInfo &) +{} + +INSTANTIATE_TEST_SUITE_P( + ConstRefSharedConstPtrCallbackTests, + DispatchTests, + ::testing::Values( + // lambda + InstanceContext{"lambda", rclcpp::AnySubscriptionCallback().set( + [](const std::shared_ptr &) {})}, + InstanceContext{"lambda_with_info", + rclcpp::AnySubscriptionCallback().set( + [](const std::shared_ptr &, const rclcpp::MessageInfo &) {})}, + // free function + InstanceContext{"free_function", rclcpp::AnySubscriptionCallback().set( + const_ref_shared_const_ptr_free_func)}, + InstanceContext{"free_function_with_info", + rclcpp::AnySubscriptionCallback().set( + const_ref_shared_const_ptr_w_info_free_func)}, + // bind function + BindContext &>("bind_method"), + BindContext &, const rclcpp::MessageInfo &>( + "bind_method_with_info") + ), + format_parameter +); - // Unique_ptr is_moved - EXPECT_NO_THROW( - any_subscription_callback_.dispatch_intra_process(std::move(msg_unique_ptr_), message_info_)); - EXPECT_EQ(callback_count, 2); -} +// +// Versions of `std::shared_ptr` +// +void shared_ptr_free_func(std::shared_ptr) {} +void shared_ptr_w_info_free_func( + std::shared_ptr, const rclcpp::MessageInfo &) +{} + +INSTANTIATE_TEST_SUITE_P( + SharedPtrCallbackTests, + DispatchTests, + ::testing::Values( + // lambda + InstanceContext{"lambda", rclcpp::AnySubscriptionCallback().set( + [](std::shared_ptr) {})}, + InstanceContext{"lambda_with_info", + rclcpp::AnySubscriptionCallback().set( + [](std::shared_ptr, const rclcpp::MessageInfo &) {})}, + // free function + InstanceContext{"free_function", rclcpp::AnySubscriptionCallback().set( + shared_ptr_free_func)}, + InstanceContext{"free_function_with_info", + rclcpp::AnySubscriptionCallback().set( + shared_ptr_w_info_free_func)}, + // bind function + BindContext>("bind_method"), + BindContext, const rclcpp::MessageInfo &>( + "bind_method_with_info") + ), + format_parameter +); diff --git a/rclcpp/test/rclcpp/test_create_subscription.cpp b/rclcpp/test/rclcpp/test_create_subscription.cpp index 401e6c33ed..fd947485e2 100644 --- a/rclcpp/test/rclcpp/test_create_subscription.cpp +++ b/rclcpp/test/rclcpp/test_create_subscription.cpp @@ -42,7 +42,7 @@ TEST_F(TestCreateSubscription, create) { auto node = std::make_shared("my_node", "/ns"); const rclcpp::QoS qos(10); auto options = rclcpp::SubscriptionOptions(); - auto callback = [](const test_msgs::msg::Empty::SharedPtr) {}; + auto callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; auto subscription = rclcpp::create_subscription(node, "topic_name", qos, callback, options); @@ -55,7 +55,7 @@ TEST_F(TestCreateSubscription, create_with_overriding_options) { const rclcpp::QoS qos(10); auto options = rclcpp::SubscriptionOptions(); options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); - auto callback = [](const test_msgs::msg::Empty::SharedPtr) {}; + auto callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; auto subscription = rclcpp::create_subscription(node, "topic_name", qos, callback, options); @@ -67,7 +67,7 @@ TEST_F(TestCreateSubscription, create_separated_node_topics_and_parameters) { auto node = std::make_shared("my_node", "/ns"); const rclcpp::QoS qos(10); auto options = rclcpp::SubscriptionOptions(); - auto callback = [](const test_msgs::msg::Empty::SharedPtr) {}; + auto callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; auto node_parameters = node->get_node_parameters_interface(); auto node_topics = node->get_node_topics_interface(); @@ -86,7 +86,7 @@ TEST_F(TestCreateSubscription, create_with_statistics) { options.topic_stats_options.publish_topic = "topic_statistics"; options.topic_stats_options.publish_period = 5min; - auto callback = [](const test_msgs::msg::Empty::SharedPtr) {}; + auto callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; auto subscription = rclcpp::create_subscription(node, "topic_name", qos, callback, options); diff --git a/rclcpp/test/rclcpp/test_memory_strategy.cpp b/rclcpp/test/rclcpp/test_memory_strategy.cpp index 93fa0c8d5e..574bc1aee5 100644 --- a/rclcpp/test/rclcpp/test_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/test_memory_strategy.cpp @@ -98,7 +98,7 @@ TEST_F(TestMemoryStrategy, get_subscription_by_handle) { { auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - auto subscription_callback = [](const test_msgs::msg::Empty::SharedPtr) {}; + auto subscription_callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; const rclcpp::QoS qos(10); { @@ -344,7 +344,7 @@ TEST_F(TestMemoryStrategy, get_group_by_subscription) { callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - auto subscription_callback = [](const test_msgs::msg::Empty::SharedPtr) {}; + auto subscription_callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; const rclcpp::QoS qos(10); rclcpp::SubscriptionOptions subscription_options; diff --git a/rclcpp/test/rclcpp/test_node.cpp b/rclcpp/test/rclcpp/test_node.cpp index 6297c4ebbe..e1fb4b8ced 100644 --- a/rclcpp/test/rclcpp/test_node.cpp +++ b/rclcpp/test/rclcpp/test_node.cpp @@ -2591,7 +2591,7 @@ TEST_F(TestNode, get_publishers_subscriptions_info_by_topic) { false }; rclcpp::QoS qos2 = rclcpp::QoS(qos_initialization2, rmw_qos_profile_default2); - auto callback = [](const test_msgs::msg::BasicTypes::SharedPtr msg) { + auto callback = [](test_msgs::msg::BasicTypes::ConstSharedPtr msg) { (void)msg; }; auto subscriber = diff --git a/rclcpp/test/rclcpp/test_parameter_client.cpp b/rclcpp/test/rclcpp/test_parameter_client.cpp index c7f8ad6581..04240b7af9 100644 --- a/rclcpp/test/rclcpp/test_parameter_client.cpp +++ b/rclcpp/test/rclcpp/test_parameter_client.cpp @@ -33,7 +33,7 @@ using namespace std::chrono_literals; class TestParameterClient : public ::testing::Test { public: - void OnMessage(const rcl_interfaces::msg::ParameterEvent::SharedPtr event) + void OnMessage(rcl_interfaces::msg::ParameterEvent::ConstSharedPtr event) { (void)event; } diff --git a/rclcpp/test/rclcpp/test_parameter_event_handler.cpp b/rclcpp/test/rclcpp/test_parameter_event_handler.cpp index 868419249c..097b0ee391 100644 --- a/rclcpp/test/rclcpp/test_parameter_event_handler.cpp +++ b/rclcpp/test/rclcpp/test_parameter_event_handler.cpp @@ -27,9 +27,9 @@ class TestParameterEventHandler : public rclcpp::ParameterEventHandler : ParameterEventHandler(node) {} - void test_event(const rcl_interfaces::msg::ParameterEvent::SharedPtr event) + void test_event(rcl_interfaces::msg::ParameterEvent::ConstSharedPtr event) { - event_callback(event); + event_callback(*event); } size_t num_event_callbacks() @@ -264,20 +264,20 @@ TEST_F(TestNode, EventCallback) double product; auto cb = [&int_param, &double_param, &product, &received, - this](const rcl_interfaces::msg::ParameterEvent::SharedPtr & event) + this](const rcl_interfaces::msg::ParameterEvent & event) { auto node_name = node->get_fully_qualified_name(); - if (event->node == node_name) { + if (event.node == node_name) { received = true; } rclcpp::Parameter p; - if (ParameterEventHandler::get_parameter_from_event(*event, p, "my_int", node_name)) { + if (ParameterEventHandler::get_parameter_from_event(event, p, "my_int", node_name)) { int_param = p.get_value(); } try { - p = ParameterEventHandler::get_parameter_from_event(*event, "my_double", node_name); + p = ParameterEventHandler::get_parameter_from_event(event, "my_double", node_name); double_param = p.get_value(); } catch (...) { } @@ -286,12 +286,12 @@ TEST_F(TestNode, EventCallback) }; auto cb2 = - [&bool_param, this](const rcl_interfaces::msg::ParameterEvent::SharedPtr & event) + [&bool_param, this](const rcl_interfaces::msg::ParameterEvent & event) { rclcpp::Parameter p; - if (event->node == diff_ns_name) { + if (event.node == diff_ns_name) { if (ParameterEventHandler::get_parameter_from_event( - *event, p, "my_bool", diff_ns_name)) + event, p, "my_bool", diff_ns_name)) { bool_param = p.get_value(); } @@ -405,13 +405,13 @@ TEST_F(TestNode, LastInFirstCallForParameterEventCallbacks) // The callbacks will log the current time for comparison purposes. Add a bit of a stall // to ensure that the time noted in the back-to-back calls isn't the same auto cb1 = - [this, &time_1](const rcl_interfaces::msg::ParameterEvent::SharedPtr &) + [this, &time_1](const rcl_interfaces::msg::ParameterEvent &) { time_1 = node->now(); std::this_thread::sleep_for(std::chrono::milliseconds(10)); }; auto cb2 = - [this, &time_2](const rcl_interfaces::msg::ParameterEvent::SharedPtr &) + [this, &time_2](const rcl_interfaces::msg::ParameterEvent &) { time_2 = node->now(); std::this_thread::sleep_for(std::chrono::milliseconds(10)); diff --git a/rclcpp/test/rclcpp/test_publisher_subscription_count_api.cpp b/rclcpp/test/rclcpp/test_publisher_subscription_count_api.cpp index 9e009acb95..eac46c1d8d 100644 --- a/rclcpp/test/rclcpp/test_publisher_subscription_count_api.cpp +++ b/rclcpp/test/rclcpp/test_publisher_subscription_count_api.cpp @@ -122,7 +122,7 @@ class TestPublisherSubscriptionCount : public ::testing::Test } protected: - static void OnMessage(const test_msgs::msg::Empty::SharedPtr msg) + static void OnMessage(test_msgs::msg::Empty::ConstSharedPtr msg) { (void)msg; } diff --git a/rclcpp/test/rclcpp/test_qos_event.cpp b/rclcpp/test/rclcpp/test_qos_event.cpp index 6c94aaff97..960ffdcb4c 100644 --- a/rclcpp/test/rclcpp/test_qos_event.cpp +++ b/rclcpp/test/rclcpp/test_qos_event.cpp @@ -42,7 +42,7 @@ class TestQosEvent : public ::testing::Test node = std::make_shared("test_qos_event", "/ns"); - message_callback = [node = node.get()](const test_msgs::msg::Empty::SharedPtr /*msg*/) { + message_callback = [node = node.get()](test_msgs::msg::Empty::ConstSharedPtr /*msg*/) { RCLCPP_INFO(node->get_logger(), "Message received"); }; } @@ -55,7 +55,7 @@ class TestQosEvent : public ::testing::Test static constexpr char topic_name[] = "test_topic"; rclcpp::Node::SharedPtr node; bool is_fastrtps; - std::function message_callback; + std::function message_callback; }; constexpr char TestQosEvent::topic_name[]; diff --git a/rclcpp/test/rclcpp/test_serialized_message_allocator.cpp b/rclcpp/test/rclcpp/test_serialized_message_allocator.cpp index 968ad0ac2d..78ab77a28f 100644 --- a/rclcpp/test/rclcpp/test_serialized_message_allocator.cpp +++ b/rclcpp/test/rclcpp/test_serialized_message_allocator.cpp @@ -58,7 +58,7 @@ TEST(TestSerializedMessageAllocator, borrow_from_subscription) { std::shared_ptr sub = node->create_subscription( "~/dummy_topic", 10, - [](std::shared_ptr test_msg) {(void) test_msg;}); + [](std::shared_ptr test_msg) {(void) test_msg;}); auto msg0 = sub->create_serialized_message(); EXPECT_EQ(0u, msg0->capacity()); diff --git a/rclcpp/test/rclcpp/test_subscription.cpp b/rclcpp/test/rclcpp/test_subscription.cpp index 9b190fea18..0e798cabef 100644 --- a/rclcpp/test/rclcpp/test_subscription.cpp +++ b/rclcpp/test/rclcpp/test_subscription.cpp @@ -34,7 +34,7 @@ using namespace std::chrono_literals; class TestSubscription : public ::testing::Test { public: - void OnMessage(const test_msgs::msg::Empty::SharedPtr msg) + void OnMessage(test_msgs::msg::Empty::ConstSharedPtr msg) { (void)msg; } @@ -80,7 +80,7 @@ class TestSubscriptionInvalidIntraprocessQos class TestSubscriptionSub : public ::testing::Test { public: - void OnMessage(const test_msgs::msg::Empty::SharedPtr msg) + void OnMessage(test_msgs::msg::Empty::ConstSharedPtr msg) { (void)msg; } @@ -113,7 +113,7 @@ class SubscriptionClassNodeInheritance : public rclcpp::Node { } - void OnMessage(const test_msgs::msg::Empty::SharedPtr msg) + void OnMessage(test_msgs::msg::Empty::ConstSharedPtr msg) { (void)msg; } @@ -130,7 +130,7 @@ class SubscriptionClassNodeInheritance : public rclcpp::Node class SubscriptionClass { public: - void OnMessage(const test_msgs::msg::Empty::SharedPtr msg) + void OnMessage(test_msgs::msg::Empty::ConstSharedPtr msg) { (void)msg; } @@ -150,7 +150,7 @@ class SubscriptionClass TEST_F(TestSubscription, construction_and_destruction) { initialize(); using test_msgs::msg::Empty; - auto callback = [](const Empty::SharedPtr msg) { + auto callback = [](Empty::ConstSharedPtr msg) { (void)msg; }; { @@ -162,7 +162,7 @@ TEST_F(TestSubscription, construction_and_destruction) { // get_subscription_handle() const rclcpp::SubscriptionBase * const_sub = sub.get(); EXPECT_NE(nullptr, const_sub->get_subscription_handle()); - EXPECT_FALSE(sub->use_take_shared_method()); + EXPECT_TRUE(sub->use_take_shared_method()); EXPECT_NE(nullptr, sub->get_message_type_support_handle().typesupport_identifier); EXPECT_NE(nullptr, sub->get_message_type_support_handle().data); @@ -182,7 +182,7 @@ TEST_F(TestSubscription, construction_and_destruction) { */ TEST_F(TestSubscriptionSub, construction_and_destruction) { using test_msgs::msg::Empty; - auto callback = [](const Empty::SharedPtr msg) { + auto callback = [](Empty::ConstSharedPtr msg) { (void)msg; }; { @@ -216,7 +216,7 @@ TEST_F(TestSubscriptionSub, construction_and_destruction) { TEST_F(TestSubscription, various_creation_signatures) { initialize(); using test_msgs::msg::Empty; - auto cb = [](test_msgs::msg::Empty::SharedPtr) {}; + auto cb = [](test_msgs::msg::Empty::ConstSharedPtr) {}; { auto sub = node->create_subscription("topic", 1, cb); (void)sub; diff --git a/rclcpp/test/rclcpp/test_subscription_publisher_count_api.cpp b/rclcpp/test/rclcpp/test_subscription_publisher_count_api.cpp index 1e71e28c5e..92297e0e04 100644 --- a/rclcpp/test/rclcpp/test_subscription_publisher_count_api.cpp +++ b/rclcpp/test/rclcpp/test_subscription_publisher_count_api.cpp @@ -108,7 +108,7 @@ class TestSubscriptionPublisherCount : public ::testing::Test } protected: - static void OnMessage(const test_msgs::msg::Empty::SharedPtr msg) + static void OnMessage(test_msgs::msg::Empty::ConstSharedPtr msg) { (void)msg; } diff --git a/rclcpp/test/rclcpp/test_wait_set.cpp b/rclcpp/test/rclcpp/test_wait_set.cpp index bbfc9383c8..f9d97a5759 100644 --- a/rclcpp/test/rclcpp/test_wait_set.cpp +++ b/rclcpp/test/rclcpp/test_wait_set.cpp @@ -221,7 +221,7 @@ TEST_F(TestWaitSet, add_guard_condition_to_two_different_wait_set) { wait_set2.add_guard_condition(guard_condition); }, std::runtime_error); - auto do_nothing = [](const std::shared_ptr) {}; + auto do_nothing = [](std::shared_ptr) {}; auto sub = node->create_subscription("~/test", 1, do_nothing); wait_set1.add_subscription(sub); ASSERT_THROW( @@ -281,7 +281,7 @@ TEST_F(TestWaitSet, add_remove_wait) { rclcpp::SubscriptionOptions subscription_options; subscription_options.event_callbacks.deadline_callback = [](auto) {}; subscription_options.event_callbacks.liveliness_callback = [](auto) {}; - auto do_nothing = [](const std::shared_ptr) {}; + auto do_nothing = [](std::shared_ptr) {}; auto sub = node->create_subscription( "~/test", 1, do_nothing, subscription_options); diff --git a/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp b/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp index 2282a92bb1..7a15e2cf49 100644 --- a/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp +++ b/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp @@ -74,7 +74,7 @@ TEST_F(TestDynamicStorage, default_construct_destruct) { TEST_F(TestDynamicStorage, iterables_construct_destruct) { auto subscription = node->create_subscription( - "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {}); + "topic", 10, [](test_msgs::msg::Empty::ConstSharedPtr) {}); auto timer = node->create_wall_timer(std::chrono::seconds(100), []() {}); auto guard_condition = std::make_shared(); auto service = @@ -110,7 +110,7 @@ TEST_F(TestDynamicStorage, add_remove_dynamically) { options.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; auto subscription = node->create_subscription( - "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {}, options); + "topic", 10, [](test_msgs::msg::Empty::ConstSharedPtr) {}, options); rclcpp::SubscriptionWaitSetMask mask{true, true, true}; wait_set.add_subscription(subscription, mask); @@ -203,7 +203,7 @@ TEST_F(TestDynamicStorage, add_remove_out_of_scope) { { auto subscription = node->create_subscription( - "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {}); + "topic", 10, [](test_msgs::msg::Empty::ConstSharedPtr) {}); wait_set.add_subscription(subscription); // This is short, so if it's not cleaned up, it will trigger wait and it won't timeout @@ -238,7 +238,7 @@ TEST_F(TestDynamicStorage, wait_subscription) { auto publisher = node->create_publisher("topic", 10); auto subscription = node->create_subscription( - "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {}); + "topic", 10, [](test_msgs::msg::Empty::ConstSharedPtr) {}); wait_set.add_subscription(subscription); { diff --git a/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp b/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp index 8e406276f1..1e4d5f805e 100644 --- a/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp +++ b/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp @@ -67,7 +67,7 @@ class TestWaitable : public rclcpp::Waitable TEST_F(TestStaticStorage, iterables_construct_destruct) { auto subscription = node->create_subscription( - "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {}); + "topic", 10, [](test_msgs::msg::Empty::ConstSharedPtr) {}); // This is long, so it can stick around and be removed auto timer = node->create_wall_timer(std::chrono::seconds(100), []() {}); auto guard_condition = std::make_shared(); @@ -132,7 +132,7 @@ TEST_F(TestStaticStorage, fixed_storage_needs_pruning) { TEST_F(TestStaticStorage, wait_subscription) { auto publisher = node->create_publisher("topic", 10); auto subscription = node->create_subscription( - "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {}); + "topic", 10, [](test_msgs::msg::Empty::ConstSharedPtr) {}); rclcpp::SubscriptionWaitSetMask mask{true, true, true}; rclcpp::StaticWaitSet<1, 0, 0, 0, 0, 0> wait_set({{{subscription, mask}}}); diff --git a/rclcpp/test/rclcpp/wait_set_policies/test_storage_policy_common.cpp b/rclcpp/test/rclcpp/wait_set_policies/test_storage_policy_common.cpp index 130d281e13..ba0f4d16e8 100644 --- a/rclcpp/test/rclcpp/wait_set_policies/test_storage_policy_common.cpp +++ b/rclcpp/test/rclcpp/wait_set_policies/test_storage_policy_common.cpp @@ -79,7 +79,7 @@ TEST_F(TestStoragePolicyCommon, rcl_wait_set_resize_error) { rclcpp::WaitSet wait_set; auto subscription = node->create_subscription( - "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {}); + "topic", 10, [](test_msgs::msg::Empty::ConstSharedPtr) {}); rclcpp::SubscriptionWaitSetMask mask{true, true, true}; auto mock = mocking_utils::patch_and_return( @@ -103,7 +103,7 @@ TEST_F(TestStoragePolicyCommon, rcl_wait_set_clear_error) { TEST_F(TestStoragePolicyCommon, rcl_wait_set_add_subscription_error) { rclcpp::WaitSet wait_set; auto subscription = node->create_subscription( - "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {}); + "topic", 10, [](test_msgs::msg::Empty::ConstSharedPtr) {}); rclcpp::SubscriptionWaitSetMask mask{true, true, true}; auto mock = mocking_utils::patch_and_return( diff --git a/rclcpp/test/rclcpp/wait_set_policies/test_thread_safe_synchronization.cpp b/rclcpp/test/rclcpp/wait_set_policies/test_thread_safe_synchronization.cpp index 8f4870f156..99d07f1a19 100644 --- a/rclcpp/test/rclcpp/wait_set_policies/test_thread_safe_synchronization.cpp +++ b/rclcpp/test/rclcpp/wait_set_policies/test_thread_safe_synchronization.cpp @@ -75,7 +75,7 @@ TEST_F(TestThreadSafeStorage, default_construct_destruct) { TEST_F(TestThreadSafeStorage, iterables_construct_destruct) { auto subscription = node->create_subscription( - "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {}); + "topic", 10, [](test_msgs::msg::Empty::ConstSharedPtr) {}); // This is long, so it can stick around auto timer = node->create_wall_timer(std::chrono::seconds(100), []() {}); auto guard_condition = std::make_shared(); @@ -113,7 +113,7 @@ TEST_F(TestThreadSafeStorage, add_remove_dynamically) { options.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; auto subscription = node->create_subscription( - "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {}, options); + "topic", 10, [](test_msgs::msg::Empty::ConstSharedPtr) {}, options); rclcpp::SubscriptionWaitSetMask mask{true, true, true}; wait_set.add_subscription(subscription, mask); @@ -207,7 +207,7 @@ TEST_F(TestThreadSafeStorage, add_remove_out_of_scope) { { auto subscription = node->create_subscription( - "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {}); + "topic", 10, [](test_msgs::msg::Empty::ConstSharedPtr) {}); wait_set.add_subscription(subscription); // This is short, so if it's not cleaned up, it will trigger wait @@ -242,7 +242,7 @@ TEST_F(TestThreadSafeStorage, wait_subscription) { auto publisher = node->create_publisher("topic", 10); auto subscription = node->create_subscription( - "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {}); + "topic", 10, [](test_msgs::msg::Empty::ConstSharedPtr) {}); wait_set.add_subscription(subscription); {